pass ros message to controllers

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",

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
 

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]
 

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