Test of New Path planning

run_path_planning.sh

@@ -1,3 +1,6 @@
 #!/bin/bash
 source install/setup.bash
-colcon build --packages-select path_planning && ros2 run path_planning center_line
+#colcon build --packages-select path_planning 
+# ros2 run path_planning center_line
+ros2 run path_planning trajectory_generation
+

src/action/stanley_controller/stanley_controller/stanley_controller.py

@@ -77,7 +77,7 @@ def __init__(self):
         self.k_speed = 1.0 #speed Controller gain
         self.cam_fron_axle_dist= 1 #[m] Wheel base of vehicle
         self.max_steer = 27.0  # [degrees] max steering angle
-        self.target_speed = 20/3.6 #[m/s]
+        self.target_speed = 3.6/3.6 #[m/s]
 
         #Subscribe for car pose and track
         self.create_subscription(PoseArray, "moa/selected_trajectory", self.selected_trajectory_handler, 5)
@@ -107,8 +107,10 @@ def main_hearback(self, msg):
             delta = math.degrees(theta_e + theta_d)
             delta = np.clip(delta, -self.max_steer, self.max_steer)
             self.steering_angle = delta
+            self.target_speed = 3.6/3.6
         else:
             self.steering_angle = 0
+            self.target_speed = 0
             self.get_logger().info("Warning: no trajectory found, will set steering angle to 0!!!!")
 
         # Publish command for velocity

src/planning/path_planning/setup.py

@@ -25,6 +25,7 @@
     entry_points={
         'console_scripts': [
             'trajectory_generator = path_planning.trajectory_generator_HRHCS:main',
+            'trajectory_generation = path_planning.trajectory_generation:main',
             'trajectory_optimisation = path_planning.trajectory_optimisation_CS:main',
             'center_line = path_planning.simple_centerline_planner:main',
             'shortest_path = path_planning.trajectory_shortest_path:main',

unity_qucik_start.sh

@@ -9,5 +9,6 @@ gnome-terminal -- bash -c "source run_foxglove_ros.sh; exec bash"
 gnome-terminal -- bash -c "source run_localization.sh; exec bash"
 gnome-terminal -- bash -c "source run_path_planning_visualizer.sh; exec bash"
 gnome-terminal -- bash -c "source run_path_planning.sh; exec bash"
+gnome-terminal -- bash -c "source run_path_planning_second.sh; exec bash"
 gnome-terminal -- bash -c "source run_simulation_car_control.sh; exec bash"