fixing less cone problem

UOA-FSAE · Apr 24, 2024 · ea3bff4 · ea3bff4
1 parent 480406f
commit ea3bff4
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 5 deletions.
diff --git a/src/planning/path_planning/path_planning/trajectory_generation.py b/src/planning/path_planning/path_planning/trajectory_generation.py
@@ -159,10 +159,10 @@ def get_position_of_cart(self, cone_map):
         return x, y, theta
 
     def get_transformation_matrix(self, position_and_orientation):
-        # theta = position_and_orientation[2] - np.pi/2
+        theta = position_and_orientation[2] - np.pi/2
         cart_x = position_and_orientation[0]
         cart_y = position_and_orientation[1]
-        theta = position_and_orientation[2]
+        # theta = position_and_orientation[2]
         # 2d trasformation matrix 
         rotation_matrix = np.array([[np.cos(theta), -np.sin(theta)],[np.sin(theta), np.cos(theta)]])
         position_vector = np.array([[cart_x], [cart_y]])

diff --git a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py b/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py
@@ -76,8 +76,8 @@ def set_boundaries(self, msg: ConeMap):
                         rightboundary.append([x,y])
 
             # interpolate
-            funcL = interpolate.interp1d([P[0] for P in leftboundary], [P[1] for P in leftboundary], kind='slinear')
-            funcR = interpolate.interp1d([P[0] for P in rightboundary], [P[1] for P in rightboundary], kind='slinear')
+            funcL = interpolate.interp1d([P[0] for P in leftboundary], [P[1] for P in leftboundary], kind='cubic')
+            funcR = interpolate.interp1d([P[0] for P in rightboundary], [P[1] for P in rightboundary], kind='cubic')
 
             xlrange = np.linspace(min([P[0] for P in leftboundary]), max([P[0] for P in leftboundary]))
             xrrange = np.linspace(min([P[0] for P in rightboundary]), max([P[0] for P in rightboundary]))

diff --git a/src/planning/path_planning/setup.py b/src/planning/path_planning/setup.py
@@ -28,7 +28,8 @@
             'trajectory_optimisation = path_planning.trajectory_optimisation_CS:main',
             'center_line = path_planning.simple_centerline_planner:main',
             'shortest_path = path_planning.trajectory_shortest_path:main',
-            'shortest_path_viz = path_planning.shortest_path_viz:main'
+            'shortest_path_viz = path_planning.shortest_path_viz:main',
+            'trajectory_generation = path_planning.trajectory_generation:main'
         ],
     },
 )