Merge pull request #100 from UOA-FSAE/improve_centerline

Improve centerline
UOA-FSAE · Jun 5, 2024 · ccd4fe5 · ccd4fe5
2 parents f0013e6 + 3ab3b4e
commit ccd4fe5
Show file tree

Hide file tree

Showing 8 changed files with 397 additions and 595 deletions.
diff --git a/src/moa/moa_bringup/launch/sim_launch.py b/src/moa/moa_bringup/launch/sim_launch.py
@@ -8,7 +8,7 @@ def generate_launch_description():
             package='aruco_detection',
             executable='aruco_detection',
             name='aruco_detection'
-        ), 
+        ),
 
         # cone mapping
         launch_ros.actions.Node(

diff --git a/src/planning/path_planning/path_planning/bound_coods b/src/planning/path_planning/path_planning/bound_coods
@@ -0,0 +1,4 @@
+-13.984399896783033 -13.753242748183307 -13.522085599583582 -13.290928450983856 -13.05977130238413 -12.828614153784404 -12.597457005184678 -12.366299856584952 -12.135142707985226 -11.9039855593855 -11.672828410785774 -11.441671262186048 -11.210514113586322 -10.979356964986597 -10.74819981638687 -10.517042667787145 -10.285885519187419 -10.054728370587693 -9.823571221987965 -9.592414073388241 -9.361256924788513 -9.13009977618879 -8.898942627589062 -8.667785478989337 -8.43662833038961 -8.205471181789886 -7.974314033190159 -7.743156884590433 -7.511999735990707 -7.280842587390981 -7.049685438791255 -6.818528290191529 -6.587371141591803 -6.356213992992077 -6.125056844392351 -5.893899695792625 -5.662742547192899 -5.431585398593173 -5.200428249993447 -4.969271101393721 -4.738113952793995 -4.506956804194269 -4.275799655594543 -4.044642506994817 -3.8134853583950914 -3.5823282097953655 -3.3511710611956396 -3.1200139125959137 -2.8888567639961877 -2.6576996153964614 
+18.00000101729649 17.87396674394038 17.747932470584274 17.62189819722816 17.49586392387205 17.36982965051594 17.24379537715983 17.11776110380372 16.99172683044761 16.8656925570915 16.723749949164805 16.564488014449786 16.405226079734767 16.24596414501975 16.086702210304736 15.927440275589715 15.768178340874702 15.60891640615968 15.449654471444662 15.243598109882772 15.016342330608962 14.789086551335156 14.561830772061345 14.334574992787541 14.10731921351373 13.880063434239922 13.652807654966114 13.389405065848925 13.115060535444195 12.840716005039466 12.566371474634739 12.292026944230006 12.017682413825277 11.743337883420548 11.40061738510908 11.045069945062286 10.689522505015493 10.333975064968698 9.978427624921904 9.620174315110118 9.129500537664088 8.638826760218056 8.148152982772025 7.657479205325993 7.0797583828357205 6.368003065412987 5.6562477479902515 4.926519334899599 3.7132599223374685 2.5000005097753353 
+-9.104301541482327 -8.871501531845682 -8.638701522209038 -8.405901512572395 -8.17310150293575 -7.940301493299106 -7.707501483662462 -7.474701474025817 -7.241901464389173 -7.009101454752529 -6.776301445115885 -6.543501435479241 -6.310701425842597 -6.077901416205952 -5.845101406569308 -5.612301396932663 -5.37950138729602 -5.146701377659376 -4.9139013680227315 -4.681101358386087 -4.4483013487494425 -4.215501339112799 -3.9827013294761544 -3.74990131983951 -3.5171013102028663 -3.284301300566222 -3.0515012909295773 -2.818701281292933 -2.5859012716562892 -2.3531012620196448 -2.1203012523830003 -1.8875012427463567 -1.6547012331097122 -1.4219012234730677 -1.189101213836424 -0.9563012041997787 -0.723501194563136 -0.4907011849264915 -0.257901175289847 -0.02510116565320253 0.20769884398344196 0.44049885362008645 0.6732988632567292 0.9060988728933737 1.1388988825300181 1.3716988921666626 1.6044989018033071 1.8372989114399516 2.0700989210765943 2.302898930713238 
+21.09999984182598 20.925851561371385 20.75170328091679 20.57755500046219 20.403406720007595 20.229258439552996 20.055110159098398 19.880961878643802 19.706813598189207 19.532665317734608 19.35851703728001 19.18082601200009 18.9672104316014 18.753594851202706 18.539979270804018 18.32636369040533 18.11274811000664 17.899132529607947 17.68551694920926 17.471901368810567 17.258285788411875 17.024671570043896 16.73384583089838 16.44302009175287 16.15219435260736 15.861368613461845 15.570542874316335 15.27971713517082 14.98889139602531 14.698065656879798 14.361528507753558 14.001735788644298 13.641943069535035 13.282150350425773 12.922357631316512 12.562564912207248 12.202772193097989 11.78718052761864 11.299531749635975 10.811882971653308 10.324234193670641 9.836585415687976 9.348936637705314 8.701670759843026 7.984220134415498 7.266769508987968 6.549318883560439 5.5676250313189986 4.348812112735477 3.12999919415195 
diff --git a/src/planning/path_planning/path_planning/bound_coods2 b/src/planning/path_planning/path_planning/bound_coods2
@@ -0,0 +1,4 @@
+-13.984399896783033 -13.753242748183307 -13.522085599583582 -13.290928450983856 -13.05977130238413 -12.828614153784404 -12.597457005184678 -12.366299856584952 -12.135142707985226 -11.9039855593855 -11.672828410785774 -11.441671262186048 -11.210514113586322 -10.979356964986597 -10.74819981638687 -10.517042667787145 -10.285885519187419 -10.054728370587693 -9.823571221987965 -9.592414073388241 -9.361256924788513 -9.13009977618879 -8.898942627589062 -8.667785478989337 -8.43662833038961 -8.205471181789886 -7.974314033190159 -7.743156884590433 -7.511999735990707 -7.280842587390981 -7.049685438791255 -6.818528290191529 -6.587371141591803 -6.356213992992077 -6.125056844392351 -5.893899695792625 -5.662742547192899 -5.431585398593173 -5.200428249993447 -4.969271101393721 -4.738113952793995 -4.506956804194269 -4.275799655594543 -4.044642506994817 -3.8134853583950914 -3.5823282097953655 -3.3511710611956396 -3.1200139125959137 -2.8888567639961877 -2.6576996153964614 
+18.00000101729649 17.87396674394038 17.747932470584274 17.62189819722816 17.49586392387205 17.36982965051594 17.24379537715983 17.11776110380372 16.99172683044761 16.8656925570915 16.723749949164805 16.564488014449786 16.405226079734767 16.24596414501975 16.086702210304736 15.927440275589715 15.768178340874702 15.60891640615968 15.449654471444662 15.243598109882772 15.016342330608962 14.789086551335156 14.561830772061345 14.334574992787541 14.10731921351373 13.880063434239922 13.652807654966114 13.389405065848925 13.115060535444195 12.840716005039466 12.566371474634739 12.292026944230006 12.017682413825277 11.743337883420548 11.40061738510908 11.045069945062286 10.689522505015493 10.333975064968698 9.978427624921904 9.620174315110118 9.129500537664088 8.638826760218056 8.148152982772025 7.657479205325993 7.0797583828357205 6.368003065412987 5.6562477479902515 4.926519334899599 3.7132599223374685 2.5000005097753353 
+-9.104301541482327 -8.871501531845682 -8.638701522209038 -8.405901512572395 -8.17310150293575 -7.940301493299106 -7.707501483662462 -7.474701474025817 -7.241901464389173 -7.009101454752529 -6.776301445115885 -6.543501435479241 -6.310701425842597 -6.077901416205952 -5.845101406569308 -5.612301396932663 -5.37950138729602 -5.146701377659376 -4.9139013680227315 -4.681101358386087 -4.4483013487494425 -4.215501339112799 -3.9827013294761544 -3.74990131983951 -3.5171013102028663 -3.284301300566222 -3.0515012909295773 -2.818701281292933 -2.5859012716562892 -2.3531012620196448 -2.1203012523830003 -1.8875012427463567 -1.6547012331097122 -1.4219012234730677 -1.189101213836424 -0.9563012041997787 -0.723501194563136 -0.4907011849264915 -0.257901175289847 -0.02510116565320253 0.20769884398344196 0.44049885362008645 0.6732988632567292 0.9060988728933737 1.1388988825300181 1.3716988921666626 1.6044989018033071 1.8372989114399516 2.0700989210765943 2.302898930713238 
+21.09999984182598 20.925851561371385 20.75170328091679 20.57755500046219 20.403406720007595 20.229258439552996 20.055110159098398 19.880961878643802 19.706813598189207 19.532665317734608 19.35851703728001 19.18082601200009 18.9672104316014 18.753594851202706 18.539979270804018 18.32636369040533 18.11274811000664 17.899132529607947 17.68551694920926 17.471901368810567 17.258285788411875 17.024671570043896 16.73384583089838 16.44302009175287 16.15219435260736 15.861368613461845 15.570542874316335 15.27971713517082 14.98889139602531 14.698065656879798 14.361528507753558 14.001735788644298 13.641943069535035 13.282150350425773 12.922357631316512 12.562564912207248 12.202772193097989 11.78718052761864 11.299531749635975 10.811882971653308 10.324234193670641 9.836585415687976 9.348936637705314 8.701670759843026 7.984220134415498 7.266769508987968 6.549318883560439 5.5676250313189986 4.348812112735477 3.12999919415195 
diff --git a/src/planning/path_planning/path_planning/boundaries_drawn.png b/src/planning/path_planning/path_planning/boundaries_drawn.png
diff --git a/src/planning/path_planning/path_planning/trajectory_generation.py b/src/planning/path_planning/path_planning/trajectory_generation.py
@@ -22,7 +22,7 @@ def __init__(self):
             namespace='',
             parameters=[
                 ('debug', True),
-                ('timer', 7.0),
+                ('timer', 0.1),
             ]
         )
 
@@ -122,7 +122,7 @@ def my_trajectory_generator(self, cone_map, radius, npoints):
             # inverse tan is in radians
             angle = np.arctan(dx/dy)
             angs[i] = (angle if dx < 0 else angle)
-            print(f"passed angle = {angs[i]} for dx = {dx}")
+            # print(f"passed angle = {angs[i]} for dx = {dx}")
 
             # transform coordinates from fixed to car 
             post_trans_pose = self.apply_transformation(car_position, rotation_matrix, val, y[i])
@@ -148,6 +148,8 @@ def my_trajectory_generator(self, cone_map, radius, npoints):
         # list of points as tuples
         points = [(x[i],y[i]) for i in range(n)]
 
+        # self.get_logger().info(f"len of traj = {len(trajectories)}")
+
         return trajectories, angs
 
     def get_position_of_cart(self, cone_map):