Merge pull request #99 from UOA-FSAE/stanley_controller

Stanley controller to nightly

run_controller.sh

@@ -1,5 +1,5 @@
 #!/bin/bash
 source install/setup.bash
-colcon build --packages-select head_to_goal_control && ros2 run head_to_goal_control controller
-#colcon build --packages-select stanley_controller && ros2 run stanley_controller controller
+#colcon build --packages-select head_to_goal_control && ros2 run head_to_goal_control controller
+colcon build --packages-select stanley_controller && ros2 run stanley_controller controller
 

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

@@ -76,37 +76,41 @@ def __init__(self):
         self.k_stanley = 5.0 #stanley Controller gain
         self.k_speed = 1.0 #speed Controller gain
         self.cam_fron_axle_dist= 1 #[m] Wheel base of vehicle
-        self.max_steer = np.radians(27.0)  # [rad] max steering angle
-        self.target_speed = 150/3.6 #[m/s]
+        self.max_steer = 27.0  # [degrees] max steering angle
+        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)
-        self.create_subscription(ConeMap, "cone_map", self.main_hearback, 5)
+        self.create_subscription(Pose, "car_position", self.main_hearback, 5)
         #Publish result
         self.cmd_vel_pub = self.create_publisher(AckermannDrive, "/drive", 5)
         self.cmd_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5)
         self.create_publisher(Pose, "moa/track_point", 5)
 
     def main_hearback(self, msg):
-        car_pose = msg.cones[0].pose.pose
+        car_pose = msg
         camera_position = [car_pose.position.x,car_pose.position.y]
         car_yaw = car_pose.orientation.w
+        self.car_yaw_corrected = self.normalize_angle(car_yaw-4.71)
+
 
         if hasattr(self, "trajectory_in_global_frame"):
             #Get Car front axle center position
-            axle_pos = self.get_front_axle_position(camera_position,car_yaw)
+            axle_pos = self.get_front_axle_position(camera_position,self.car_yaw_corrected)
             #Get closest point on track and distance error
-            cls_point,error_front_axle = self.get_closest_track_point(axle_pos,car_yaw)
-            #Compute target yaw
+            cls_point,error_front_axle = self.get_closest_track_point(axle_pos,self.car_yaw_corrected)
+            #Compute target yaw - Angle from positive x in radians
             target_yaw = self.cal_target_yaw(cls_point)
             #Compute steering angle
-            theta_e = self.normalize_angle(target_yaw-car_yaw)
-            theta_d = np.arctan2(self.k_stanley * error_front_axle, self.target_speed)
-            delta = theta_e + theta_d
+            theta_e = -(target_yaw-self.car_yaw_corrected)
+            theta_d = -np.arctan2(self.k_stanley * error_front_axle, self.target_speed)
+            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
@@ -156,13 +160,32 @@ def get_closest_track_point(self,axle_pos,car_yaw):
         return target_idx, error_front_axle
 
     def cal_target_yaw(self,cls_point):
+        if(cls_point==(len(self.ty)-1)):
+            dy = self.ty[cls_point]-self.ty[cls_point-1]
+            dx = self.tx[cls_point]-self.tx[cls_point-1]
+        else:
+            dy = self.ty[cls_point+1]-self.ty[cls_point]
+            dx = self.tx[cls_point+1]-self.tx[cls_point]
+
+        target_yaw_op1 = self.normalize_angle(np.arctan2(dy,dx))
+        target_yaw_op2 = self.normalize_angle(target_yaw_op1 + np.pi)
+
+        yaw_diff_1 = abs(target_yaw_op1-self.car_yaw_corrected)
+        yaw_diff_2 = abs(target_yaw_op2-self.car_yaw_corrected)
+        target_yaw2 = target_yaw_op1 if yaw_diff_1<yaw_diff_2 else target_yaw_op2
+
         dy_dx = np.gradient(self.ty, self.tx)
         # The gradient at the specified point_index
-        rate= dy_dx[cls_point]
-        return np.arctan(rate)
+        rate = dy_dx[cls_point]
+        target_yaw = np.arctan(rate)
+        if(rate)<0: 
+            target_yaw = 3.14+target_yaw
+
+
+        return target_yaw2
 
     def normalize_angle(self,angle):
-        return angle_mod(angle)
+        return angle_mod(angle,zero_2_2pi=True)
 
 
 

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"