Autonsystem update (#79)

* Update stanley_controller.py

Tweaked params, added yaw rate compensation

* Added yaw rate

* Added yaw rate to controller.py

* updated yaw rate gain after mock roll

* pass ros message to controllers

---------

Co-authored-by: Mikhail Khrenov <mkhrenov@cmu.edu>
Co-authored-by: Buggy <buggy@buggy.com>

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
+        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.get_world_pose_and_speed(self.self_odom_msg)
+                state_msg = self.self_odom_msg
         else:
             with self.lock:
-                self_pose, self_speed = self.get_world_pose_and_speed(self.gps_odom_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)
+            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
+        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
@@ -16,12 +18,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.012 # rad / (rad/s)
 
     def __init__(self, buggy_name, start_index=0) -> None:
         super(StanleyController, self).__init__(start_index, buggy_name)
@@ -33,22 +36,29 @@ def __init__(self, buggy_name, start_index=0) -> None:
         )
 
     def compute_control(
-        self, current_pose: Pose, trajectory: Trajectory, current_speed: 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
+            yaw_rate (float): current yaw rate of the buggy (rad/s)
 
         Returns:
             float (desired steering angle)
         """
         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
@@ -71,19 +81,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 +106,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)