finished stanley controller

rb_ws/src/buggy/buggy/controller/stanley_controller.py

@@ -0,0 +1,150 @@
+import sys
+import numpy as np
+
+from sensor_msgs.msg import NavSatFix
+from geometry_msgs.msg import Pose as ROSPose
+from nav_msgs.msg import Odometry
+
+sys.path.append("/rb_ws/src/buggy/buggy")
+from rb_ws.src.buggy.buggy.util.trajectory import Trajectory
+from controller.controller_superclass import Controller
+from util.pose import Pose
+
+import utm
+
+
+class StanleyController(Controller):
+    """
+    Stanley Controller (front axle used as reference point)
+    Referenced from this paper: https://ai.stanford.edu/~gabeh/papers/hoffmann_stanley_control07.pdf
+    """
+
+    LOOK_AHEAD_DIST_CONST = 0.05 # s
+    MIN_LOOK_AHEAD_DIST = 0.1 #m
+    MAX_LOOK_AHEAD_DIST = 2.0 #m
+
+    CROSS_TRACK_GAIN = 1.3
+    K_SOFT = 1.0 # m/s
+    K_D_YAW = 0.012 # rad / (rad/s)
+
+    def __init__(self, start_index, namespace, node):
+        super(StanleyController, self).__init__(start_index, namespace, node)
+        self.debug_reference_pos_publisher = self.node.create_publisher(
+            "/auton/debug/reference_navsat", NavSatFix, queue_size=1
+        )
+        self.debug_error_publisher = self.node.create_publisher(
+            "/auton/debug/error", ROSPose, queue_size=1
+        )
+
+    def compute_control(self, state_msg : Odometry, trajectory : Trajectory):
+        """Computes the steering angle determined by Stanley controller.
+        Does this by looking at the crosstrack error + heading error
+
+        Args:
+            state_msg: ros Odometry message
+            trajectory (Trajectory): reference trajectory
+
+        Returns:
+            float (desired steering angle)
+        """
+        if self.current_traj_index >= trajectory.get_num_points() - 1:
+            self.node.get_logger.error("[Stanley]: Ran out of path to follow!")
+            raise Exception("[Stanley]: Ran out of path to follow!")
+
+        current_rospose = state_msg.pose.pose
+        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_rospose.orientation.z
+        x, y = current_rospose.position.x, current_rospose.position.y #(Easting, Northing)
+
+        front_x = x + StanleyController.WHEELBASE * np.cos(heading)
+        front_y = y + StanleyController.WHEELBASE * np.sin(heading)
+
+        # setting range of indices to search so we don't have to search the entire path
+        traj_index = trajectory.get_closest_index_on_path(
+            front_x,
+            front_y,
+            start_index=self.current_traj_index - 20,
+            end_index=self.current_traj_index + 50,
+        )
+        self.current_traj_index = max(traj_index, self.current_traj_index)
+
+
+        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
+
+        ref_heading = trajectory.get_heading_by_index(
+            trajectory.get_index_from_distance(traj_dist))
+
+        error_heading = ref_heading - heading
+        error_heading = np.arctan2(np.sin(error_heading), np.cos(error_heading)) #Bounds error_heading
+
+        # Calculate cross track error by finding the distance from the buggy to the tangent line of
+        # the reference trajectory
+        closest_position = trajectory.get_position_by_index(self.current_traj_index)
+        next_position = trajectory.get_position_by_index(
+            self.current_traj_index + 0.0001
+        )
+        x1 = closest_position[0]
+        y1 = closest_position[1]
+        x2 = next_position[0]
+        y2 = next_position[1]
+        error_dist = -((x - x1) * (y2 - y1) - (y - y1) * (x2 - x1)) / np.sqrt(
+            (y2 - y1) ** 2 + (x2 - x1) ** 2
+        )
+
+
+        cross_track_component = -np.arctan2(
+            StanleyController.CROSS_TRACK_GAIN * error_dist, current_speed + StanleyController.K_SOFT
+        )
+
+        # Calculate yaw rate error
+        r_meas = yaw_rate
+        r_traj = current_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
+
+        #Determine steering_command
+        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)
+
+
+        #Calculate error, where x is in orientation of buggy, y is cross track error
+        current_pose = Pose(current_rospose.pose.x, current_rospose.pose.y, heading)
+        reference_error = current_pose.convert_point_from_global_to_local_frame(closest_position)
+        reference_error -= np.array([StanleyController.WHEELBASE, 0]) #Translae back to back wheel to get accurate error
+
+        error_pose = ROSPose()
+        error_pose.position.x = reference_error[0]
+        error_pose.position.y = reference_error[1]
+        self.debug_error_publisher.publish(error_pose)
+
+        # Publish reference position for debugging
+        try:
+            reference_navsat = NavSatFix()
+            lat, lon = utm.to_latlon(closest_position[0], closest_position[1], 17, "T")
+            reference_navsat.latitude = lat
+            reference_navsat.longitude = lon
+            self.debug_reference_pos_publisher.publish(reference_navsat)
+        except Exception as e:
+            self.node.get_logger().warn(
+                "[Stanley] Unable to convert closest track position lat lon; Error: " + str(e)
+            )
+
+        return steering_cmd
+
+
+
+
+
+
+
+
+
+
+

rb_ws/src/buggy/buggy/util/pose.py

@@ -0,0 +1,219 @@
+import numpy as np
+
+from geometry_msgs.msg import Pose as ROSPose
+from tf.transformations import euler_from_quaternion
+from tf.transformations import quaternion_from_euler
+
+
+class Pose:
+    """
+    A data structure for storing 2D poses, as well as a set of
+    convenience methods for transforming/manipulating poses
+
+    TODO: All Pose objects are with respect to World coordinates.
+    """
+
+    __x = None
+    __y = None
+    __theta = None
+
+    @staticmethod
+    def rospose_to_pose(rospose: ROSPose):
+        """
+        Converts a geometry_msgs/Pose to a pose3d/Pose
+
+        Args:
+            posestamped (geometry_msgs/Pose): pose to convert
+
+        Returns:
+            Pose: converted pose
+        """
+        (_, _, yaw) = euler_from_quaternion(
+            [
+                rospose.orientation.x,
+                rospose.orientation.y,
+                rospose.orientation.z,
+                rospose.orientation.w,
+            ]
+        )
+
+        p = Pose(rospose.position.x, rospose.position.y, yaw)
+        return p
+
+    def heading_to_quaternion(heading):
+        q = quaternion_from_euler(0, 0, heading)
+        return q
+
+    def __init__(self, x, y, theta):
+        self.x = x
+        self.y = y
+        self.theta = theta
+
+    def __repr__(self) -> str:
+        return f"Pose(x={self.x}, y={self.y}, theta={self.theta})"
+
+    def copy(self):
+        return Pose(self.x, self.y, self.theta)
+
+    @property
+    def x(self):
+        return self.__x
+
+    @x.setter
+    def x(self, x):
+        self.__x = x
+
+    @property
+    def y(self):
+        return self.__y
+
+    @y.setter
+    def y(self, y):
+        self.__y = y
+
+    @property
+    def theta(self):
+        if self.__theta > np.pi or self.__theta < -np.pi:
+            raise ValueError("Theta is not in [-pi, pi]")
+        return self.__theta
+
+    @theta.setter
+    def theta(self, theta):
+        # normalize theta to [-pi, pi]
+        self.__theta = np.arctan2(np.sin(theta), np.cos(theta))
+
+    def to_mat(self):
+        """
+        Returns the pose as a 3x3 homogeneous transformation matrix
+        """
+        return np.array(
+            [
+                [np.cos(self.theta), -np.sin(self.theta), self.x],
+                [np.sin(self.theta), np.cos(self.theta), self.y],
+                [0, 0, 1],
+            ]
+        )
+
+    @staticmethod
+    def from_mat(mat):
+        """
+        Creates a pose from a 3x3 homogeneous transformation matrix
+        """
+        return Pose(mat[0, 2], mat[1, 2], np.arctan2(mat[1, 0], mat[0, 0]))
+
+    def invert(self):
+        """
+        Inverts the pose
+        """
+        return Pose.from_mat(np.linalg.inv(self.to_mat()))
+
+    def convert_pose_from_global_to_local_frame(self, pose):
+        """
+        Converts the inputted pose from the global frame to the local frame
+        (relative to the current pose)
+        """
+        return pose / self
+
+    def convert_pose_from_local_to_global_frame(self, pose):
+        """
+        Converts the inputted pose from the local frame to the global frame
+        (relative to the current pose)
+        """
+        return pose * self
+
+    def convert_point_from_global_to_local_frame(self, point):
+        """
+        Converts the inputted point from the global frame to the local frame
+        (relative to the current pose)
+        """
+        point_hg = np.array([point[0], point[1], 1])
+        point_hg_local = np.linalg.inv(self.to_mat()) @ point_hg
+        return (
+            point_hg_local[0] / point_hg_local[2],
+            point_hg_local[1] / point_hg_local[2],
+        )
+
+    def convert_point_from_local_to_global_frame(self, point):
+        """
+        Converts the inputted point from the local frame to the global frame
+        (relative to the current pose)
+        """
+        point_hg = np.array([point[0], point[1], 1])
+        point_hg_global = self.to_mat() @ point_hg
+        return (
+            point_hg_global[0] / point_hg_global[2],
+            point_hg_global[1] / point_hg_global[2],
+        )
+
+    def convert_point_array_from_global_to_local_frame(self, points):
+        """
+        Converts the inputted point array from the global frame to the local frame
+        (relative to the current pose)
+
+        Args:
+            points (np.ndarray) [Size: (N,2)]: array of points to convert
+        """
+        points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))])
+        points_hg_local = np.linalg.inv(self.to_mat()) @ points_hg.T
+        return (points_hg_local[:2, :] / points_hg_local[2, :]).T
+
+    def convert_point_array_from_local_to_global_frame(self, points):
+        """
+        Converts the inputted point array from the local frame to the global frame
+        (relative to the current pose)
+
+        Args:
+            points (np.ndarray) [Size: (N,2)]: array of points to convert
+        """
+        points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))])
+        points_hg_global = self.to_mat() @ points_hg.T
+        return (points_hg_global[:2, :] / points_hg_global[2, :]).T
+
+    def rotate(self, angle):
+        """
+        Rotates the pose by the given angle
+        """
+        self.theta += angle
+
+    def translate(self, x, y):
+        """
+        Translates the pose by the given x and y distances
+        """
+        self.x += x
+        self.y += y
+
+    def __add__(self, other):
+        return Pose(self.x + other.x, self.y + other.y, self.theta + other.theta)
+
+    def __sub__(self, other):
+        return Pose(self.x - other.x, self.y - other.y, self.theta - other.theta)
+
+    def __neg__(self):
+        return Pose(-self.x, -self.y, -self.theta)
+
+    def __mul__(self, other):
+        p1_mat = self.to_mat()
+        p2_mat = other.to_mat()
+
+        return Pose.from_mat(p1_mat @ p2_mat)
+
+    def __truediv__(self, other):
+        p1_mat = self.to_mat()
+        p2_mat = other.to_mat()
+
+        return Pose.from_mat(np.linalg.inv(p2_mat) @ p1_mat)
+
+
+if __name__ == "__main__":
+    # TODO: again do we want example code in these classes
+    rospose = ROSPose()
+    rospose.position.x = 1
+    rospose.position.y = 2
+    rospose.position.z = 3
+    rospose.orientation.x = 0
+    rospose.orientation.y = 0
+    rospose.orientation.z = -0.061461
+    rospose.orientation.w = 0.9981095
+
+    pose = Pose.rospose_to_pose(rospose)
+    print(pose)  # Pose(x=1, y=2, theta=-0.123)