From 2aba16d0a676c4752676e64357907b2e75138dcb Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sat, 21 Dec 2024 21:20:26 -0800 Subject: [PATCH 01/14] initial setup --- .../buggy/buggy/controller/controller_node.py | 93 +++++++++++++++++++ .../buggy/controller/controller_superclass.py | 0 .../buggy/controller/stanley_controller.py | 0 rb_ws/src/buggy/launch/controller.xml | 4 + rb_ws/src/buggy/setup.py | 3 +- 5 files changed, 99 insertions(+), 1 deletion(-) create mode 100644 rb_ws/src/buggy/buggy/controller/controller_node.py create mode 100644 rb_ws/src/buggy/buggy/controller/controller_superclass.py create mode 100644 rb_ws/src/buggy/buggy/controller/stanley_controller.py create mode 100644 rb_ws/src/buggy/launch/controller.xml diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py new file mode 100644 index 00000000..db56fe5b --- /dev/null +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -0,0 +1,93 @@ +import threading + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float32, Float64, Bool +from nav_msgs.msg import Odometry + +class Controller(Node): + + def __init__(self): + """ + Constructor for Controller class. + + Creates a ROS node with a publisher that periodically sends a message + indicating whether the node is still alive. + + """ + super().__init__('watchdog') + + + #Parameters + self.declare_parameter("controller_name", "stanley") + self.local_controller_name = self.get_parameter("controller_name") + + # Publishers + self.init_check_publisher = self.create_subscription(Bool, + "debug/init_safety_check", queue_size=1 + ) + self.steer_publisher = self.create_subscription.Publisher( + Float64, "/buggy/steering", queue_size=1 + ) + self.heading_publisher = self.create_subscription.Publisher( + Float32, "/auton/debug/heading", queue_size=1 + ) + self.distance_publisher = self.create_subscription.Publisher( + Float64, "/auton/debug/distance", queue_size=1 + ) + + # Subscribers + self.odom_subscriber = self.create_subscription(Odometry, 'self/buggy/state', self.odom_listener, 1) + self.traj_subscriber = self.create_subscription(Odometry, 'self/cur_traj', self.traj_listener, 1) + + self.lock = threading.Lock() + self.ticks = 0 + #TODO: FIGURE OUT WHAT THESE ARE BEFORE MERGING + self.self_odom_msg = None + self.gps_odom_msg = None + self.other_odom_msg = None + self.use_gps_pos = False + + timer_period = 0.01 # seconds (100 Hz) + self.timer = self.create_timer(timer_period, self.loop) + self.i = 0 # Loop Counter + + # + + def loop(self): + # Loop for the code that operates every 10ms + msg = Bool() + msg.data = True + self.heartbeat_publisher.publish(msg) + + def odom_listener(self, msg : Odometry): + ''' + This is the subscriber that updates the buggies state for navigation + msg, should be a CLEAN state as defined in the wiki + ''' + raise NotImplemented + + def traj_listener(self, msg): #TYPE UNKOWN AS OF NOW?? CUSTOM TYPE WHEN #TODO: FIGURE OUT BEFORE MERGE + ''' + This is the subscriber that updates the buggies trajectory for navigation + ''' + raise NotImplemented + + +def main(args=None): + rclpy.init(args=args) + + watchdog = Controller() + + rclpy.spin(watchdog) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + watchdog.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py new file mode 100644 index 00000000..e69de29b diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py new file mode 100644 index 00000000..e69de29b diff --git a/rb_ws/src/buggy/launch/controller.xml b/rb_ws/src/buggy/launch/controller.xml new file mode 100644 index 00000000..52f0ac7f --- /dev/null +++ b/rb_ws/src/buggy/launch/controller.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index 42318d08..cd28d34d 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -26,7 +26,8 @@ 'console_scripts': [ 'hello_world = buggy.hello_world:main', 'sim_single = buggy.simulator.engine:main', - 'watchdog = buggy.watchdog.watchdog_node:main' + 'watchdog = buggy.watchdog.watchdog_node:main', + 'controller = buggy.controller.controller_node:main' ], }, ) \ No newline at end of file From d610f825558c5097449525e16ac2cd550df1958a Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 22 Dec 2024 13:25:24 -0800 Subject: [PATCH 02/14] wrote init_check, based on old trajectory (slightly modified) --- .../buggy/buggy/controller/controller_node.py | 95 +++++++++++++++---- 1 file changed, 76 insertions(+), 19 deletions(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index db56fe5b..ae800e75 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -1,4 +1,7 @@ import threading +import sys + +import numpy as np import rclpy from rclpy.node import Node @@ -6,6 +9,9 @@ from std_msgs.msg import Float32, Float64, Bool from nav_msgs.msg import Odometry +sys.path.append("/rb_ws/src/buggy/buggy") +from rb_ws.src.buggy.buggy.util.trajectory_old import Trajectory + class Controller(Node): def __init__(self): @@ -20,8 +26,22 @@ def __init__(self): #Parameters + self.declare_parameter("dist", 0.0) #Starting Distance along path + start_dist = self.get_parameter("dist") + + self.declare_parameter("traj_name", "buggycourse_path.json") + traj_name = self.get_parameter("traj_name") + self.cur_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good + + start_index = self.cur_traj.get_index_from_distance(start_dist) + self.declare_parameter("controller_name", "stanley") - self.local_controller_name = self.get_parameter("controller_name") + match self.get_parameter("controller_name"): + case "stanley": + self.controller = StanleyController(start_index = start_index) #IMPORT STANLEY + case _: + self.get_logger().error("Invalid Controller Name!") + raise Exception("Invalid Controller Argument") # Publishers self.init_check_publisher = self.create_subscription(Bool, @@ -42,39 +62,76 @@ def __init__(self): self.traj_subscriber = self.create_subscription(Odometry, 'self/cur_traj', self.traj_listener, 1) self.lock = threading.Lock() - self.ticks = 0 - #TODO: FIGURE OUT WHAT THESE ARE BEFORE MERGING - self.self_odom_msg = None - self.gps_odom_msg = None - self.other_odom_msg = None - self.use_gps_pos = False + + self.odom = None timer_period = 0.01 # seconds (100 Hz) self.timer = self.create_timer(timer_period, self.loop) - self.i = 0 # Loop Counter - - # - - def loop(self): - # Loop for the code that operates every 10ms - msg = Bool() - msg.data = True - self.heartbeat_publisher.publish(msg) - + def odom_listener(self, msg : Odometry): ''' This is the subscriber that updates the buggies state for navigation msg, should be a CLEAN state as defined in the wiki ''' - raise NotImplemented + with self.lock: + self.odom = msg def traj_listener(self, msg): #TYPE UNKOWN AS OF NOW?? CUSTOM TYPE WHEN #TODO: FIGURE OUT BEFORE MERGE ''' This is the subscriber that updates the buggies trajectory for navigation ''' - raise NotImplemented + with self.lock: + self.cur_traj, self.controller.current_traj_index = Trajectory.unpack(msg) + + def init_check(self): + """ + Checks if it's safe to switch the buggy into autonomous driving mode. + Specifically, it checks: + if we can recieve odometry messages from the buggy + if the covariance is acceptable (less than 1 meter) + if the buggy thinks it is facing in the correct direction wrt the local trajectory (not 180 degrees flipped) + + Returns: + A boolean describing the status of the buggy (safe for auton or unsafe for auton) + """ + if (self.odom == None): + self.get_logger().warn("WARNING: no available position estimate") + return False + + elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1**2): + self.get_logger().warn("checking position estimate certainty") + return False + + #Originally under a lock, doesn't seem necessary? + current_heading = self.odom.pose.pose.orientation.z + closest_heading = self.cur_traj.get_heading_by_index(self.cur_traj.get_closest_index_on_path(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y)) + + self.get_logger().info("current heading: " + str(np.rad2deg(current_heading))) + self.heading_publisher.publish(Float32(np.rad2deg(current_heading))) + + #Converting headings from [-pi, pi] to [0, 2pi] + if (current_heading < 0): + current_heading = 2 * np.pi + current_heading + if (closest_heading < 0): + closest_heading = 2 * np.pi + closest_heading + + if (abs(current_heading - closest_heading) >= np.pi/2): + self.get_logger().warn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(current_heading))) + return False + + return True + + + def loop(self): + # Loop for the code that operates every 10ms + msg = Bool() + msg.data = True + self.heartbeat_publisher.publish(msg) + + + def main(args=None): rclpy.init(args=args) From 78d6ec7881c540fb43a30131c4b82ee40a115b65 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Mon, 23 Dec 2024 17:36:22 -0800 Subject: [PATCH 03/14] finished controller node --- .../buggy/buggy/controller/controller_node.py | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index ae800e75..74115721 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -53,9 +53,6 @@ def __init__(self): self.heading_publisher = self.create_subscription.Publisher( Float32, "/auton/debug/heading", queue_size=1 ) - self.distance_publisher = self.create_subscription.Publisher( - Float64, "/auton/debug/distance", queue_size=1 - ) # Subscribers self.odom_subscriber = self.create_subscription(Odometry, 'self/buggy/state', self.odom_listener, 1) @@ -64,6 +61,7 @@ def __init__(self): self.lock = threading.Lock() self.odom = None + self.passed_init = False timer_period = 0.01 # seconds (100 Hz) self.timer = self.create_timer(timer_period, self.loop) @@ -121,14 +119,19 @@ def init_check(self): return True - - - def loop(self): - # Loop for the code that operates every 10ms - msg = Bool() - msg.data = True - self.heartbeat_publisher.publish(msg) + if not self.passed_init: + self.passed_init = self.init_check() + self.init_check_publisher.publish(self.passed_init) + if self.passed_init: + self.get_logger.info("Passed Initialization Check") + else: + return + + self.heading_publisher.publish(Float32(np.rad2deg(self.odom.pose.pose.orientation.z))) + steering_angle = self.controller.compute_control(self.odom, self.cur_traj) + steering_angle_deg = np.rad2deg(steering_angle) + self.steer_publisher.publish(Float64(steering_angle_deg)) From 15496e6421c667485e173c05fa48361c0de5c826 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Mon, 23 Dec 2024 17:36:48 -0800 Subject: [PATCH 04/14] small modif --- rb_ws/src/buggy/buggy/controller/controller_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index 74115721..91fef615 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -22,7 +22,7 @@ def __init__(self): indicating whether the node is still alive. """ - super().__init__('watchdog') + super().__init__('controller') #Parameters From b10857fd578d50555a5f1d5165c53abd21bccb73 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 24 Dec 2024 18:33:36 -0800 Subject: [PATCH 05/14] almost done with compute control --- .../buggy/buggy/controller/controller_node.py | 17 ++++--- .../buggy/controller/controller_superclass.py | 50 +++++++++++++++++++ 2 files changed, 59 insertions(+), 8 deletions(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index 91fef615..051f4d0a 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -10,7 +10,8 @@ from nav_msgs.msg import Odometry sys.path.append("/rb_ws/src/buggy/buggy") -from rb_ws.src.buggy.buggy.util.trajectory_old import Trajectory +from util.trajectory_old import Trajectory +from controller.stanley_controller import StanleyController class Controller(Node): @@ -38,20 +39,20 @@ def __init__(self): self.declare_parameter("controller_name", "stanley") match self.get_parameter("controller_name"): case "stanley": - self.controller = StanleyController(start_index = start_index) #IMPORT STANLEY + self.controller = StanleyController(start_index = start_index, buggy_name = self.get_namespace(), node=self) #IMPORT STANLEY case _: self.get_logger().error("Invalid Controller Name!") raise Exception("Invalid Controller Argument") # Publishers - self.init_check_publisher = self.create_subscription(Bool, - "debug/init_safety_check", queue_size=1 + self.init_check_publisher = self.create_publisher(Bool, + "debug/init_safety_check", 1 ) - self.steer_publisher = self.create_subscription.Publisher( - Float64, "/buggy/steering", queue_size=1 + self.steer_publisher = self.create_publisher( + Float64, "/buggy/steering", 1 ) - self.heading_publisher = self.create_subscription.Publisher( - Float32, "/auton/debug/heading", queue_size=1 + self.heading_publisher = self.create_publisher( + Float32, "/auton/debug/heading", 1 ) # Subscribers diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py index e69de29b..6944ccd9 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py +++ b/rb_ws/src/buggy/buggy/controller/controller_superclass.py @@ -0,0 +1,50 @@ +from abc import ABC, abstractmethod +import sys + +from sensor_msgs.msg import NavSatFix +from nav_msgs.msg import Odometry + + +sys.path.append("/rb_ws/src/buggy/buggy") +from rb_ws.src.buggy.buggy.util.trajectory_old import Trajectory + +class Controller(ABC): + """ + Base class for all controllers. + + The controller takes in the current state of the buggy and a reference + trajectory. It must then compute the desired control output. + + The method that it does this by is up to the implementation, of course. + Example schemes include Pure Pursuit, Stanley, and LQR. + """ + + # TODO: move this to a constants class + NAND_WHEELBASE = 1.3 + SC_WHEELBASE = 1.104 + + def __init__(self, start_index: int, namespace : str, node) -> None: + self.namespace = namespace + if namespace.upper() == '/NAND': + Controller.WHEELBASE = self.NAND_WHEELBASE + else: + Controller.WHEELBASE = self.SC_WHEELBASE + + self.current_traj_index = start_index + self.node = node + + @abstractmethod + def compute_control( + self, state_msg: Odometry, trajectory: Trajectory, + ) -> float: + """ + Computes the desired control output given the current state and reference trajectory + + Args: + state: (Odometry): state of the buggy, including position, attitude and associated twists + trajectory (Trajectory): reference trajectory + + Returns: + float (desired steering angle) + """ + raise NotImplementedError \ No newline at end of file From f495ddc5bf1c63e7ac9face5eb37465391bbeec9 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 24 Dec 2024 19:16:49 -0800 Subject: [PATCH 06/14] finished stanley controller --- .../buggy/controller/stanley_controller.py | 150 +++++++ rb_ws/src/buggy/buggy/util/pose.py | 219 +++++++++++ rb_ws/src/buggy/buggy/util/trajectory.py | 365 ++++++++++++++++++ 3 files changed, 734 insertions(+) create mode 100644 rb_ws/src/buggy/buggy/util/pose.py create mode 100644 rb_ws/src/buggy/buggy/util/trajectory.py diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py index e69de29b..d8be46b6 100644 --- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py +++ b/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 + + + + + + + + + + + diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/buggy/util/pose.py new file mode 100644 index 00000000..985755a2 --- /dev/null +++ b/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) diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py new file mode 100644 index 00000000..a7da7dfb --- /dev/null +++ b/rb_ws/src/buggy/buggy/util/trajectory.py @@ -0,0 +1,365 @@ +import json +import time +import uuid +import matplotlib.pyplot as plt + +from buggy.msg import TrajectoryMsg + +import numpy as np +from scipy.interpolate import Akima1DInterpolator, CubicSpline + +import utm + + +class Trajectory: + """A wrapper around a trajectory JSON file that does some under-the-hood math. Will + interpolate the data points and calculate other information such as distance along the trajectory + and instantaneous curvature. + + Currently, the trajectory is assumed to be a straight line between each waypoint. This is + not a good assumption, but it's good enough for now. This means that the curvature is zero + anywhere between waypoints. + + This class also has a method that will find the closest point on the trajectory to a given + point in the world. This is useful for finding where the buggy is on the trajectory. + + Use https://rdudhagra.github.io/eracer-portal/ to make trajectories and save the JSON file + """ + + def __init__(self, json_filepath=None, positions = None, interpolator="CubicSpline") -> None: + """ + Args: + json_filepath (String): file path to the path json file (begins at /rb_ws) + positions [[float, float]]: reference trajectory in world coordinates + current_speed (float): current speed of the buggy + + Returns: + float (desired steering angle) + """ + self.distances = np.zeros((0, 1)) # (N/dt x 1) [d, d, ...] + self.positions = np.zeros((0, 2)) # (N x 2) [(x,y), (x,y), ...] + self.indices = None # (N x 1) [0, 1, 2, ...] + self.interpolation = None # scipy.interpolate.PPoly + + # read positions from file + if positions is None: + positions = [] + # Load the json file + with open(json_filepath, "r") as f: + data = json.load(f) + + # Iterate through the waypoints and extract the positions + num_waypoints = len(data) + for i in range(0, num_waypoints): + + waypoint = data[i] + + lat = waypoint["lat"] + lon = waypoint["lon"] + + # Convert to world coordinates + x, y = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized + positions.append([x, y]) + + positions = np.array(positions) + + num_indices = positions.shape[0] + + if interpolator == "Akima": + self.positions = positions + self.indices = np.arange(num_indices) + self.interpolation = Akima1DInterpolator(self.indices, self.positions) + self.interpolation.extrapolate = True + elif interpolator == "CubicSpline": + temp_traj = Trajectory(positions=positions, interpolator="Akima") + tot_len = temp_traj.distances[-1] + interp_dists = np.linspace(0, tot_len, num_indices) + + self.indices = np.arange(num_indices) + self.positions = [ + temp_traj.get_position_by_distance(interp_dist) + for interp_dist in interp_dists + ] + self.positions = np.array(self.positions) + + self.interpolation = CubicSpline(self.indices, self.positions) + self.interpolation.extrapolate = True + + # TODO: check units + # Calculate the distances along the trajectory + dt = 0.01 #dt is time step (in seconds (?)) + ts = np.arange(len(self.positions) - 1, step=dt) # times corresponding to each position (?) + drdt = self.interpolation( + ts, nu=1 + ) # Calculate derivatives of interpolated path wrt indices + ds = np.sqrt(drdt[:, 0] ** 2 + drdt[:, 1] ** 2) * dt # distances to each interpolated point + s = np.cumsum(np.hstack([[0], ds[:-1]])) + self.distances = s + self.dt = dt + + def get_num_points(self): + """Gets the number of points along the trajectory + + Returns: + int: number of points + """ + return len(self.positions) + + def get_heading_by_index(self, index): + """Gets the heading at given index along trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: (theta) in rads + """ + # theta = np.interp(index, self.indices, self.positions[:, 2]) + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + theta = np.arctan2(dydt, dxdt) + + return theta + + def get_heading_by_distance(self, distance): + """Gets the heading at given distance along trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + float: (theta) in rads + """ + index = self.get_index_from_distance(distance) + return self.get_heading_by_index(index) + + def get_position_by_index(self, index): + """Gets the position at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + tuple: (x, y) + """ + # Interpolate the position + + return self.interpolation(index) + + def get_position_by_distance(self, distance): + """Gets the position at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + tuple: (x, y) + """ + # Interpolate the position + index = self.get_index_from_distance(distance) + return self.get_position_by_index(index) + + def get_steering_angle_by_index(self, index, wheelbase): + """Gets the bicycle-model steering angle at a given distance along the trajectory, + interpolating if necessary. Assumes that the origin point of the buggy is at the + center of the rear axle. + + Args: + index (float): index along the trajectory + wheelbase (float): wheelbase of the buggy in meters + + Returns: + float: steering angle in rads + """ + curvature = self.get_curvature_by_index(index) + return np.arctan(wheelbase * curvature) + + def get_steering_angle_by_distance(self, distance, wheelbase): + """Gets the bicycle-model steering angle at a given distance along the trajectory, + interpolating if necessary. Assumes that the origin point of the buggy is at the + center of the rear axle. + + Args: + distance (float): distance along the trajectory in meters + wheelbase (float): wheelbase of the buggy in meters + + Returns: + float: steering angle in rads + """ + index = self.get_index_from_distance(distance) + return self.get_steering_angle_by_index(index, wheelbase) + + def get_index_from_distance(self, distance): + """Gets the index at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + int: index along the trajectory + """ + # Interpolate the index + index = np.interp( + distance, + self.distances, + np.linspace(0, len(self.distances), len(self.distances)), + ) + + return index * self.dt + + def get_distance_from_index(self, index): + """Gets the distance at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: distance along the trajectory in meters + """ + # Interpolate the distance + distance = np.interp( + index / self.dt, np.arange(len(self.distances)), self.distances + ) + + return distance + + def get_curvature_by_index(self, index): + """Gets the curvature at a given index along the trajectory, + interpolating if necessary + + Args: + index (float): index along the trajectory + + Returns: + float: curvature + """ + # Interpolate the curvature + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + ddxdtt, ddydtt = self.interpolation(index, nu=2).reshape((-1, 2)).T + + curvature = (dxdt * ddydtt - dydt * ddxdtt) / ( + (dxdt**2 + dydt**2) ** (3 / 2) + ) + + return curvature + + def get_curvature_by_distance(self, distance): + """Gets the curvature at a given distance along the trajectory, + interpolating if necessary + + Args: + distance (float): distance along the trajectory in meters + + Returns: + float: curvature + """ + # Interpolate the curvature + index = self.get_index_from_distance(distance) + + return self.get_curvature_by_index(index) + + def get_dynamics_by_index(self, index, wheelbase): + """Gets the dynamics at a given index along the trajectory, + interpolating if necessary. Convenience function that returns + all of the dynamics at once rather than requiring multiple + calls to other helper functions. In this way, we can reuse calls + to the interpolator, improving performance. + + Args: + index (float): index along the trajectory + + Returns: + tuple: (x, y, theta, steering_angle) + """ + # Interpolate the dynamics + x, y = self.interpolation(index).reshape((-1, 2)).T + dxdt, dydt = self.interpolation(index, nu=1).reshape((-1, 2)).T + ddxdtt, ddydtt = self.interpolation(index, nu=2).reshape((-1, 2)).T + + curvature = (dxdt * ddydtt - dydt * ddxdtt) / ( + (dxdt**2 + dydt**2) ** (3 / 2) + ) + theta = np.arctan2(dydt, dxdt) + + return np.stack((x, y, theta, np.arctan(wheelbase * curvature)), axis=-1) + + def get_unit_normal_by_index(self, index): + """Gets the index of the closest point on the trajectory to the given point + + Args: + index: Nx1 numpy array: indexes along trajectory + Returns: + Nx2 numpy array: unit normal of the trajectory at index + """ + + derivative = self.interpolation(index, nu=1) + unit_derivative = derivative / np.linalg.norm(derivative, axis=1)[:, None] + + # (x, y), rotated by 90 deg ccw = (-y, x) + unit_normal = np.vstack((-unit_derivative[:, 1], unit_derivative[:, 0])).T + return unit_normal + + def get_closest_index_on_path( + self, x, y, start_index=0, end_index=None, subsample_resolution=1000 + ): + """Gets the index of the closest point on the trajectory to the given point + + Args: + x (float): x coordinate + y (float): y coordinate + start_index (int, optional): index to start searching from. Defaults to 0. + end_index (int, optional): index to end searching at. Defaults to None (disable). + subsample_resolution: resolution of the resulting interpolation + Returns: + float: index along the trajectory + """ + # If end_index is not specified, use the length of the trajectory + if end_index is None: + end_index = len(self.positions) #sketch, 0-indexing where?? + + # Floor/ceil the start/end indices + start_index = max(0, int(np.floor(start_index))) + end_index = int(np.ceil(end_index)) + + # Calculate the distance from the point to each point on the trajectory + distances = (self.positions[start_index : end_index + 1, 0] - x) ** 2 + ( + self.positions[start_index : end_index + 1, 1] - y + ) ** 2 #Don't need to squareroot as it is a relative distance + + min_ind = np.argmin(distances) + start_index + + start_index = max(0, min_ind - 1) #Protection in case min_ind is too low + end_index = min(len(self.positions), min_ind + 1) #Prtoecting in case min_ind too high + #Theoretically start_index and end_index are just two apart + + # Now interpolate at a higher resolution to get a more accurate result + r_interp = self.interpolation( + np.linspace(start_index, end_index, subsample_resolution + 1)) + x_interp, y_interp = r_interp[:, 0], r_interp[:, 1] #x_interp, y_interp are numpy column vectors + + distances = (x_interp - x) ** 2 + (y_interp - y) ** 2 #Again distances are relative + + # Return the rational index of the closest point + return ( + np.argmin(distances) / subsample_resolution * (end_index - start_index) + + start_index + ) + + def pack(self, x, y) -> TrajectoryMsg: + traj = TrajectoryMsg() + traj.easting = self.positions[:, 0] + traj.northing = self.positions[:, 1] + traj.time = time.time() + traj.cur_idx = self.get_closest_index_on_path(x,y) + return traj + + def unpack(trajMsg : TrajectoryMsg): + pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0) + cur_idx = trajMsg.cur_idx + return Trajectory(positions=pos), cur_idx + From fdf7a3afe5e8f85f6223f8826eb6f3810c5551a5 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 29 Dec 2024 12:54:36 -0800 Subject: [PATCH 07/14] finished for now --- rb_ws/src/buggy/buggy/controller/controller_node.py | 2 +- rb_ws/src/buggy/buggy/controller/controller_superclass.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index 051f4d0a..bcc46529 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -10,7 +10,7 @@ from nav_msgs.msg import Odometry sys.path.append("/rb_ws/src/buggy/buggy") -from util.trajectory_old import Trajectory +from util.trajectory import Trajectory from controller.stanley_controller import StanleyController class Controller(Node): diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py index 6944ccd9..ce152c07 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py +++ b/rb_ws/src/buggy/buggy/controller/controller_superclass.py @@ -6,7 +6,7 @@ sys.path.append("/rb_ws/src/buggy/buggy") -from rb_ws.src.buggy.buggy.util.trajectory_old import Trajectory +from util.trajectory import Trajectory class Controller(ABC): """ From 19a1815cde6bbf39c36d5ecfa0c6b48e874192d2 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Mon, 30 Dec 2024 21:21:33 -0800 Subject: [PATCH 08/14] debugging, nearly done --- .../buggy/buggy/controller/controller_node.py | 37 +++++++++++-------- .../buggy/controller/stanley_controller.py | 8 ++-- rb_ws/src/buggy/buggy/simulator/engine.py | 17 +++++---- rb_ws/src/buggy/buggy/util/pose.py | 9 ++--- rb_ws/src/buggy/buggy/util/trajectory.py | 8 ++-- rb_ws/src/buggy/buggy/{ => util}/util.py | 0 rb_ws/src/buggy/launch/sim_2d_single.xml | 12 ++++-- rb_ws/src/buggy/setup.py | 2 +- 8 files changed, 52 insertions(+), 41 deletions(-) rename rb_ws/src/buggy/buggy/{ => util}/util.py (100%) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index bcc46529..ee1bb2fe 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -24,22 +24,23 @@ def __init__(self): """ super().__init__('controller') + self.get_logger().info('INITIALIZED.') #Parameters self.declare_parameter("dist", 0.0) #Starting Distance along path - start_dist = self.get_parameter("dist") + start_dist = self.get_parameter("dist").value - self.declare_parameter("traj_name", "buggycourse_path.json") - traj_name = self.get_parameter("traj_name") + self.declare_parameter("traj_name", "buggycourse_safe.json") + traj_name = self.get_parameter("traj_name").value self.cur_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good start_index = self.cur_traj.get_index_from_distance(start_dist) self.declare_parameter("controller_name", "stanley") - match self.get_parameter("controller_name"): + match self.get_parameter("controller_name").value: case "stanley": - self.controller = StanleyController(start_index = start_index, buggy_name = self.get_namespace(), node=self) #IMPORT STANLEY + self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY case _: self.get_logger().error("Invalid Controller Name!") raise Exception("Invalid Controller Argument") @@ -49,14 +50,14 @@ def __init__(self): "debug/init_safety_check", 1 ) self.steer_publisher = self.create_publisher( - Float64, "/buggy/steering", 1 + Float64, "input/steering", 1 ) self.heading_publisher = self.create_publisher( - Float32, "/auton/debug/heading", 1 + Float32, "auton/debug/heading", 1 ) # Subscribers - self.odom_subscriber = self.create_subscription(Odometry, 'self/buggy/state', self.odom_listener, 1) + self.odom_subscriber = self.create_subscription(Odometry, 'self/state', self.odom_listener, 1) self.traj_subscriber = self.create_subscription(Odometry, 'self/cur_traj', self.traj_listener, 1) self.lock = threading.Lock() @@ -106,7 +107,9 @@ def init_check(self): closest_heading = self.cur_traj.get_heading_by_index(self.cur_traj.get_closest_index_on_path(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y)) self.get_logger().info("current heading: " + str(np.rad2deg(current_heading))) - self.heading_publisher.publish(Float32(np.rad2deg(current_heading))) + msg = Float32() + msg.data = np.rad2deg(current_heading) + self.heading_publisher.publish(msg) #Converting headings from [-pi, pi] to [0, 2pi] if (current_heading < 0): @@ -123,30 +126,32 @@ def init_check(self): def loop(self): if not self.passed_init: self.passed_init = self.init_check() - self.init_check_publisher.publish(self.passed_init) + msg = Bool() + msg.data = self.passed_init + self.init_check_publisher.publish(msg) if self.passed_init: - self.get_logger.info("Passed Initialization Check") + self.get_logger().info("Passed Initialization Check") else: return - self.heading_publisher.publish(Float32(np.rad2deg(self.odom.pose.pose.orientation.z))) + self.heading_publisher.publish(Float32(data=np.rad2deg(self.odom.pose.pose.orientation.z))) steering_angle = self.controller.compute_control(self.odom, self.cur_traj) steering_angle_deg = np.rad2deg(steering_angle) - self.steer_publisher.publish(Float64(steering_angle_deg)) + self.steer_publisher.publish(Float64(data=float(steering_angle_deg))) def main(args=None): rclpy.init(args=args) - watchdog = Controller() + controller = Controller() - rclpy.spin(watchdog) + rclpy.spin(controller) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) - watchdog.destroy_node() + controller.destroy_node() rclpy.shutdown() diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py index d8be46b6..932538c2 100644 --- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py +++ b/rb_ws/src/buggy/buggy/controller/stanley_controller.py @@ -6,7 +6,7 @@ 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 util.trajectory import Trajectory from controller.controller_superclass import Controller from util.pose import Pose @@ -30,10 +30,10 @@ class StanleyController(Controller): 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 + NavSatFix, "auton/debug/reference_navsat", 1 ) self.debug_error_publisher = self.node.create_publisher( - "/auton/debug/error", ROSPose, queue_size=1 + ROSPose, "auton/debug/error", 1 ) def compute_control(self, state_msg : Odometry, trajectory : Trajectory): @@ -115,7 +115,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): #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) + current_pose = Pose(current_rospose.position.x, current_rospose.position.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 diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index efaf6dbe..e03c2c65 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -10,7 +10,7 @@ import numpy as np import utm sys.path.append("/rb_ws/src/buggy/buggy") -from util import Constants +from util.util import Constants class Simulator(Node): @@ -63,12 +63,12 @@ def __init__(self): self.timer = self.create_timer(timer_period, self.loop) self.steering_subscriber = self.create_subscription( - Float64, "buggy/input/steering", self.update_steering_angle, 1 + Float64, "input/steering", self.update_steering_angle, 1 ) # To read from velocity self.velocity_subscriber = self.create_subscription( - Float64, "velocity", self.update_velocity, 1 + Float64, "sim/velocity", self.update_velocity, 1 ) # for X11 matplotlib (direction included) @@ -76,10 +76,10 @@ def __init__(self): # simulate the INS's outputs (noise included) # this is published as a BuggyState (UTM and radians) - self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1) + self.pose_publisher = self.create_publisher(Odometry, "self/state", 1) self.navsatfix_noisy_publisher = self.create_publisher( - NavSatFix, "state/pose_navsat_noisy", 1 + NavSatFix, "self/pose_navsat_noisy", 1 ) def update_steering_angle(self, data: Float64): @@ -155,13 +155,12 @@ def publish(self): odom.header.stamp = time_stamp odom_pose = Pose() - east, north = utm.from_latlon(lat_noisy, long_noisy) + east, north, _, _ = utm.from_latlon(lat_noisy, long_noisy) odom_pose.position.x = float(east) odom_pose.position.y = float(north) odom_pose.position.z = float(260) - odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2) - odom_pose.orientation.w = np.cos(np.deg2rad(self.heading) / 2) + odom_pose.orientation.z = np.deg2rad(self.heading) # NOTE: autonsystem only cares about magnitude of velocity, so we don't need to split into components odom_twist = Twist() @@ -183,6 +182,8 @@ def main(args=None): rclpy.init(args=args) sim = Simulator() rclpy.spin(sim) + + sim.destroy_node() rclpy.shutdown() if __name__ == "__main__": diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/buggy/util/pose.py index 985755a2..b39e03dd 100644 --- a/rb_ws/src/buggy/buggy/util/pose.py +++ b/rb_ws/src/buggy/buggy/util/pose.py @@ -1,8 +1,7 @@ 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 +from scipy.spatial.transform import Rotation class Pose: @@ -28,20 +27,20 @@ def rospose_to_pose(rospose: ROSPose): Returns: Pose: converted pose """ - (_, _, yaw) = euler_from_quaternion( + (_, _, yaw) = Rotation.from_quat( [ rospose.orientation.x, rospose.orientation.y, rospose.orientation.z, rospose.orientation.w, ] - ) + ).as_euler('xyz') p = Pose(rospose.position.x, rospose.position.y, yaw) return p def heading_to_quaternion(heading): - q = quaternion_from_euler(0, 0, heading) + q = Rotation.from_euler([0, 0, heading]).as_quat() return q def __init__(self, x, y, theta): diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py index a7da7dfb..85b996f2 100644 --- a/rb_ws/src/buggy/buggy/util/trajectory.py +++ b/rb_ws/src/buggy/buggy/util/trajectory.py @@ -3,7 +3,7 @@ import uuid import matplotlib.pyplot as plt -from buggy.msg import TrajectoryMsg +# from buggy.msg import TrajectoryMsg import numpy as np from scipy.interpolate import Akima1DInterpolator, CubicSpline @@ -58,7 +58,7 @@ def __init__(self, json_filepath=None, positions = None, interpolator="CubicSpli lon = waypoint["lon"] # Convert to world coordinates - x, y = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized + x, y, _, _ = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized positions.append([x, y]) positions = np.array(positions) @@ -350,7 +350,7 @@ def get_closest_index_on_path( + start_index ) - def pack(self, x, y) -> TrajectoryMsg: + """ def pack(self, x, y) -> TrajectoryMsg: traj = TrajectoryMsg() traj.easting = self.positions[:, 0] traj.northing = self.positions[:, 1] @@ -361,5 +361,5 @@ def pack(self, x, y) -> TrajectoryMsg: def unpack(trajMsg : TrajectoryMsg): pos = np.array([trajMsg.easting, trajMsg.northing]).transpose(1, 0) cur_idx = trajMsg.cur_idx - return Trajectory(positions=pos), cur_idx + return Trajectory(positions=pos), cur_idx """ diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util/util.py similarity index 100% rename from rb_ws/src/buggy/buggy/util.py rename to rb_ws/src/buggy/buggy/util/util.py diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index 03982798..bc455d62 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -1,17 +1,23 @@ - + + + + + + + + + - - diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index a44f0053..a8a022ec 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -26,7 +26,7 @@ 'console_scripts': [ 'hello_world = buggy.hello_world:main', 'sim_single = buggy.simulator.engine:main', - 'controller = buggy.controller.controller_node:main' + 'controller = buggy.controller.controller_node:main', 'buggy_state_converter = buggy.buggy_state_converter:main', 'watchdog = buggy.watchdog.watchdog_node:main', ], From cac2ec74e9a468eccc27d86533c51dda4ebf512d Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 12:13:38 -0800 Subject: [PATCH 09/14] pylint pt. 1 --- .gitignore | 4 +++- .../buggy/controller/controller_superclass.py | 1 - .../buggy/buggy/controller/stanley_controller.py | 14 +++++++------- rb_ws/src/buggy/buggy/util/trajectory.py | 3 --- rb_ws/src/buggy/launch/sim_2d_single.xml | 4 ++-- 5 files changed, 12 insertions(+), 14 deletions(-) diff --git a/.gitignore b/.gitignore index 60a64061..e8edffcc 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,6 @@ rb_ws/bags docker-compose.yml~ *TEMP_DO_NOT_EDIT.txt rb_ws/src/buggy/bags/* -*.bag \ No newline at end of file +*.bag +.vision*/* +vision/data*/* \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/buggy/controller/controller_superclass.py index ce152c07..f960de75 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py +++ b/rb_ws/src/buggy/buggy/controller/controller_superclass.py @@ -1,7 +1,6 @@ from abc import ABC, abstractmethod import sys -from sensor_msgs.msg import NavSatFix from nav_msgs.msg import Odometry diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/buggy/controller/stanley_controller.py index 932538c2..34076535 100644 --- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py +++ b/rb_ws/src/buggy/buggy/controller/stanley_controller.py @@ -35,7 +35,7 @@ def __init__(self, start_index, namespace, node): self.debug_error_publisher = self.node.create_publisher( ROSPose, "auton/debug/error", 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 @@ -50,7 +50,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): 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 @@ -58,7 +58,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): 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) @@ -76,12 +76,12 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): 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 @@ -103,7 +103,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): 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) @@ -118,7 +118,7 @@ def compute_control(self, state_msg : Odometry, trajectory : Trajectory): current_pose = Pose(current_rospose.position.x, current_rospose.position.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] diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py index 85b996f2..cf9f662c 100644 --- a/rb_ws/src/buggy/buggy/util/trajectory.py +++ b/rb_ws/src/buggy/buggy/util/trajectory.py @@ -1,7 +1,4 @@ import json -import time -import uuid -import matplotlib.pyplot as plt # from buggy.msg import TrajectoryMsg diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index bc455d62..2c62b1b1 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -13,11 +13,11 @@ - + From 354330b28427f03d9dc7420062edef5bde3da257 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 12:14:49 -0800 Subject: [PATCH 10/14] pylint?? --- rb_ws/src/buggy/buggy/controller/controller_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index ee1bb2fe..af60dc05 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -38,7 +38,7 @@ def __init__(self): start_index = self.cur_traj.get_index_from_distance(start_dist) self.declare_parameter("controller_name", "stanley") - match self.get_parameter("controller_name").value: + match (self.get_parameter("controller_name").value): case "stanley": self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY case _: From fa64058ccd2df4838ff921d697041f2f9fce5dd0 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 12:18:41 -0800 Subject: [PATCH 11/14] fixed for python 3.8 --- .../src/buggy/buggy/controller/controller_node.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index af60dc05..132efce9 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -38,12 +38,14 @@ def __init__(self): start_index = self.cur_traj.get_index_from_distance(start_dist) self.declare_parameter("controller_name", "stanley") - match (self.get_parameter("controller_name").value): - case "stanley": - self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY - case _: - self.get_logger().error("Invalid Controller Name!") - raise Exception("Invalid Controller Argument") + + controller_name = self.get_parameter("controller_name").value + print(controller_name.lower) + if (controller_name.lower() == "stanley"): + self.controller = StanleyController(start_index = start_index, namespace = self.get_namespace(), node=self) #IMPORT STANLEY + else: + self.get_logger().error("Invalid Controller Name: " + controller_name.lower()) + raise Exception("Invalid Controller Argument") # Publishers self.init_check_publisher = self.create_publisher(Bool, From fcfefca10bbe739582e713036c6fd65834018af9 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sun, 5 Jan 2025 12:20:18 -0800 Subject: [PATCH 12/14] removed trailing whitespace --- .../buggy/buggy/controller/controller_node.py | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index 132efce9..700bef06 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -25,7 +25,7 @@ def __init__(self): """ super().__init__('controller') self.get_logger().info('INITIALIZED.') - + #Parameters self.declare_parameter("dist", 0.0) #Starting Distance along path @@ -38,7 +38,7 @@ def __init__(self): start_index = self.cur_traj.get_index_from_distance(start_dist) self.declare_parameter("controller_name", "stanley") - + controller_name = self.get_parameter("controller_name").value print(controller_name.lower) if (controller_name.lower() == "stanley"): @@ -69,7 +69,7 @@ def __init__(self): timer_period = 0.01 # seconds (100 Hz) self.timer = self.create_timer(timer_period, self.loop) - + def odom_listener(self, msg : Odometry): ''' This is the subscriber that updates the buggies state for navigation @@ -77,7 +77,7 @@ def odom_listener(self, msg : Odometry): ''' with self.lock: self.odom = msg - + def traj_listener(self, msg): #TYPE UNKOWN AS OF NOW?? CUSTOM TYPE WHEN #TODO: FIGURE OUT BEFORE MERGE ''' This is the subscriber that updates the buggies trajectory for navigation @@ -99,11 +99,11 @@ def init_check(self): if (self.odom == None): self.get_logger().warn("WARNING: no available position estimate") return False - + elif (self.odom.pose.covariance[0] ** 2 + self.odom.pose.covariance[7] ** 2 > 1**2): self.get_logger().warn("checking position estimate certainty") return False - + #Originally under a lock, doesn't seem necessary? current_heading = self.odom.pose.pose.orientation.z closest_heading = self.cur_traj.get_heading_by_index(self.cur_traj.get_closest_index_on_path(self.odom.pose.pose.position.x, self.odom.pose.pose.position.y)) @@ -118,11 +118,11 @@ def init_check(self): current_heading = 2 * np.pi + current_heading if (closest_heading < 0): closest_heading = 2 * np.pi + closest_heading - + if (abs(current_heading - closest_heading) >= np.pi/2): self.get_logger().warn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: " + str(np.rad2deg(current_heading))) return False - + return True def loop(self): @@ -135,13 +135,13 @@ def loop(self): self.get_logger().info("Passed Initialization Check") else: return - + self.heading_publisher.publish(Float32(data=np.rad2deg(self.odom.pose.pose.orientation.z))) steering_angle = self.controller.compute_control(self.odom, self.cur_traj) steering_angle_deg = np.rad2deg(steering_angle) self.steer_publisher.publish(Float64(data=float(steering_angle_deg))) - + def main(args=None): rclpy.init(args=args) From d6eda8ad731a8f70bd5db81c5f6e637174dd69d6 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Mon, 6 Jan 2025 19:13:37 -0800 Subject: [PATCH 13/14] review fixes --- .../src/buggy/buggy/controller/controller_node.py | 2 +- rb_ws/src/buggy/buggy/simulator/engine.py | 2 +- .../buggy/buggy/util/{util.py => constants.py} | 0 rb_ws/src/buggy/buggy/util/pose.py | 15 --------------- rb_ws/src/buggy/buggy/util/trajectory.py | 2 +- 5 files changed, 3 insertions(+), 18 deletions(-) rename rb_ws/src/buggy/buggy/util/{util.py => constants.py} (100%) diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/buggy/controller/controller_node.py index 700bef06..9d13593c 100644 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/buggy/controller/controller_node.py @@ -78,7 +78,7 @@ def odom_listener(self, msg : Odometry): with self.lock: self.odom = msg - def traj_listener(self, msg): #TYPE UNKOWN AS OF NOW?? CUSTOM TYPE WHEN #TODO: FIGURE OUT BEFORE MERGE + def traj_listener(self, msg): ''' This is the subscriber that updates the buggies trajectory for navigation ''' diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index e03c2c65..b3cb7746 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -10,7 +10,7 @@ import numpy as np import utm sys.path.append("/rb_ws/src/buggy/buggy") -from util.util import Constants +from rb_ws.src.buggy.buggy.util.constants import Constants class Simulator(Node): diff --git a/rb_ws/src/buggy/buggy/util/util.py b/rb_ws/src/buggy/buggy/util/constants.py similarity index 100% rename from rb_ws/src/buggy/buggy/util/util.py rename to rb_ws/src/buggy/buggy/util/constants.py diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/buggy/util/pose.py index b39e03dd..f12a4ffd 100644 --- a/rb_ws/src/buggy/buggy/util/pose.py +++ b/rb_ws/src/buggy/buggy/util/pose.py @@ -201,18 +201,3 @@ def __truediv__(self, other): 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) diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/buggy/util/trajectory.py index cf9f662c..72fbcaee 100644 --- a/rb_ws/src/buggy/buggy/util/trajectory.py +++ b/rb_ws/src/buggy/buggy/util/trajectory.py @@ -55,7 +55,7 @@ def __init__(self, json_filepath=None, positions = None, interpolator="CubicSpli lon = waypoint["lon"] # Convert to world coordinates - x, y, _, _ = utm.from_latlon(lat, lon) #TODO: Before Merging, make sure that we can do this nonlocalized + x, y, _, _ = utm.from_latlon(lat, lon) positions.append([x, y]) positions = np.array(positions) From 88a2acd42b96713ffafa4ad02bec6d5b70c52ff3 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 7 Jan 2025 12:34:35 -0800 Subject: [PATCH 14/14] fixed gitignore --- .gitignore | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index e8edffcc..f3fcd37e 100644 --- a/.gitignore +++ b/.gitignore @@ -4,7 +4,7 @@ rb_ws/bags .python-requirements.txt.un~ docker-compose.yml~ *TEMP_DO_NOT_EDIT.txt -rb_ws/src/buggy/bags/* +rb_ws/src/buggy/bags/ *.bag -.vision*/* -vision/data*/* \ No newline at end of file +.vision/ +vision/data/ \ No newline at end of file