diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py index 3ad76cc..cbb3cc2 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 8436dc2..98bf97a 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 cee504c..76a0373 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 6b36a81..438eb72 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