From 2aeceaae70659a4e8d1b2731c30d237926f80fab Mon Sep 17 00:00:00 2001 From: Mikhail Khrenov Date: Wed, 3 Apr 2024 17:02:46 -0400 Subject: [PATCH 1/5] Update stanley_controller.py Tweaked params, added yaw rate compensation --- .../buggy/scripts/auton/stanley_controller.py | 47 ++++++++++--------- 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py index d39e3cf6..85de73de 100755 --- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py @@ -16,12 +16,13 @@ class StanleyController(Controller): Calculations based off FRONT axle of Buggy """ - LOOK_AHEAD_DIST_CONST = 0.1 + LOOK_AHEAD_DIST_CONST = 0.05 # s MIN_LOOK_AHEAD_DIST = 0.1 - MAX_LOOK_AHEAD_DIST = 2 + MAX_LOOK_AHEAD_DIST = 2.0 - CROSS_TRACK_GAIN = 1 - HEADING_GAIN = 0.3 + CROSS_TRACK_GAIN = 1.3 + K_SOFT = 1.0 # m/s + K_D_YAW = 0.06 # rad / (rad/s) def __init__(self, buggy_name, start_index=0) -> None: super(StanleyController, self).__init__(start_index, buggy_name) @@ -33,7 +34,7 @@ def __init__(self, buggy_name, start_index=0) -> None: ) def compute_control( - self, current_pose: Pose, trajectory: Trajectory, current_speed: float + self, current_pose: Pose, trajectory: Trajectory, current_speed: float, yaw_rate: float ): """Computes the steering angle necessary for stanley controller. Does this by looking at the crosstrack error + heading error @@ -41,7 +42,8 @@ def compute_control( Args: current_pose (Pose): current pose (x, y, theta) (UTM coordinates) trajectory (Trajectory): reference trajectory - current_speed (float): current speed of the buggy + current_speed (float): current speed of the buggy (m/s) + yaw_rate (float): current yaw rate of the buggy (rad/s) Returns: float (desired steering angle) @@ -71,19 +73,15 @@ def compute_control( lookahead_dist = np.clip( self.LOOK_AHEAD_DIST_CONST * current_speed, self.MIN_LOOK_AHEAD_DIST, - self.MAX_LOOK_AHEAD_DIST, - ) - traj_dist = ( - trajectory.get_distance_from_index(self.current_traj_index) + lookahead_dist - ) + self.MAX_LOOK_AHEAD_DIST) + + traj_dist = trajectory.get_distance_from_index(self.current_traj_index) + lookahead_dist + ref_heading = trajectory.get_heading_by_index( - trajectory.get_index_from_distance(traj_dist) - ) + trajectory.get_index_from_distance(traj_dist)) + error_heading = ref_heading - current_pose.theta - error_heading = ( - np.arctan2(np.sin(error_heading), np.cos(error_heading)) - * StanleyController.HEADING_GAIN - ) + error_heading = np.arctan2(np.sin(error_heading), np.cos(error_heading)) # Calculate cross track error by finding the distance from the buggy to the tangent line of # the reference trajectory @@ -100,14 +98,17 @@ def compute_control( ) speed = current_speed - if current_speed < 1: - speed = 1 - - cross_track_error = -np.arctan2( - StanleyController.CROSS_TRACK_GAIN * error_dist, speed + cross_track_component = -np.arctan2( + StanleyController.CROSS_TRACK_GAIN * error_dist, speed + StanleyController.K_SOFT ) - steering_cmd = error_heading + cross_track_error + # Calculate yaw rate error + r_meas = yaw_rate + r_traj = speed * (trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist) + 0.05) + - trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist))) / 0.05 + + + steering_cmd = error_heading + cross_track_component + StanleyController.K_D_YAW * (r_traj - r_meas) steering_cmd = np.clip(steering_cmd, -np.pi / 9, np.pi / 9) reference_position = trajectory.get_position_by_index(self.current_traj_index) From 9168155800d61102962c7c33bd30aeaa8e99b956 Mon Sep 17 00:00:00 2001 From: Mikhail Khrenov Date: Wed, 3 Apr 2024 17:04:42 -0400 Subject: [PATCH 2/5] Added yaw rate --- rb_ws/src/buggy/scripts/auton/autonsystem.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index e1aba539..89bd5b23 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -130,7 +130,7 @@ def init_check(self): # waits until covariance is acceptable to check heading with self.lock: - self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg) + self_pose, _, _ = self.get_world_pose_and_speed(self.self_odom_msg) current_heading = self_pose.theta closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path(self_pose.x, self_pose.y)) print("current heading: ", np.rad2deg(current_heading)) @@ -163,7 +163,7 @@ def tick_caller(self): # initialize global trajectory index with self.lock: - _, _ = self.get_world_pose_and_speed(self.self_odom_msg) + _, _, _ = self.get_world_pose_and_speed(self.self_odom_msg) t_planner = threading.Thread(target=self.planner_thread) t_controller = threading.Thread(target=self.local_controller_thread) @@ -192,7 +192,7 @@ def get_world_pose_and_speed(self, msg): # Get data from message pose_gps = Pose.rospose_to_pose(current_rospose) - return World.gps_to_world_pose(pose_gps), current_speed + return World.gps_to_world_pose(pose_gps), current_speed, msg.twist.twist.angular.z def local_controller_thread(self): while (not rospy.is_shutdown()): @@ -201,13 +201,13 @@ def local_controller_thread(self): def local_controller_tick(self): with self.lock: - self_pose, self_speed = self.get_world_pose_and_speed(self.self_odom_msg) + self_pose, self_speed, self_yaw_rate = self.get_world_pose_and_speed(self.self_odom_msg) self.heading_publisher.publish(Float32(np.rad2deg(self_pose.theta))) # Compute control output steering_angle = self.local_controller.compute_control( - self_pose, self.cur_traj, self_speed) + self_pose, self.cur_traj, self_speed, self_yaw_rate) steering_angle_deg = np.rad2deg(steering_angle) self.steer_publisher.publish(Float64(steering_angle_deg)) @@ -217,8 +217,8 @@ def planner_thread(self): self.rosrate_planner.sleep() if not self.other_odom_msg is None: with self.lock: - self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg) - other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg) + self_pose, _, _ = self.get_world_pose_and_speed(self.self_odom_msg) + other_pose, _, _ = self.get_world_pose_and_speed(self.other_odom_msg) distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2 distance = np.sqrt(distance) self.distance_publisher.publish(Float64(distance)) @@ -228,8 +228,8 @@ def planner_thread(self): def planner_tick(self): with self.lock: - self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg) - other_pose, _ = self.get_world_pose_and_speed(self.other_odom_msg) + self_pose, _, _ = self.get_world_pose_and_speed(self.self_odom_msg) + other_pose, _, _ = self.get_world_pose_and_speed(self.other_odom_msg) # update local trajectory via path planner self.cur_traj, cur_idx = self.path_planner.compute_traj( From ad65c5dec77f737d68f2a84456bee83070da3c39 Mon Sep 17 00:00:00 2001 From: Mikhail Khrenov Date: Wed, 3 Apr 2024 17:05:10 -0400 Subject: [PATCH 3/5] Added yaw rate to controller.py --- rb_ws/src/buggy/scripts/auton/controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/scripts/auton/controller.py b/rb_ws/src/buggy/scripts/auton/controller.py index 08be1013..8436dc27 100755 --- a/rb_ws/src/buggy/scripts/auton/controller.py +++ b/rb_ws/src/buggy/scripts/auton/controller.py @@ -47,7 +47,7 @@ def __init__(self, start_index, buggy_name) -> None: @abstractmethod def compute_control( - self, current_pose: Pose, trajectory: Trajectory, current_speed: float + self, current_pose: Pose, trajectory: Trajectory, current_speed: float, yaw_rate: float ): """ Computes the desired control output given the current state and reference trajectory From eb10ce62fd7336cc2fa60a12bf0c8e932603b981 Mon Sep 17 00:00:00 2001 From: Buggy Date: Wed, 3 Apr 2024 19:50:47 -0400 Subject: [PATCH 4/5] updated yaw rate gain after mock roll --- .../src/buggy/scripts/auton/stanley_controller.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py index 85de73de..6b36a811 100755 --- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py @@ -22,7 +22,7 @@ class StanleyController(Controller): CROSS_TRACK_GAIN = 1.3 K_SOFT = 1.0 # m/s - K_D_YAW = 0.06 # rad / (rad/s) + K_D_YAW = 0.012 # rad / (rad/s) def __init__(self, buggy_name, start_index=0) -> None: super(StanleyController, self).__init__(start_index, buggy_name) @@ -74,14 +74,14 @@ def compute_control( self.LOOK_AHEAD_DIST_CONST * current_speed, self.MIN_LOOK_AHEAD_DIST, self.MAX_LOOK_AHEAD_DIST) - + traj_dist = trajectory.get_distance_from_index(self.current_traj_index) + lookahead_dist - + ref_heading = trajectory.get_heading_by_index( trajectory.get_index_from_distance(traj_dist)) - + error_heading = ref_heading - current_pose.theta - error_heading = np.arctan2(np.sin(error_heading), np.cos(error_heading)) + error_heading = np.arctan2(np.sin(error_heading), np.cos(error_heading)) # Calculate cross track error by finding the distance from the buggy to the tangent line of # the reference trajectory @@ -104,9 +104,9 @@ def compute_control( # Calculate yaw rate error r_meas = yaw_rate - r_traj = speed * (trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist) + 0.05) + r_traj = speed * (trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist) + 0.05) - trajectory.get_heading_by_index(trajectory.get_index_from_distance(traj_dist))) / 0.05 - + steering_cmd = error_heading + cross_track_component + StanleyController.K_D_YAW * (r_traj - r_meas) steering_cmd = np.clip(steering_cmd, -np.pi / 9, np.pi / 9) From f9042e719e9852ed5928b74a8154e7731c5ce2c6 Mon Sep 17 00:00:00 2001 From: Jackack Date: Thu, 4 Apr 2024 18:43:31 -0400 Subject: [PATCH 5/5] pass ros message to controllers --- rb_ws/src/buggy/scripts/auton/autonsystem.py | 44 ++++++++----------- rb_ws/src/buggy/scripts/auton/controller.py | 5 ++- .../auton/model_predictive_controller.py | 12 ++++- .../buggy/scripts/auton/stanley_controller.py | 14 ++++-- 4 files changed, 44 insertions(+), 31 deletions(-) diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 3ad76cc9..cbb3cc2f 100755 --- a/rb_ws/src/buggy/scripts/auton/autonsystem.py +++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py @@ -144,7 +144,7 @@ def init_check(self): # waits until covariance is acceptable to check heading with self.lock: - self_pose, _, _ = self.get_world_pose_and_speed(self.self_odom_msg) + self_pose = self.get_world_pose(self.self_odom_msg) current_heading = self_pose.theta closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path(self_pose.x, self_pose.y)) print("current heading: ", np.rad2deg(current_heading)) @@ -176,9 +176,6 @@ def tick_caller(self): self.init_check_publisher.publish(True) # initialize global trajectory index - with self.lock: - _, _, _ = self.get_world_pose_and_speed(self.self_odom_msg) - t_planner = threading.Thread(target=self.planner_thread) t_controller = threading.Thread(target=self.local_controller_thread) @@ -196,17 +193,12 @@ def tick_caller(self): if self.has_other_buggy: t_planner.join() - - def get_world_pose_and_speed(self, msg): + def get_world_pose(self, msg): current_rospose = msg.pose.pose - # Check if the pose covariance is a sane value. Publish a warning if insane - current_speed = np.sqrt( - msg.twist.twist.linear.x**2 + msg.twist.twist.linear.y**2 - ) # Get data from message pose_gps = Pose.rospose_to_pose(current_rospose) - return World.gps_to_world_pose(pose_gps), current_speed, msg.twist.twist.angular.z + return World.gps_to_world_pose(pose_gps) def local_controller_thread(self): while (not rospy.is_shutdown()): @@ -216,17 +208,18 @@ def local_controller_thread(self): def local_controller_tick(self): if not self.use_gps_pos: with self.lock: - self_pose, self_speed, self_yaw_rate = self.get_world_pose_and_speed(self.self_odom_msg) + state_msg = self.self_odom_msg else: with self.lock: - self_pose, self_speed, self_yaw_rate = self.get_world_pose_and_speed(self.self_gps_msg) - + state_msg = self.gps_odom_msg - self.heading_publisher.publish(Float32(np.rad2deg(self_pose.theta))) + # For viz and debugging + pose = self.get_world_pose(state_msg) + self.heading_publisher.publish(Float32(np.rad2deg(pose.theta))) # Compute control output steering_angle = self.local_controller.compute_control( - self_pose, self.cur_traj, self_speed, self_yaw_rate) + state_msg, self.cur_traj) steering_angle_deg = np.rad2deg(steering_angle) self.steer_publisher.publish(Float64(steering_angle_deg)) @@ -236,24 +229,25 @@ def planner_thread(self): self.rosrate_planner.sleep() if not self.other_odom_msg is None: with self.lock: - self_pose, _, _ = self.get_world_pose_and_speed(self.self_odom_msg) - other_pose, _, _ = self.get_world_pose_and_speed(self.other_odom_msg) - distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2 - distance = np.sqrt(distance) - self.distance_publisher.publish(Float64(distance)) + self_pose = self.get_world_pose(self.self_odom_msg) + other_pose = self.get_world_pose(self.other_odom_msg) + + distance = (self_pose.x - other_pose.x) ** 2 + (self_pose.y - other_pose.y) ** 2 + distance = np.sqrt(distance) + self.distance_publisher.publish(Float64(distance)) self.planner_tick() def planner_tick(self): if not self.use_gps_pos: with self.lock: - self_pose, _, _ = self.get_world_pose_and_speed(self.self_odom_msg) + self_pose = self.get_world_pose(self.self_odom_msg) else: with self.lock: - self_pose, _, _ = self.get_world_pose_and_speed(self.gps_odom_msg) + self_pose = self.get_world_pose(self.gps_odom_msg) with self.lock: - other_pose, _, _ = self.get_world_pose_and_speed(self.other_odom_msg) + other_pose = self.get_world_pose(self.other_odom_msg) # update local trajectory via path planner self.cur_traj, cur_idx = self.path_planner.compute_traj( @@ -292,7 +286,7 @@ def planner_tick(self): type=str, help="Path of curb data, relative to /rb_ws/src/buggy/paths/", default="" -, required=True) +, required=False) parser.add_argument( "--other_name", diff --git a/rb_ws/src/buggy/scripts/auton/controller.py b/rb_ws/src/buggy/scripts/auton/controller.py index 8436dc27..98bf97aa 100755 --- a/rb_ws/src/buggy/scripts/auton/controller.py +++ b/rb_ws/src/buggy/scripts/auton/controller.py @@ -4,6 +4,7 @@ from pose import Pose from sensor_msgs.msg import NavSatFix from world import World +from nav_msgs.msg import Odometry class Controller(ABC): @@ -47,13 +48,13 @@ def __init__(self, start_index, buggy_name) -> None: @abstractmethod def compute_control( - self, current_pose: Pose, trajectory: Trajectory, current_speed: float, yaw_rate: float + self, state_msg: Odometry, trajectory: Trajectory, ): """ Computes the desired control output given the current state and reference trajectory Args: - state (numpy.ndarray [size: (3,)]): current pose (x, y, theta) + state: (Odometry): state of the buggy, including position, attitude and associated twists trajectory (Trajectory): reference trajectory current_speed (float): current speed of the buggy diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py index cee504cc..76a03738 100644 --- a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py +++ b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py @@ -14,6 +14,8 @@ from sensor_msgs.msg import NavSatFix from geometry_msgs.msg import Pose as ROSPose from geometry_msgs.msg import Pose2D +from nav_msgs.msg import Odometry + from pose import Pose from trajectory import Trajectory @@ -723,7 +725,15 @@ def compute_trajectory(self, current_pose: Pose, trajectory: Trajectory, current return state_trajectory # return steer_angle - def compute_control(self, current_pose: Pose, trajectory: Trajectory, current_speed: float): + def compute_control(self, + state_msg: Odometry, trajectory: Trajectory): + + current_rospose = state_msg.pose.pose + current_pose = World.gps_to_world_pose(Pose.rospose_to_pose(current_rospose)) + current_speed = np.sqrt( + state_msg.twist.twist.linear.x**2 + state_msg.twist.twist.linear.y**2 + ) + state_trajectory = self.compute_trajectory(current_pose, trajectory, current_speed) steer_angle = state_trajectory[0, self.N_STATES - 1] diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py index 6b36a811..438eb720 100755 --- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py @@ -3,6 +3,8 @@ import rospy from sensor_msgs.msg import NavSatFix from geometry_msgs.msg import Pose as ROSPose +from nav_msgs.msg import Odometry + from pose import Pose from trajectory import Trajectory @@ -34,15 +36,14 @@ def __init__(self, buggy_name, start_index=0) -> None: ) def compute_control( - self, current_pose: Pose, trajectory: Trajectory, current_speed: float, yaw_rate: float + self, state_msg: Odometry, trajectory: Trajectory ): """Computes the steering angle necessary for stanley controller. Does this by looking at the crosstrack error + heading error Args: - current_pose (Pose): current pose (x, y, theta) (UTM coordinates) + state_msg: ros Odometry message trajectory (Trajectory): reference trajectory - current_speed (float): current speed of the buggy (m/s) yaw_rate (float): current yaw rate of the buggy (rad/s) Returns: @@ -51,6 +52,13 @@ def compute_control( if self.current_traj_index >= trajectory.get_num_points() - 1: raise Exception("[Stanley]: Ran out of path to follow!") + current_rospose = state_msg.pose.pose + current_pose = World.gps_to_world_pose(Pose.rospose_to_pose(current_rospose)) + current_speed = np.sqrt( + state_msg.twist.twist.linear.x**2 + state_msg.twist.twist.linear.y**2 + ) + yaw_rate = state_msg.twist.twist.angular.z + heading = current_pose.theta # in radians x = current_pose.x y = current_pose.y