From d7ede11de02fbad16301c4a3c0ad80104aae15f0 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 23 Oct 2024 23:37:57 -0400 Subject: [PATCH 01/35] initial commit for buggy_state_converter --- python-requirements.txt | 2 + .../src/buggy/buggy/buggy_state_converter.py | 110 ++++++++++++++++++ rb_ws/src/buggy/setup.py | 3 +- 3 files changed, 114 insertions(+), 1 deletion(-) create mode 100644 rb_ws/src/buggy/buggy/buggy_state_converter.py diff --git a/python-requirements.txt b/python-requirements.txt index 68b0c6f..71c2c61 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -5,6 +5,8 @@ numpy osqp pandas pymap3d +pyproj +rclpy scipy setuptools==58.2.0 trimesh diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py new file mode 100644 index 0000000..3d5c442 --- /dev/null +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +import numpy as np +import pyproj + + +class BuggyStateConverter(Node): + def __init__(self): + super().__init__("buggy_state_converter") + + # ROS2 Subscriber: /ekf/odometry_earth + self.subscription = self.create_subscription( + Odometry, "/ekf/odometry_earth", self.ekf_odometry_callback, 10 + ) + + # ROS2 Publisher: /nav/odometry_earth + self.publisher_ = self.create_publisher(Odometry, "/nav/odometry_earth", 10) + + # Initialize pyproj Transformer for ECEF -> UTM conversion + self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( + "epsg:4978", "epsg:32633", always_xy=True + ) # Change EPSG as required + + def ekf_odometry_callback(self, msg): + """Callback function to process incoming odometry message, convert it, and publish the transformed message""" + + # Create a new Odometry message for the output + converted_msg = Odometry() + converted_msg.header = msg.header + + # ---- 1. Convert ECEF Position to UTM Coordinates ---- + ecef_x = msg.pose.pose.position.x + ecef_y = msg.pose.pose.position.y + ecef_z = msg.pose.pose.position.z + + # Convert ECEF to UTM using pyproj + utm_x, utm_y, _ = self.ecef_to_utm_transformer.transform(ecef_x, ecef_y, ecef_z) + + # Set the converted UTM position in the new message + converted_msg.pose.pose.position.x = utm_x # UTM Easting + converted_msg.pose.pose.position.y = utm_y # UTM Northing + converted_msg.pose.pose.position.z = 0.0 # Ignored in this context + + # ---- 2. Convert Quaternion to Heading (Radians) ---- + qx = msg.pose.pose.orientation.x + qy = msg.pose.pose.orientation.y + qz = msg.pose.pose.orientation.z + qw = msg.pose.pose.orientation.w + + # Convert quaternion to euler angles (roll, pitch, yaw) + yaw = self.quaternion_to_yaw(qx, qy, qz, qw) + + # Store the heading in the x component of the orientation (0 is East) + converted_msg.pose.pose.orientation.x = yaw + + # ---- 3. Copy Covariances (Unchanged) ---- + converted_msg.pose.covariance = msg.pose.covariance + converted_msg.twist.covariance = msg.twist.covariance + + # ---- 4. Convert Linear Velocities ---- + converted_msg.twist.twist.linear.x = ( + msg.twist.twist.linear.x + ) # m/s in x-direction + converted_msg.twist.twist.linear.y = ( + msg.twist.twist.linear.y + ) # m/s in y-direction + converted_msg.twist.twist.linear.z = ( + msg.twist.twist.linear.z + ) # Keep original Z velocity (??) + + # ---- 5. Convert Angular Velocities (rad/s for heading rate of change) ---- + converted_msg.twist.twist.angular.x = ( + msg.twist.twist.angular.z + ) # rad/s for heading change rate + converted_msg.twist.twist.angular.y = ( + msg.twist.twist.angular.y + ) # Keep original Y angular velocity (??) + converted_msg.twist.twist.angular.z = ( + msg.twist.twist.angular.z + ) # Keep original Z angular velocity (??) + + # Publish the converted message + self.publisher_.publish(converted_msg) + + def quaternion_to_yaw(self, qx, qy, qz, qw): + """Convert a quaternion to yaw (heading) in radians.""" + # Extract yaw (z-axis rotation) from quaternion + siny_cosp = 2.0 * (qw * qz + qx * qy) + cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) + return np.arctan2(siny_cosp, cosy_cosp) + + +def main(args=None): + rclpy.init(args=args) + + # Create the BuggyStateConverter node and spin it + state_converter = BuggyStateConverter() + # while rclpy.ok(): <- is this needed? + rclpy.spin(state_converter) + + # Shutdown when done + state_converter.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index 0a9e40b..9446c0f 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -25,7 +25,8 @@ entry_points={ 'console_scripts': [ 'hello_world = buggy.hello_world:main', - 'watchdog = buggy.watchdog.watchdog_node:main' + 'watchdog = buggy.watchdog.watchdog_node:main', + 'buggy_state_converter = buggy.buggy_state_converter:main', ], }, ) \ No newline at end of file From b2c1a378f11ce7544864a508be0e0c66ff9bfc24 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 30 Oct 2024 18:24:25 -0400 Subject: [PATCH 02/35] added NAND-self and SC-other functionality --- .../src/buggy/buggy/buggy_state_converter.py | 141 +++++++++++------- 1 file changed, 91 insertions(+), 50 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 3d5c442..e0e39d4 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -11,23 +11,50 @@ class BuggyStateConverter(Node): def __init__(self): super().__init__("buggy_state_converter") - # ROS2 Subscriber: /ekf/odometry_earth - self.subscription = self.create_subscription( - Odometry, "/ekf/odometry_earth", self.ekf_odometry_callback, 10 + # Determine namespace to configure the subscriber and publisher correctly + namespace = self.get_namespace() + + # Create publisher and subscriber for "self" namespace + self.self_raw_state_subscriber = self.create_subscription( + Odometry, "self/raw_state", self.self_raw_state_callback, 10 ) + self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10) - # ROS2 Publisher: /nav/odometry_earth - self.publisher_ = self.create_publisher(Odometry, "/nav/odometry_earth", 10) + # Check if namespace is "/SC" to add an additional "other" subscriber/publisher + if namespace == "/SC": + self.other_raw_state_subscriber = self.create_subscription( + Odometry, "other/raw_state", self.other_raw_state_callback, 10 + ) + self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10) - # Initialize pyproj Transformer for ECEF -> UTM conversion + # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( "epsg:4978", "epsg:32633", always_xy=True - ) # Change EPSG as required - - def ekf_odometry_callback(self, msg): - """Callback function to process incoming odometry message, convert it, and publish the transformed message""" - - # Create a new Odometry message for the output + ) # Update EPSG if required + + def self_raw_state_callback(self, msg): + """ Callback for processing self/raw_state messages and publishing to self/state """ + namespace = self.get_namespace() + + if namespace == "/SC": + converted_msg = self.convert_SC_state(msg) + elif namespace == "/NAND": + converted_msg = self.convert_NAND_state(msg) + else: + self.get_logger().warn("Namespace not recognized for buggy state conversion.") + return + + # Publish the converted message to self/state + self.self_state_publisher.publish(converted_msg) + + def other_raw_state_callback(self, msg): + """ Callback for processing other/raw_state messages and publishing to other/state """ + # Convert the /SC message and publish to other/state + converted_msg = self.convert_SC_state(msg) + self.other_state_publisher.publish(converted_msg) + + def convert_SC_state(self, msg): + """ Converts /SC namespace raw state to clean state units and structure """ converted_msg = Odometry() converted_msg.header = msg.header @@ -36,58 +63,73 @@ def ekf_odometry_callback(self, msg): ecef_y = msg.pose.pose.position.y ecef_z = msg.pose.pose.position.z - # Convert ECEF to UTM using pyproj + # Convert ECEF to UTM utm_x, utm_y, _ = self.ecef_to_utm_transformer.transform(ecef_x, ecef_y, ecef_z) - - # Set the converted UTM position in the new message converted_msg.pose.pose.position.x = utm_x # UTM Easting converted_msg.pose.pose.position.y = utm_y # UTM Northing - converted_msg.pose.pose.position.z = 0.0 # Ignored in this context + converted_msg.pose.pose.position.z = 0.0 # ignored # ---- 2. Convert Quaternion to Heading (Radians) ---- - qx = msg.pose.pose.orientation.x - qy = msg.pose.pose.orientation.y - qz = msg.pose.pose.orientation.z - qw = msg.pose.pose.orientation.w - - # Convert quaternion to euler angles (roll, pitch, yaw) + qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w yaw = self.quaternion_to_yaw(qx, qy, qz, qw) - - # Store the heading in the x component of the orientation (0 is East) converted_msg.pose.pose.orientation.x = yaw + # ignored: + # converted_msg.pose.pose.orientation.y = qy + # converted_msg.pose.pose.orientation.z = qz + # converted_msg.pose.pose.orientation.w = qw + + # ---- 3. Copy Covariances (Unchanged) ---- + converted_msg.pose.covariance = msg.pose.covariance + converted_msg.twist.covariance = msg.twist.covariance + + # ---- 4. Copy Linear Velocities ---- + converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction + converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in x-direction + converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # Keep original Z velocity (??) + + # ---- 5. Copy Angular Velocities ---- + converted_msg.twist.twist.angular.x = msg.twist.twist.angular.z # rad/s for heading change rate (using yaw from twist.angular) + converted_msg.twist.twist.angular.y = 0.0 # ignored + converted_msg.twist.twist.angular.z = 0.0 # ignored + + return converted_msg + + def convert_NAND_state(self, msg): + """ Converts /NAND namespace raw state to clean state units and structure """ + converted_msg = Odometry() + converted_msg.header = msg.header + + # ---- 1. Directly use UTM Coordinates ---- + converted_msg.pose.pose.position.x = msg.pose.pose.position.x # UTM Easting + converted_msg.pose.pose.position.y = msg.pose.pose.position.y # UTM Northing + converted_msg.pose.pose.position.z = 0.0 # ignored + + # ---- 2. Orientation in Radians with 0 at East ---- + converted_msg.pose.pose.orientation.x = msg.pose.pose.orientation.x + # ignored: + # converted_msg.pose.pose.orientation.y = msg.pose.pose.orientation.y + # converted_msg.pose.pose.orientation.z = msg.pose.pose.orientation.z + # converted_msg.pose.pose.orientation.w = msg.pose.pose.orientation.w # ---- 3. Copy Covariances (Unchanged) ---- converted_msg.pose.covariance = msg.pose.covariance converted_msg.twist.covariance = msg.twist.covariance - # ---- 4. Convert Linear Velocities ---- - converted_msg.twist.twist.linear.x = ( - msg.twist.twist.linear.x - ) # m/s in x-direction - converted_msg.twist.twist.linear.y = ( - msg.twist.twist.linear.y - ) # m/s in y-direction - converted_msg.twist.twist.linear.z = ( - msg.twist.twist.linear.z - ) # Keep original Z velocity (??) - - # ---- 5. Convert Angular Velocities (rad/s for heading rate of change) ---- - converted_msg.twist.twist.angular.x = ( - msg.twist.twist.angular.z - ) # rad/s for heading change rate - converted_msg.twist.twist.angular.y = ( - msg.twist.twist.angular.y - ) # Keep original Y angular velocity (??) - converted_msg.twist.twist.angular.z = ( - msg.twist.twist.angular.z - ) # Keep original Z angular velocity (??) - - # Publish the converted message - self.publisher_.publish(converted_msg) + # ---- 4. Copy Linear Velocities ---- + # CHECK: ROS serial translator node must change scalar speed to velocity x/y components before pushing to raw_state + converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction + converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in y-direction + converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # Keep original Z velocity (??) + + # ---- 5. Copy Angular Velocities ---- + converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x + converted_msg.twist.twist.angular.y = 0.0 + converted_msg.twist.twist.angular.z = 0.0 + + return converted_msg def quaternion_to_yaw(self, qx, qy, qz, qw): """Convert a quaternion to yaw (heading) in radians.""" - # Extract yaw (z-axis rotation) from quaternion siny_cosp = 2.0 * (qw * qz + qx * qy) cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) return np.arctan2(siny_cosp, cosy_cosp) @@ -98,7 +140,6 @@ def main(args=None): # Create the BuggyStateConverter node and spin it state_converter = BuggyStateConverter() - # while rclpy.ok(): <- is this needed? rclpy.spin(state_converter) # Shutdown when done From c3790b1cb00cd6856ee3d2f904164149f9f4faba Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 30 Oct 2024 18:32:48 -0400 Subject: [PATCH 03/35] fix transformer epsg code --- rb_ws/src/buggy/buggy/buggy_state_converter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index e0e39d4..cb8fc1c 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -29,8 +29,8 @@ def __init__(self): # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( - "epsg:4978", "epsg:32633", always_xy=True - ) # Update EPSG if required + "epsg:4978", "epsg:32617", always_xy=True + ) # Check UTM EPSG code, using EPSG:32617 for UTM Zone 17N def self_raw_state_callback(self, msg): """ Callback for processing self/raw_state messages and publishing to self/state """ From d2d311e930d3c8e01c20e9229c5ec08e6f3d5390 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 6 Nov 2024 12:48:00 -0500 Subject: [PATCH 04/35] changed published ros msg fields --- .../src/buggy/buggy/buggy_state_converter.py | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index cb8fc1c..f3dd023 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -74,13 +74,14 @@ def convert_SC_state(self, msg): yaw = self.quaternion_to_yaw(qx, qy, qz, qw) converted_msg.pose.pose.orientation.x = yaw # ignored: - # converted_msg.pose.pose.orientation.y = qy - # converted_msg.pose.pose.orientation.z = qz - # converted_msg.pose.pose.orientation.w = qw + converted_msg.pose.pose.orientation.y = 0.0 + converted_msg.pose.pose.orientation.z = 0.0 + converted_msg.pose.pose.orientation.w = 0.0 # ---- 3. Copy Covariances (Unchanged) ---- - converted_msg.pose.covariance = msg.pose.covariance - converted_msg.twist.covariance = msg.twist.covariance + # TODO: check whether covariances should be copied over or not sent at all + # converted_msg.pose.covariance = msg.pose.covariance + # converted_msg.twist.covariance = msg.twist.covariance # ---- 4. Copy Linear Velocities ---- converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction @@ -107,16 +108,17 @@ def convert_NAND_state(self, msg): # ---- 2. Orientation in Radians with 0 at East ---- converted_msg.pose.pose.orientation.x = msg.pose.pose.orientation.x # ignored: - # converted_msg.pose.pose.orientation.y = msg.pose.pose.orientation.y - # converted_msg.pose.pose.orientation.z = msg.pose.pose.orientation.z - # converted_msg.pose.pose.orientation.w = msg.pose.pose.orientation.w + converted_msg.pose.pose.orientation.y = 0.0 + converted_msg.pose.pose.orientation.z = 0.0 + converted_msg.pose.pose.orientation.w = 0.0 # ---- 3. Copy Covariances (Unchanged) ---- - converted_msg.pose.covariance = msg.pose.covariance - converted_msg.twist.covariance = msg.twist.covariance + # TODO: check whether covariances should be copied over or not sent at all + # converted_msg.pose.covariance = msg.pose.covariance + # converted_msg.twist.covariance = msg.twist.covariance # ---- 4. Copy Linear Velocities ---- - # CHECK: ROS serial translator node must change scalar speed to velocity x/y components before pushing to raw_state + # TODO: check that ROS serial translator node changes scalar speed to velocity x/y components before pushing to raw_state converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in y-direction converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # Keep original Z velocity (??) @@ -131,7 +133,8 @@ def convert_NAND_state(self, msg): def quaternion_to_yaw(self, qx, qy, qz, qw): """Convert a quaternion to yaw (heading) in radians.""" siny_cosp = 2.0 * (qw * qz + qx * qy) - cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) + cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) # assumes quaternion is normalized, should be true in ROS, + # else use (qw * qw + qx * qx - qy * qy - qz * qz) return np.arctan2(siny_cosp, cosy_cosp) From f187fab3e9d554181369b9df57cc7f64610e4254 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 6 Nov 2024 16:42:10 -0500 Subject: [PATCH 05/35] changed quaternion to yaw conversion --- python-requirements.txt | 1 + rb_ws/src/buggy/buggy/buggy_state_converter.py | 17 ++++++++++++++--- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/python-requirements.txt b/python-requirements.txt index 71c2c61..12d4591 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -9,6 +9,7 @@ pyproj rclpy scipy setuptools==58.2.0 +tf_transformations trimesh utm keyboard diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index f3dd023..4fc2f1d 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -5,7 +5,7 @@ from nav_msgs.msg import Odometry import numpy as np import pyproj - +from tf_transformations import euler_from_quaternion class BuggyStateConverter(Node): def __init__(self): @@ -70,9 +70,19 @@ def convert_SC_state(self, msg): converted_msg.pose.pose.position.z = 0.0 # ignored # ---- 2. Convert Quaternion to Heading (Radians) ---- + qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w - yaw = self.quaternion_to_yaw(qx, qy, qz, qw) - converted_msg.pose.pose.orientation.x = yaw + + # replaced with tf_transformations library function + # yaw = self.quaternion_to_yaw(qx, qy, qz, qw) + # converted_msg.pose.pose.orientation.x = yaw + + # Use euler_from_quaternion to get roll, pitch, yaw + _, _, yaw = euler_from_quaternion([qx, qy, qz, qw]) + yaw_aligned_east = yaw - np.pi / 2 # TODO: check if 0 is East + # Store the heading in the x component of the orientation + converted_msg.pose.pose.orientation.x = yaw_aligned_east + # ignored: converted_msg.pose.pose.orientation.y = 0.0 converted_msg.pose.pose.orientation.z = 0.0 @@ -130,6 +140,7 @@ def convert_NAND_state(self, msg): return converted_msg + # replaced custom function with tf_transformations library function def quaternion_to_yaw(self, qx, qy, qz, qw): """Convert a quaternion to yaw (heading) in radians.""" siny_cosp = 2.0 * (qw * qz + qx * qy) From 7f928455be8ccd33a8bc384ce0d2f9b81d40604b Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 20 Nov 2024 18:01:04 -0500 Subject: [PATCH 06/35] changed message structure handling --- .../src/buggy/buggy/buggy_state_converter.py | 144 +++++++++++------- 1 file changed, 89 insertions(+), 55 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 4fc2f1d..b45f94b 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -20,8 +20,8 @@ def __init__(self): ) self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10) - # Check if namespace is "/SC" to add an additional "other" subscriber/publisher - if namespace == "/SC": + # Check if namespace is "SC" to add an "other" subscriber/publisher + if namespace == "SC": self.other_raw_state_subscriber = self.create_subscription( Odometry, "other/raw_state", self.other_raw_state_callback, 10 ) @@ -30,15 +30,15 @@ def __init__(self): # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( "epsg:4978", "epsg:32617", always_xy=True - ) # Check UTM EPSG code, using EPSG:32617 for UTM Zone 17N + ) # TODO: Confirm UTM EPSG code, using EPSG:32617 for UTM Zone 17N def self_raw_state_callback(self, msg): """ Callback for processing self/raw_state messages and publishing to self/state """ namespace = self.get_namespace() - if namespace == "/SC": + if namespace == "SC": converted_msg = self.convert_SC_state(msg) - elif namespace == "/NAND": + elif namespace == "NAND": converted_msg = self.convert_NAND_state(msg) else: self.get_logger().warn("Namespace not recognized for buggy state conversion.") @@ -49,12 +49,18 @@ def self_raw_state_callback(self, msg): def other_raw_state_callback(self, msg): """ Callback for processing other/raw_state messages and publishing to other/state """ - # Convert the /SC message and publish to other/state - converted_msg = self.convert_SC_state(msg) + # Convert the SC message and publish to other/state + converted_msg = self.convert_NAND_other_state(msg) self.other_state_publisher.publish(converted_msg) def convert_SC_state(self, msg): - """ Converts /SC namespace raw state to clean state units and structure """ + """ + Converts self/raw_state in SC namespace to clean state units and structure + + Takes in ROS message in nav_msgs/Odometry format + Assumes that the SC namespace is using ECEF coordinates and quaternion orientation + """ + converted_msg = Odometry() converted_msg.header = msg.header @@ -64,89 +70,117 @@ def convert_SC_state(self, msg): ecef_z = msg.pose.pose.position.z # Convert ECEF to UTM - utm_x, utm_y, _ = self.ecef_to_utm_transformer.transform(ecef_x, ecef_y, ecef_z) + utm_x, utm_y, utm_z = self.ecef_to_utm_transformer.transform(ecef_x, ecef_y, ecef_z) converted_msg.pose.pose.position.x = utm_x # UTM Easting converted_msg.pose.pose.position.y = utm_y # UTM Northing - converted_msg.pose.pose.position.z = 0.0 # ignored + converted_msg.pose.pose.position.z = utm_z # UTM Altitude # ---- 2. Convert Quaternion to Heading (Radians) ---- - qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w - # replaced with tf_transformations library function - # yaw = self.quaternion_to_yaw(qx, qy, qz, qw) - # converted_msg.pose.pose.orientation.x = yaw - # Use euler_from_quaternion to get roll, pitch, yaw - _, _, yaw = euler_from_quaternion([qx, qy, qz, qw]) - yaw_aligned_east = yaw - np.pi / 2 # TODO: check if 0 is East - # Store the heading in the x component of the orientation - converted_msg.pose.pose.orientation.x = yaw_aligned_east + roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw]) - # ignored: - converted_msg.pose.pose.orientation.y = 0.0 - converted_msg.pose.pose.orientation.z = 0.0 - converted_msg.pose.pose.orientation.w = 0.0 + # Store the heading in the x component of the orientation + converted_msg.pose.pose.orientation.x = roll + converted_msg.pose.pose.orientation.y = pitch + converted_msg.pose.pose.orientation.z = yaw + converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles # ---- 3. Copy Covariances (Unchanged) ---- - # TODO: check whether covariances should be copied over or not sent at all - # converted_msg.pose.covariance = msg.pose.covariance - # converted_msg.twist.covariance = msg.twist.covariance + converted_msg.pose.covariance = msg.pose.covariance + converted_msg.twist.covariance = msg.twist.covariance # ---- 4. Copy Linear Velocities ---- converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in x-direction - converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # Keep original Z velocity (??) + converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # keep original Z velocity # ---- 5. Copy Angular Velocities ---- - converted_msg.twist.twist.angular.x = msg.twist.twist.angular.z # rad/s for heading change rate (using yaw from twist.angular) - converted_msg.twist.twist.angular.y = 0.0 # ignored - converted_msg.twist.twist.angular.z = 0.0 # ignored + converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x # copying over + converted_msg.twist.twist.angular.y = msg.twist.twist.angular.y # copying over + converted_msg.twist.twist.angular.z = msg.twist.twist.angular.z # rad/s, heading change rate return converted_msg def convert_NAND_state(self, msg): - """ Converts /NAND namespace raw state to clean state units and structure """ + """ + Converts self/raw_state in NAND namespace to clean state units and structure + Takes in ROS message in nav_msgs/Odometry format + """ + converted_msg = Odometry() converted_msg.header = msg.header # ---- 1. Directly use UTM Coordinates ---- converted_msg.pose.pose.position.x = msg.pose.pose.position.x # UTM Easting converted_msg.pose.pose.position.y = msg.pose.pose.position.y # UTM Northing - converted_msg.pose.pose.position.z = 0.0 # ignored + converted_msg.pose.pose.position.z = msg.pose.pose.position.z # UTM Altitude - # ---- 2. Orientation in Radians with 0 at East ---- + # ---- 2. Orientation in Radians ---- converted_msg.pose.pose.orientation.x = msg.pose.pose.orientation.x - # ignored: - converted_msg.pose.pose.orientation.y = 0.0 - converted_msg.pose.pose.orientation.z = 0.0 - converted_msg.pose.pose.orientation.w = 0.0 + converted_msg.pose.pose.orientation.y = msg.pose.pose.orientation.y + converted_msg.pose.pose.orientation.z = msg.pose.pose.orientation.z + converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles # ---- 3. Copy Covariances (Unchanged) ---- - # TODO: check whether covariances should be copied over or not sent at all - # converted_msg.pose.covariance = msg.pose.covariance - # converted_msg.twist.covariance = msg.twist.covariance + converted_msg.pose.covariance = msg.pose.covariance + converted_msg.twist.covariance = msg.twist.covariance - # ---- 4. Copy Linear Velocities ---- - # TODO: check that ROS serial translator node changes scalar speed to velocity x/y components before pushing to raw_state - converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction - converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in y-direction - converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # Keep original Z velocity (??) + # ---- 4. Linear Velocities in m/s ---- + # TODO: Check if scalar velocity is coming in from msg.twist.twist.linear.x + # Convert scalar speed to velocity x/y components using heading (orientation.z) + speed = msg.twist.twist.linear.x # m/s scalar velocity + heading = msg.pose.pose.orientation.z # heading in radians + + # Calculate velocity components + converted_msg.twist.twist.linear.x = speed * np.cos(heading) # m/s in x-direction + converted_msg.twist.twist.linear.y = speed * np.sin(heading) # m/s in y-direction + converted_msg.twist.twist.linear.z = 0.0 # ---- 5. Copy Angular Velocities ---- - converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x - converted_msg.twist.twist.angular.y = 0.0 - converted_msg.twist.twist.angular.z = 0.0 + converted_msg.twist.twist.angular.x = msg.twist.twist.angular.x # copying over + converted_msg.twist.twist.angular.y = msg.twist.twist.angular.y # copying over + converted_msg.twist.twist.angular.z = msg.twist.twist.angular.z # rad/s, heading change rate return converted_msg + + def convert_NAND_other_state(self, msg): + """ Converts other/raw_state in SC namespace (NAND data) to clean state units and structure """ + converted_msg = Odometry() + converted_msg.header = msg.header + + # ---- 1. Directly use UTM Coordinates ---- + converted_msg.pose.pose.position.x = msg.x # UTM Easting + converted_msg.pose.pose.position.y = msg.y # UTM Northing + converted_msg.pose.pose.position.z = msg.z # UTM Altitude -- not provided in other/raw_state + + # ---- 2. Orientation in Radians ---- + converted_msg.pose.pose.orientation.x = msg.roll # roll not provided in other/raw_state + converted_msg.pose.pose.orientation.y = msg.pitch # pitch not provided in other/raw_state + converted_msg.pose.pose.orientation.z = msg.heading # heading in radians + converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles + + # ---- 3. Copy Covariances (Unchanged) ---- + converted_msg.pose.covariance = msg.pose_covariance # not provided in other/raw_state + converted_msg.twist.covariance = msg.twist_covariance # not provided in other/raw_state + + # ---- 4. Linear Velocities in m/s ---- + # Convert scalar speed to velocity x/y components using heading (msg.heading) + speed = msg.speed # m/s scalar velocity + heading = msg.heading # heading in radians + + # Calculate velocity components + converted_msg.twist.twist.linear.x = speed * np.cos(heading) # m/s in x-direction + converted_msg.twist.twist.linear.y = speed * np.sin(heading) # m/s in y-direction + converted_msg.twist.twist.linear.z = 0.0 - # replaced custom function with tf_transformations library function - def quaternion_to_yaw(self, qx, qy, qz, qw): - """Convert a quaternion to yaw (heading) in radians.""" - siny_cosp = 2.0 * (qw * qz + qx * qy) - cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) # assumes quaternion is normalized, should be true in ROS, - # else use (qw * qw + qx * qx - qy * qy - qz * qz) - return np.arctan2(siny_cosp, cosy_cosp) + # ---- 5. Angular Velocities ---- + converted_msg.twist.twist.angular.x = msg.roll_rate # roll rate not provided in other/raw_state + converted_msg.twist.twist.angular.y = msg.pitch_rate # pitch rate not provided in other/raw_state + converted_msg.twist.twist.angular.z = msg.heading_rate # rad/s, heading change rate + + return converted_msg def main(args=None): From d728740dd4b6914c00602828404083ef23025396 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 20 Nov 2024 18:03:48 -0500 Subject: [PATCH 07/35] added node to sim file --- rb_ws/src/buggy/launch/sim_2d_single.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index ca458b8..0398279 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -10,6 +10,8 @@ + + From c34e0bb858a01dd61916ecb54ace95dafd3756a6 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Thu, 21 Nov 2024 00:24:53 -0500 Subject: [PATCH 08/35] fixed ros package errors --- python-requirements.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/python-requirements.txt b/python-requirements.txt index 12d4591..46d0064 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -6,10 +6,8 @@ osqp pandas pymap3d pyproj -rclpy scipy setuptools==58.2.0 -tf_transformations trimesh utm keyboard From 24d29fbcb5128649cde911bac8d3f03502fe0aaf Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Mon, 25 Nov 2024 09:41:03 -0500 Subject: [PATCH 09/35] pylint *sigh* --- rb_ws/src/buggy/buggy/buggy_state_converter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index b45f94b..7a73053 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -60,7 +60,7 @@ def convert_SC_state(self, msg): Takes in ROS message in nav_msgs/Odometry format Assumes that the SC namespace is using ECEF coordinates and quaternion orientation """ - + converted_msg = Odometry() converted_msg.header = msg.header @@ -144,7 +144,7 @@ def convert_NAND_state(self, msg): converted_msg.twist.twist.angular.z = msg.twist.twist.angular.z # rad/s, heading change rate return converted_msg - + def convert_NAND_other_state(self, msg): """ Converts other/raw_state in SC namespace (NAND data) to clean state units and structure """ converted_msg = Odometry() From 5ae86cb475489dce8cd7d5e2d12e2510b080d30a Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Sun, 1 Dec 2024 23:42:44 +0000 Subject: [PATCH 10/35] Ported ros_to_bnyahaj.py - Added pyserial to project requirements - Added host_comm, pose, and world.py - Copied tf1 euler/quaternion conversion code to pose.py --- python-requirements.txt | 1 + rb_ws/src/buggy/buggy/auton/pose.py | 260 ++++++++++++++++++ rb_ws/src/buggy/buggy/auton/world.py | 226 +++++++++++++++ rb_ws/src/buggy/buggy/serial/host_comm.py | 218 +++++++++++++++ .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 255 +++++++++++++++++ 5 files changed, 960 insertions(+) create mode 100644 rb_ws/src/buggy/buggy/auton/pose.py create mode 100644 rb_ws/src/buggy/buggy/auton/world.py create mode 100644 rb_ws/src/buggy/buggy/serial/host_comm.py create mode 100644 rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py diff --git a/python-requirements.txt b/python-requirements.txt index 68b0c6f..66b300e 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -5,6 +5,7 @@ numpy osqp pandas pymap3d +pyserial scipy setuptools==58.2.0 trimesh diff --git a/rb_ws/src/buggy/buggy/auton/pose.py b/rb_ws/src/buggy/buggy/auton/pose.py new file mode 100644 index 0000000..b631ca6 --- /dev/null +++ b/rb_ws/src/buggy/buggy/auton/pose.py @@ -0,0 +1,260 @@ +import math + +import numpy as np + +from geometry_msgs.msg import Pose as ROSPose + + +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 + + # https://gist.github.com/salmagro/2e698ad4fbf9dae40244769c5ab74434 + @staticmethod + def euler_from_quaternion(x, y, z, w): + """ + Converts quaternion (w in last place) to euler roll, pitch, yaw + quaternion = [x, y, z, w] + """ + + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) + roll = np.arctan2(sinr_cosp, cosr_cosp) + + sinp = 2 * (w * y - z * x) + pitch = np.arcsin(sinp) + + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) + yaw = np.arctan2(siny_cosp, cosy_cosp) + + return roll, pitch, yaw + + @staticmethod + def quaternion_from_euler(roll, pitch, yaw): + """ + Converts euler roll, pitch, yaw to quaternion (w in last place) + quat = [x, y, z, w] + Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. + """ + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + + return q + + @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) = Pose.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 = Pose.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) \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/auton/world.py b/rb_ws/src/buggy/buggy/auton/world.py new file mode 100644 index 0000000..0558688 --- /dev/null +++ b/rb_ws/src/buggy/buggy/auton/world.py @@ -0,0 +1,226 @@ +import utm +import numpy as np + +from pose import Pose + + +class World: + """Abstraction for the world coordinate system + + The real world uses GPS coordinates, aka latitude and longitude. However, + using lat/lon is bad for path planning for several reasons. First, the difference + in numbers would be very tiny for small distances, alluding to roundoff errors. + Additionally, lat/lon follows a North-East-Down coordinate system, with headings + in the clockwise direction. We want to use an East-North-Up coordinate system, so + that the heading is in the counter-clockwise direction. + + We do this by converting GPS coordinates to UTM coordinates, which are in meters. + UTM works by dividing the world into a grid, where each grid cell has a unique + identifier. A UTM coordinate consists of the grid cell identifier and the "easting" + and "northing" within that grid cell. The easting (x) and northing (y) are in meters, + and are relative to the southwest corner of the grid cell. + + Last, we offset the UTM coordinates to be relative to some arbitrary zero point. That + way, the final world coordinates are relatively close to zero, which makes debugging + easier. + + This class provides methods to convert between GPS and world coordinates. There is + a version for single coordinates and a version for numpy arrays. + """ + + # Geolocates to around the southwest corner of Phipps + WORLD_EAST_ZERO = 589106 + WORLD_NORTH_ZERO = 4476929 + + @staticmethod + def gps_to_world(lat, lon): + """Converts GPS coordinates to world coordinates + + Args: + lat (float): latitude + lon (float): longitude + + Returns: + tuple: (x, y) in meters from some arbitrary zero point + """ + utm_coords = utm.from_latlon(lat, lon) + x = utm_coords[0] - World.WORLD_EAST_ZERO + y = utm_coords[1] - World.WORLD_NORTH_ZERO + + return x, y + + @staticmethod + def utm_to_world_pose(pose: Pose) -> Pose: + """Converts UTM coordinates to world coordinates + + Args: + pose (Pose): pose with utm coordinates + + Returns: + Pose: pose with world coordinates + """ + + utm_e = pose.x + utm_n = pose.y + x = utm_e - World.WORLD_EAST_ZERO + y = utm_n - World.WORLD_NORTH_ZERO + return Pose(x, y, pose.theta) + + @staticmethod + def world_to_utm_pose(pose: Pose) -> Pose: + """Converts world coordinates to utm coordinates + + Args: + pose (Pose): pose with world coordinates + + Returns: + Pose: pose with world coordinates + """ + + utm_e = pose.x + World.WORLD_EAST_ZERO + utm_n = pose.y + World.WORLD_NORTH_ZERO + return Pose(utm_e, utm_n, pose.theta) + + @staticmethod + def world_to_utm_numpy(coords): + """Converts world coordinates to utm coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of x, y pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of utm_e, utm_n pairs + """ + + N = np.shape(coords)[0] + utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO + utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO + utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1) + + return coords + utm_offset + + @staticmethod + def utm_to_world_numpy(coords): + """Converts utm coordinates to world coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of utm_e, utm_n pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of x, y pairs + """ + + N = np.shape(coords)[0] + utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO + utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO + utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1) + + return coords - utm_offset + + + @staticmethod + def world_to_gps(x, y): + """Converts world coordinates to GPS coordinates + + Args: + x (float): x in meters from some arbitrary zero point + y (float): y in meters from some arbitrary zero point + + Returns: + tuple: (lat, lon) + """ + utm_coords = utm.to_latlon( + x + World.WORLD_EAST_ZERO, y + World.WORLD_NORTH_ZERO, 17, "T" + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return lat, lon + + @staticmethod + def utm_to_gps(x, y): + """Converts utm coordinates to GPS coordinates + + Args: + x (float): x in meters, in utm frame + y (float): y in meters, in utm frame + + Returns: + tuple: (lat, lon) + """ + utm_coords = utm.to_latlon( + x, y, 17, "T" + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return lat, lon + + @staticmethod + def gps_to_world_numpy(coords): + """Converts GPS coordinates to world coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of lat, lon pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of x, y pairs + """ + utm_coords = utm.from_latlon(coords[:, 0], coords[:, 1]) + x = utm_coords[0] - World.WORLD_EAST_ZERO + y = utm_coords[1] - World.WORLD_NORTH_ZERO + + return np.stack((x, y), axis=1) + + @staticmethod + def world_to_gps_numpy(coords): + """Converts world coordinates to GPS coordinates + + Args: + coords (numpy.ndarray [size: (N,2)]): array of x, y pairs + + Returns: + numpy.ndarray [size: (N,2)]: array of lat, lon pairs + """ + + # Pittsburgh is in UTM zone 17T. + utm_coords = utm.to_latlon( + coords[:, 0] + World.WORLD_EAST_ZERO, + coords[:, 1] + World.WORLD_NORTH_ZERO, + 17, + "T", + ) + lat = utm_coords[0] + lon = utm_coords[1] + + return np.stack((lat, lon), axis=1) + + @staticmethod + def gps_to_world_pose(pose: Pose): + """Converts GPS coordinates to world coordinates + + Args: + pose (Pose): pose with lat, lon coordinates (x=lon, y=lat) + + Returns: + Pose: pose with x, y coordinates + """ + world_coords = World.gps_to_world(pose.y, pose.x) + new_heading = pose.theta # INS uses ENU frame so no heading conversion needed + + return Pose(world_coords[0], world_coords[1], new_heading) + + @staticmethod + def world_to_gps_pose(pose: Pose): + """Converts world coordinates to GPS coordinates + + Args: + pose (Pose): pose with x, y coordinates + + Returns: + Pose: pose with lat, lon coordinates + """ + gps_coords = World.world_to_gps(pose.x, pose.y) + new_heading = pose.theta + + return Pose(gps_coords[1], gps_coords[0], new_heading) \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py new file mode 100644 index 0000000..e98c23e --- /dev/null +++ b/rb_ws/src/buggy/buggy/serial/host_comm.py @@ -0,0 +1,218 @@ +import struct +import time +from dataclasses import dataclass + +from serial import Serial + +class Crc16: + def __init__(self): + self.accum = 0 + + def update(self, b): + if isinstance(b, int): + assert(0 <= b < 256) + + crc_table = [ + 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, + 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, + 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072, + 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041, + 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2, + 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, + 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, + 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, + 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192, + 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1, + 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1, + 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2, + 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, + 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, + 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, + 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312, + 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321, + 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371, + 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, + 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, + 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, + 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2, + 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381, + 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291, + 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2, + 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, + 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, + 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, + 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, + 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, + 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202 + ] + + i = ((self.accum >> 8) ^ b) & 0xFF; + i %= 256 + self.accum = ((self.accum << 8) & 0xFF00) ^ crc_table[i]; + else: + for one_byte in b: + self.update(one_byte) + +SYNC_WORD = b'\xAA\xFF\x00\x55' + +MAX_PAYLOAD_LEN = 100 + +MSG_TYPE_DEBUG = b'DB' +MSG_TYPE_ODOMETRY = b'OD' +MSG_TYPE_STEERING = b'ST' +MSG_TYPE_BRAKE = b'BR' +MSG_TYPE_ALARM = b'AL' +MSG_TYPE_BNYATEL = b'BT' + +@dataclass +class Odometry: + x: float + y: float + radio_seqnum: int + gps_seqnum: int + +@dataclass +class BnyaTelemetry: + x: float + y: float + velocity: float # In METERS / SECOND + steering: float # In DEGREES + heading: float # In RADIANS + heading_rate: float # In RADIANS / SECOND + +class IncompletePacket(Exception): + pass + +class ChecksumMismatch(Exception): + pass + +class Comms: + def __init__(self, path_to_port): + self.port = Serial(path_to_port, 1_000_000) + self.rx_buffer = b'' + + def send_packet_raw(self, msg_type: bytes, payload: bytes): + assert(len(msg_type) == 2) + assert(len(payload) < MAX_PAYLOAD_LEN) + + checksum = Crc16() + def write_and_checksum(data: bytes): + nonlocal checksum + + self.port.write(data) + checksum.update(data) + + self.port.write(SYNC_WORD) + write_and_checksum(msg_type) + write_and_checksum(len(payload).to_bytes(2, 'little')) + write_and_checksum(payload) + self.port.write(checksum.accum.to_bytes(2, 'little')) + + def send_steering(self, angle: float): + self.send_packet_raw(MSG_TYPE_STEERING, struct.pack(' len(self.rx_buffer): + raise IncompletePacket() + + data = self.rx_buffer[index:][:count] + index += count + return data + + def read_and_checksum(count: int): + nonlocal checksum + + data = read(count) + checksum.update(data) + return data + + if read(1) != SYNC_WORD[0:1]: + self.rx_buffer = self.rx_buffer[1:] + continue + if read(1) != SYNC_WORD[1:2]: + self.rx_buffer = self.rx_buffer[1:] + continue + if read(1) != SYNC_WORD[2:3]: + self.rx_buffer = self.rx_buffer[1:] + continue + if read(1) != SYNC_WORD[3:4]: + self.rx_buffer = self.rx_buffer[1:] + continue + + msg_type = read_and_checksum(2) + msg_len = int.from_bytes(read_and_checksum(2), 'little') + + if msg_len > MAX_PAYLOAD_LEN: + self.rx_buffer = self.rx_buffer[1:] + continue + + payload = read_and_checksum(msg_len) + + rx_crc = int.from_bytes(read(2), 'little') + + self.rx_buffer = self.rx_buffer[4 + 3 + 2 + msg_len + 2:] + + if rx_crc != checksum.accum: + raise ChecksumMismatch() + + return (msg_type, payload) + + def read_packet(self): + packet = self.read_packet_raw() + if packet is None: + return None + + msg_type, payload = packet + if msg_type == MSG_TYPE_ODOMETRY: + # Odometry message + x, y, radio_seqnum, gps_seqnum = struct.unpack(' 0.01: + print(packet) + last_time = time.time() + comms.send_steering(1234.5) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py new file mode 100644 index 0000000..0e55a16 --- /dev/null +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -0,0 +1,255 @@ +import argparse +from threading import Lock +import threading + +import rclpy +from rclpy import Node + +from std_msgs.msg import Float64, Int8, UInt8, Bool + +from host_comm import * + +from buggy.auton.world import World +from buggy.auton.pose import Pose + +from nav_msgs.msg import Odometry as ROSOdom + +class Translator(Node): + """ + Translates the output from bnyahaj serial (interpreted from host_comm) to ros topics and vice versa. + Performs reading (from Bnya Serial) and writing (from Ros Topics) on different python threads, so + be careful of multithreading synchronizaiton issues. + SC: + (ROS) Self Steering topic --> (Bnyahaj) Stepper Motor + (Bnyahaj) NAND Odom --> (ROS) NAND Odom topic + + NAND: + (ROS) Self Steering --> (Bnyahaj) Stepper Motor + (Bnyahaj) Self Odom from UKF --> (ROS) NAND Odom topic + """ + + def __init__(self, self_name, other_name, teensy_name): + """ + self_name: Buggy namespace + other_name: Only requried by SC for passing buggy namespace + teensy_name: required for communication, different for SC and NAND + + Initializes the subscribers, rates, and ros topics (including debug topics) + """ + super().__init__('ROS_serial_translator') + + self.comms = Comms("/dev/" + teensy_name) + self.steer_angle = 0 + self.alarm = 0 + self.fresh_steer = False + self.lock = Lock() + + self.create_subscription( + Float64, self_name + "/buggy/input/steering", self.set_steering, 1 + ) + + self.create_subscription(Int8, self_name + "/debug/sanity_warning", self.set_alarm, 1) + + # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/83 + if other_name is None and self_name == "NAND": + self.odom_publisher = self.create_publisher( + ROSOdom, self_name + "/nav/odom", 1 + ) + else: + self.odom_publisher = self.create_publisher( + ROSOdom, other_name + "/nav/odom", 1 + ) + + # upper bound of steering update rate, make sure auton sends slower than 500 Hz or update / 2ms + self.steer_send_rate = self.create_rate(500) + + # upper bound of reading data from Bnyahaj Serial, at 1ms + self.read_rate = self.create_rate(1000) + + # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/90 + self.rc_steering_angle_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/rc_steering_angle", 1 + ) + self.steering_angle_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/steering_angle", 1 + ) + self.battery_voltage_publisher = self.create_publisher( + Float64, self_name + "/buggy/debug/battery_voltage", 1 + ) + self.operator_ready_publisher = self.create_publisher( + Bool, self_name + "/buggy/debug/operator_ready", 1 + ) + self.steering_alarm_publisher = self.create_publisher( + Bool, self_name + "/buggy/debug/steering_alarm", 1 + ) + self.brake_status_publisher = self.create_publisher( + Bool, self_name + "/buggy/debug/brake_status", 1 + ) + self.use_auton_steer_publisher = self.create_publisher( + Bool, self_name + "/buggy/debug/use_auton_steer", 1 + ) + self.rc_uplink_qual_publisher = self.create_publisher( + UInt8, self_name + "/buggy/debug/rc_uplink_quality", 1 + ) + self.nand_fix_publisher = self.create_publisher( + UInt8, self_name + "/buggy/debug/nand_fix", 1 + ) + + def set_alarm(self, msg): + """ + alarm ros topic reader, locked so that only one of the setters runs at once + """ + with self.lock: + self.get_logger().debug(f"Reading alarm of {msg.data}") + self.alarm = msg.data + + def set_steering(self, msg): + """ + Steering Angle Updater, updates the steering angle locally if updated on ros stopic + """ + self.get_logger().debug(f"Read steeering angle of: {msg.data}") + # print("Steering angle: " + str(msg.data)) + # print("SET STEERING: " + str(msg.data)) + with self.lock: + self.steer_angle = msg.data + self.fresh_steer = True + + def writer_thread(self): + """ + Sends ROS Topics to bnayhaj serial, only sends a steering angle when we receive a fresh one + Will send steering and alarm node. + TODO: Does alarm node exist for NAND? + """ + self.get_logger().info("Starting sending alarm and steering to teensy!") + + while not self.is_shutdown(): + if self.fresh_steer: + with self.lock: + self.comms.send_steering(self.steer_angle) + self.fresh_steer = False + + with self.lock: + self.comms.send_alarm(self.alarm) + + self.steer_send_rate.sleep() + + def reader_thread(self): + """ + Reads three different types of packets, that should have better ways of differntiating + Odometry -> (SC) NAND Odomotery + BnyaTelemetry -> (NAND) Self Odom + tuple -> (SC, maybe NAND?) Debug Info + """ + self.get_logger().info("Starting reading odom from teensy!") + while rclpy.ok(): + packet = self.comms.read_packet() + + if isinstance(packet, Odometry): + self.get_logger().debug("packet" + str(packet)) + # Publish to odom topic x and y coord + odom = ROSOdom() + # convert to long lat + try: + lat, long = World.utm_to_gps(packet.y, packet.x) + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + self.odom_publisher.publish(odom) + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + + elif isinstance(packet, BnyaTelemetry): + self.get_logger().debug("packet" + str(packet)) + odom = ROSOdom() + + # TODO: Not mock rolled accurately (Needs to be Fact Checked) + try: + lat, long = World.utm_to_gps(packet.x, packet.y) + odom.pose.pose.position.x = long + odom.pose.pose.position.y = lat + odom.twist.twist.angular.z = packet.heading_rate + heading = Pose.heading_to_quaternion(packet.heading) + + odom.pose.pose.orientation.x = heading[0] + odom.pose.pose.orientation.y = heading[1] + odom.pose.pose.orientation.z = heading[2] + odom.pose.pose.orientation.w = heading[3] + + speed = packet.velocity + # TODO: fix this + # why this works: autonsystem only cares about magnitude of velocity + # so setting an arbitrary axis to the speed and leave other at 0 + # works. However, this should be done properly ASAP + odom.twist.twist.linear.x = speed + self.odom_publisher.publish(odom) + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + + elif isinstance( + packet, tuple + ): # Are there any other packet that is a tuple + self.rc_steering_angle_publisher.publish(Float64(packet[0])) + self.steering_angle_publisher.publish(Float64(packet[1])) + self.battery_voltage_publisher.publish(Float64(packet[2])) + self.operator_ready_publisher.publish(Bool(packet[3])) + self.steering_alarm_publisher.publish(Bool(packet[4])) + self.brake_status_publisher.publish(Bool(packet[5])) + self.use_auton_steer_publisher.publish(Bool(packet[6])) + self.rc_uplink_qual_publisher.publish(UInt8(packet[7])) + self.nand_fix_publisher.publish(UInt8(packet[8])) + + self.read_rate.sleep() + + def loop(self): + """ + Initialies the reader and writer thread, should theoretically never finish as there are while loops + """ + p1 = threading.Thread(target=self.writer_thread) + p2 = threading.Thread(target=self.reader_thread) + + p1.start() + p2.start() + + p1.join() + p2.join() + + +# Initializes ros nodes, using self and other name +# other name is not requires, and if not submitted, use NONE +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "--self_name", type=str, help="name of ego-buggy", required=True + ) + parser.add_argument( + "--other_name", + type=str, + help="name of other buggy", + required=False, + default=None, + ) + parser.add_argument( + "--teensy_name", type=str, help="name of teensy port", required=True + ) + args, _ = parser.parse_known_args() + self_name = args.self_name + other_name = args.other_name + teensy_name = args.teensy_name + + rclpy.init() + + translate = Translator(self_name, other_name, teensy_name) + + if self_name == "SC" and other_name is None: + translate.get_logger().warn( + "Not reading NAND Odometry messages, double check roslaunch files for ros_to_bnyahaj" + ) + elif other_name is None: + translate.get_logger().info("No other name passed in, presuming that this is NAND ") + + rclpy.spin(translate) + + rclpy.shutdown() From 86da93ce7d30b012e5861086ef337139fa2ecf4b Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 4 Dec 2024 16:54:55 -0500 Subject: [PATCH 11/35] ditch tf_transformations for scipy rotation --- rb_ws/src/buggy/buggy/buggy_state_converter.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 7a73053..bd21bea 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -5,7 +5,7 @@ from nav_msgs.msg import Odometry import numpy as np import pyproj -from tf_transformations import euler_from_quaternion +from scipy.spatial.transform import Rotation class BuggyStateConverter(Node): def __init__(self): @@ -78,8 +78,9 @@ def convert_SC_state(self, msg): # ---- 2. Convert Quaternion to Heading (Radians) ---- qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w - # Use euler_from_quaternion to get roll, pitch, yaw - roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw]) + # Use Rotation.from_quat to get roll, pitch, yaw + roll, pitch, yaw = Rotation.from_quat([qx, qy, qz, qw]).as_euler('xyz'); + # roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw]) # tf_transformations bad # Store the heading in the x component of the orientation converted_msg.pose.pose.orientation.x = roll From ed35e5a0d34b05db763e61b5ec5ee9848a6fb568 Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Wed, 11 Dec 2024 19:50:19 +0000 Subject: [PATCH 12/35] Removed pose/world.py, updated ros_to_bnyahaj.py - Using scipy/utm package directly, removed dependency on pose/world.py - Added steering angle debug on publish - Standardizes use of rclpy.ok() instead of self.is_shutdown() - All packets are sent over debug telemetry --- rb_ws/src/buggy/buggy/auton/pose.py | 260 ------------------ rb_ws/src/buggy/buggy/auton/world.py | 226 --------------- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 35 ++- 3 files changed, 17 insertions(+), 504 deletions(-) delete mode 100644 rb_ws/src/buggy/buggy/auton/pose.py delete mode 100644 rb_ws/src/buggy/buggy/auton/world.py diff --git a/rb_ws/src/buggy/buggy/auton/pose.py b/rb_ws/src/buggy/buggy/auton/pose.py deleted file mode 100644 index b631ca6..0000000 --- a/rb_ws/src/buggy/buggy/auton/pose.py +++ /dev/null @@ -1,260 +0,0 @@ -import math - -import numpy as np - -from geometry_msgs.msg import Pose as ROSPose - - -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 - - # https://gist.github.com/salmagro/2e698ad4fbf9dae40244769c5ab74434 - @staticmethod - def euler_from_quaternion(x, y, z, w): - """ - Converts quaternion (w in last place) to euler roll, pitch, yaw - quaternion = [x, y, z, w] - """ - - sinr_cosp = 2 * (w * x + y * z) - cosr_cosp = 1 - 2 * (x * x + y * y) - roll = np.arctan2(sinr_cosp, cosr_cosp) - - sinp = 2 * (w * y - z * x) - pitch = np.arcsin(sinp) - - siny_cosp = 2 * (w * z + x * y) - cosy_cosp = 1 - 2 * (y * y + z * z) - yaw = np.arctan2(siny_cosp, cosy_cosp) - - return roll, pitch, yaw - - @staticmethod - def quaternion_from_euler(roll, pitch, yaw): - """ - Converts euler roll, pitch, yaw to quaternion (w in last place) - quat = [x, y, z, w] - Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. - """ - cy = math.cos(yaw * 0.5) - sy = math.sin(yaw * 0.5) - cp = math.cos(pitch * 0.5) - sp = math.sin(pitch * 0.5) - cr = math.cos(roll * 0.5) - sr = math.sin(roll * 0.5) - - q = [0] * 4 - q[0] = cy * cp * cr + sy * sp * sr - q[1] = cy * cp * sr - sy * sp * cr - q[2] = sy * cp * sr + cy * sp * cr - q[3] = sy * cp * cr - cy * sp * sr - - return q - - @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) = Pose.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 = Pose.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) \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/auton/world.py b/rb_ws/src/buggy/buggy/auton/world.py deleted file mode 100644 index 0558688..0000000 --- a/rb_ws/src/buggy/buggy/auton/world.py +++ /dev/null @@ -1,226 +0,0 @@ -import utm -import numpy as np - -from pose import Pose - - -class World: - """Abstraction for the world coordinate system - - The real world uses GPS coordinates, aka latitude and longitude. However, - using lat/lon is bad for path planning for several reasons. First, the difference - in numbers would be very tiny for small distances, alluding to roundoff errors. - Additionally, lat/lon follows a North-East-Down coordinate system, with headings - in the clockwise direction. We want to use an East-North-Up coordinate system, so - that the heading is in the counter-clockwise direction. - - We do this by converting GPS coordinates to UTM coordinates, which are in meters. - UTM works by dividing the world into a grid, where each grid cell has a unique - identifier. A UTM coordinate consists of the grid cell identifier and the "easting" - and "northing" within that grid cell. The easting (x) and northing (y) are in meters, - and are relative to the southwest corner of the grid cell. - - Last, we offset the UTM coordinates to be relative to some arbitrary zero point. That - way, the final world coordinates are relatively close to zero, which makes debugging - easier. - - This class provides methods to convert between GPS and world coordinates. There is - a version for single coordinates and a version for numpy arrays. - """ - - # Geolocates to around the southwest corner of Phipps - WORLD_EAST_ZERO = 589106 - WORLD_NORTH_ZERO = 4476929 - - @staticmethod - def gps_to_world(lat, lon): - """Converts GPS coordinates to world coordinates - - Args: - lat (float): latitude - lon (float): longitude - - Returns: - tuple: (x, y) in meters from some arbitrary zero point - """ - utm_coords = utm.from_latlon(lat, lon) - x = utm_coords[0] - World.WORLD_EAST_ZERO - y = utm_coords[1] - World.WORLD_NORTH_ZERO - - return x, y - - @staticmethod - def utm_to_world_pose(pose: Pose) -> Pose: - """Converts UTM coordinates to world coordinates - - Args: - pose (Pose): pose with utm coordinates - - Returns: - Pose: pose with world coordinates - """ - - utm_e = pose.x - utm_n = pose.y - x = utm_e - World.WORLD_EAST_ZERO - y = utm_n - World.WORLD_NORTH_ZERO - return Pose(x, y, pose.theta) - - @staticmethod - def world_to_utm_pose(pose: Pose) -> Pose: - """Converts world coordinates to utm coordinates - - Args: - pose (Pose): pose with world coordinates - - Returns: - Pose: pose with world coordinates - """ - - utm_e = pose.x + World.WORLD_EAST_ZERO - utm_n = pose.y + World.WORLD_NORTH_ZERO - return Pose(utm_e, utm_n, pose.theta) - - @staticmethod - def world_to_utm_numpy(coords): - """Converts world coordinates to utm coordinates - - Args: - coords (numpy.ndarray [size: (N,2)]): array of x, y pairs - - Returns: - numpy.ndarray [size: (N,2)]: array of utm_e, utm_n pairs - """ - - N = np.shape(coords)[0] - utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO - utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO - utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1) - - return coords + utm_offset - - @staticmethod - def utm_to_world_numpy(coords): - """Converts utm coordinates to world coordinates - - Args: - coords (numpy.ndarray [size: (N,2)]): array of utm_e, utm_n pairs - - Returns: - numpy.ndarray [size: (N,2)]: array of x, y pairs - """ - - N = np.shape(coords)[0] - utm_offset_e = np.ones((N, )) * World.WORLD_EAST_ZERO - utm_offset_n = np.ones((N, )) * World.WORLD_NORTH_ZERO - utm_offset = np.stack((utm_offset_e, utm_offset_n), axis=1) - - return coords - utm_offset - - - @staticmethod - def world_to_gps(x, y): - """Converts world coordinates to GPS coordinates - - Args: - x (float): x in meters from some arbitrary zero point - y (float): y in meters from some arbitrary zero point - - Returns: - tuple: (lat, lon) - """ - utm_coords = utm.to_latlon( - x + World.WORLD_EAST_ZERO, y + World.WORLD_NORTH_ZERO, 17, "T" - ) - lat = utm_coords[0] - lon = utm_coords[1] - - return lat, lon - - @staticmethod - def utm_to_gps(x, y): - """Converts utm coordinates to GPS coordinates - - Args: - x (float): x in meters, in utm frame - y (float): y in meters, in utm frame - - Returns: - tuple: (lat, lon) - """ - utm_coords = utm.to_latlon( - x, y, 17, "T" - ) - lat = utm_coords[0] - lon = utm_coords[1] - - return lat, lon - - @staticmethod - def gps_to_world_numpy(coords): - """Converts GPS coordinates to world coordinates - - Args: - coords (numpy.ndarray [size: (N,2)]): array of lat, lon pairs - - Returns: - numpy.ndarray [size: (N,2)]: array of x, y pairs - """ - utm_coords = utm.from_latlon(coords[:, 0], coords[:, 1]) - x = utm_coords[0] - World.WORLD_EAST_ZERO - y = utm_coords[1] - World.WORLD_NORTH_ZERO - - return np.stack((x, y), axis=1) - - @staticmethod - def world_to_gps_numpy(coords): - """Converts world coordinates to GPS coordinates - - Args: - coords (numpy.ndarray [size: (N,2)]): array of x, y pairs - - Returns: - numpy.ndarray [size: (N,2)]: array of lat, lon pairs - """ - - # Pittsburgh is in UTM zone 17T. - utm_coords = utm.to_latlon( - coords[:, 0] + World.WORLD_EAST_ZERO, - coords[:, 1] + World.WORLD_NORTH_ZERO, - 17, - "T", - ) - lat = utm_coords[0] - lon = utm_coords[1] - - return np.stack((lat, lon), axis=1) - - @staticmethod - def gps_to_world_pose(pose: Pose): - """Converts GPS coordinates to world coordinates - - Args: - pose (Pose): pose with lat, lon coordinates (x=lon, y=lat) - - Returns: - Pose: pose with x, y coordinates - """ - world_coords = World.gps_to_world(pose.y, pose.x) - new_heading = pose.theta # INS uses ENU frame so no heading conversion needed - - return Pose(world_coords[0], world_coords[1], new_heading) - - @staticmethod - def world_to_gps_pose(pose: Pose): - """Converts world coordinates to GPS coordinates - - Args: - pose (Pose): pose with x, y coordinates - - Returns: - Pose: pose with lat, lon coordinates - """ - gps_coords = World.world_to_gps(pose.x, pose.y) - new_heading = pose.theta - - return Pose(gps_coords[1], gps_coords[0], new_heading) \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 0e55a16..17f5ae7 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -4,14 +4,13 @@ import rclpy from rclpy import Node +from scipy.spatial.transform import Rotation as R +import utm from std_msgs.msg import Float64, Int8, UInt8, Bool from host_comm import * -from buggy.auton.world import World -from buggy.auton.pose import Pose - from nav_msgs.msg import Odometry as ROSOdom class Translator(Node): @@ -122,10 +121,11 @@ def writer_thread(self): """ self.get_logger().info("Starting sending alarm and steering to teensy!") - while not self.is_shutdown(): + while rclpy.ok(): if self.fresh_steer: with self.lock: self.comms.send_steering(self.steer_angle) + self.get_logger().debug(f"Sent steering angle of: {self.steer_angle}") self.fresh_steer = False with self.lock: @@ -143,14 +143,14 @@ def reader_thread(self): self.get_logger().info("Starting reading odom from teensy!") while rclpy.ok(): packet = self.comms.read_packet() + self.get_logger().debug("packet" + str(packet)) if isinstance(packet, Odometry): - self.get_logger().debug("packet" + str(packet)) # Publish to odom topic x and y coord odom = ROSOdom() # convert to long lat try: - lat, long = World.utm_to_gps(packet.y, packet.x) + lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") odom.pose.pose.position.x = long odom.pose.pose.position.y = lat self.odom_publisher.publish(odom) @@ -160,16 +160,15 @@ def reader_thread(self): ) elif isinstance(packet, BnyaTelemetry): - self.get_logger().debug("packet" + str(packet)) odom = ROSOdom() # TODO: Not mock rolled accurately (Needs to be Fact Checked) try: - lat, long = World.utm_to_gps(packet.x, packet.y) + lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") odom.pose.pose.position.x = long odom.pose.pose.position.y = lat odom.twist.twist.angular.z = packet.heading_rate - heading = Pose.heading_to_quaternion(packet.heading) + heading = R.from_euler('x', packet.heading, degrees=False).as_quat() odom.pose.pose.orientation.x = heading[0] odom.pose.pose.orientation.y = heading[1] @@ -191,15 +190,15 @@ def reader_thread(self): elif isinstance( packet, tuple ): # Are there any other packet that is a tuple - self.rc_steering_angle_publisher.publish(Float64(packet[0])) - self.steering_angle_publisher.publish(Float64(packet[1])) - self.battery_voltage_publisher.publish(Float64(packet[2])) - self.operator_ready_publisher.publish(Bool(packet[3])) - self.steering_alarm_publisher.publish(Bool(packet[4])) - self.brake_status_publisher.publish(Bool(packet[5])) - self.use_auton_steer_publisher.publish(Bool(packet[6])) - self.rc_uplink_qual_publisher.publish(UInt8(packet[7])) - self.nand_fix_publisher.publish(UInt8(packet[8])) + self.rc_steering_angle_publisher.publish(Float64(data=packet[0])) + self.steering_angle_publisher.publish(Float64(data=packet[1])) + self.battery_voltage_publisher.publish(Float64(data=packet[2])) + self.operator_ready_publisher.publish(Bool(data=packet[3])) + self.steering_alarm_publisher.publish(Bool(data=packet[4])) + self.brake_status_publisher.publish(Bool(data=packet[5])) + self.use_auton_steer_publisher.publish(Bool(data=packet[6])) + self.rc_uplink_qual_publisher.publish(UInt8(data=packet[7])) + self.nand_fix_publisher.publish(UInt8(data=packet[8])) self.read_rate.sleep() From 48a477e4adeccd56c78a64e489a1644406399ebb Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Sun, 15 Dec 2024 18:49:52 -0500 Subject: [PATCH 13/35] fixed namespaces --- .../src/buggy/buggy/buggy_state_converter.py | 28 +++++++++---------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index bd21bea..48b1086 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -11,7 +11,6 @@ class BuggyStateConverter(Node): def __init__(self): super().__init__("buggy_state_converter") - # Determine namespace to configure the subscriber and publisher correctly namespace = self.get_namespace() # Create publisher and subscriber for "self" namespace @@ -21,7 +20,7 @@ def __init__(self): self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10) # Check if namespace is "SC" to add an "other" subscriber/publisher - if namespace == "SC": + if namespace == "/SC": self.other_raw_state_subscriber = self.create_subscription( Odometry, "other/raw_state", self.other_raw_state_callback, 10 ) @@ -36,15 +35,14 @@ def self_raw_state_callback(self, msg): """ Callback for processing self/raw_state messages and publishing to self/state """ namespace = self.get_namespace() - if namespace == "SC": + if namespace == "/SC": converted_msg = self.convert_SC_state(msg) - elif namespace == "NAND": + elif namespace == "/NAND": converted_msg = self.convert_NAND_state(msg) else: - self.get_logger().warn("Namespace not recognized for buggy state conversion.") + self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}") return - # Publish the converted message to self/state self.self_state_publisher.publish(converted_msg) def other_raw_state_callback(self, msg): @@ -79,7 +77,7 @@ def convert_SC_state(self, msg): qx, qy, qz, qw = msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w # Use Rotation.from_quat to get roll, pitch, yaw - roll, pitch, yaw = Rotation.from_quat([qx, qy, qz, qw]).as_euler('xyz'); + roll, pitch, yaw = Rotation.from_quat([qx, qy, qz, qw]).as_euler('xyz') # roll, pitch, yaw = euler_from_quaternion([qx, qy, qz, qw]) # tf_transformations bad # Store the heading in the x component of the orientation @@ -154,17 +152,17 @@ def convert_NAND_other_state(self, msg): # ---- 1. Directly use UTM Coordinates ---- converted_msg.pose.pose.position.x = msg.x # UTM Easting converted_msg.pose.pose.position.y = msg.y # UTM Northing - converted_msg.pose.pose.position.z = msg.z # UTM Altitude -- not provided in other/raw_state + converted_msg.pose.pose.position.z = msg.z # UTM Altitude (not provided in other/raw_state, defaults to 0.0) # ---- 2. Orientation in Radians ---- - converted_msg.pose.pose.orientation.x = msg.roll # roll not provided in other/raw_state - converted_msg.pose.pose.orientation.y = msg.pitch # pitch not provided in other/raw_state + converted_msg.pose.pose.orientation.x = msg.roll # (roll not provided in other/raw_state, defaults to 0.0) + converted_msg.pose.pose.orientation.y = msg.pitch # (pitch not provided in other/raw_state, defaults to 0.0) converted_msg.pose.pose.orientation.z = msg.heading # heading in radians - converted_msg.pose.pose.orientation.w = 0.0 # fourth (quaternion) term irrelevant for euler angles + converted_msg.pose.pose.orientation.w = 0.0 # fourth quaternion term irrelevant for euler angles # ---- 3. Copy Covariances (Unchanged) ---- - converted_msg.pose.covariance = msg.pose_covariance # not provided in other/raw_state - converted_msg.twist.covariance = msg.twist_covariance # not provided in other/raw_state + converted_msg.pose.covariance = msg.pose_covariance # (not provided in other/raw_state) + converted_msg.twist.covariance = msg.twist_covariance # (not provided in other/raw_state) # ---- 4. Linear Velocities in m/s ---- # Convert scalar speed to velocity x/y components using heading (msg.heading) @@ -177,8 +175,8 @@ def convert_NAND_other_state(self, msg): converted_msg.twist.twist.linear.z = 0.0 # ---- 5. Angular Velocities ---- - converted_msg.twist.twist.angular.x = msg.roll_rate # roll rate not provided in other/raw_state - converted_msg.twist.twist.angular.y = msg.pitch_rate # pitch rate not provided in other/raw_state + converted_msg.twist.twist.angular.x = msg.roll_rate # (roll rate not provided in other/raw_state, defaults to 0.0) + converted_msg.twist.twist.angular.y = msg.pitch_rate # (pitch rate not provided in other/raw_state, defaults to 0.0) converted_msg.twist.twist.angular.z = msg.heading_rate # rad/s, heading change rate return converted_msg From a57c3168a01960035ad6be8c056e57eaa92f4143 Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Wed, 18 Dec 2024 01:05:52 +0000 Subject: [PATCH 14/35] Removed import alias for SciPy Rotation in ros_to_bnyahaj.py, fixed possible swapping of packet x and y for ROS odom topic publishing. --- rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py index 17f5ae7..ca4bbea 100644 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py @@ -4,7 +4,7 @@ import rclpy from rclpy import Node -from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Rotation import utm from std_msgs.msg import Float64, Int8, UInt8, Bool @@ -150,7 +150,8 @@ def reader_thread(self): odom = ROSOdom() # convert to long lat try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") + # TODO check if the order of y and x here is accurate + lat, long = utm.to_latlon(packet.y, packet.x, 17, "T") odom.pose.pose.position.x = long odom.pose.pose.position.y = lat self.odom_publisher.publish(odom) @@ -168,7 +169,7 @@ def reader_thread(self): odom.pose.pose.position.x = long odom.pose.pose.position.y = lat odom.twist.twist.angular.z = packet.heading_rate - heading = R.from_euler('x', packet.heading, degrees=False).as_quat() + heading = Rotation.from_euler('x', packet.heading, degrees=False).as_quat() odom.pose.pose.orientation.x = heading[0] odom.pose.pose.orientation.y = heading[1] From 55c4cae1b39073aff8c993c195f25ca0420a635e Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 18 Dec 2024 22:43:50 -0800 Subject: [PATCH 15/35] cleanup --- rb_ws/src/buggy/buggy/simulator/engine.py | 80 +++++++---------------- rb_ws/src/buggy/buggy/util.py | 8 +++ 2 files changed, 31 insertions(+), 57 deletions(-) create mode 100644 rb_ws/src/buggy/buggy/util.py diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 361ae4b..912ec56 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -8,25 +8,17 @@ from nav_msgs.msg import Odometry import numpy as np import utm +from util import Utils class Simulator(Node): - # simulator constants: - UTM_EAST_ZERO = 589702.87 - UTM_NORTH_ZERO = 4477172.947 - UTM_ZONE_NUM = 17 - UTM_ZONE_LETTER = "T" - #TODO: make these values accurate - WHEELBASE_SC = 1.17 - WHEELBASE_NAND= 1.17 def __init__(self): super().__init__('sim_single') self.get_logger().info('INITIALIZED.') - self.starting_poses = { - "Hill1_NAND": (Simulator.UTM_EAST_ZERO + 0, Simulator.UTM_NORTH_ZERO + 0, -110), - "Hill1_SC": (Simulator.UTM_EAST_ZERO + 20, Simulator.UTM_NORTH_ZERO + 30, -110), + "Hill1_NAND": (Utils.UTM_EAST_ZERO + 0, Utils.UTM_NORTH_ZERO + 0, -110), + "Hill1_SC": (Utils.UTM_EAST_ZERO + 20, Utils.UTM_NORTH_ZERO + 30, -110), "WESTINGHOUSE": (589647, 4477143, -150), "UC_TO_PURNELL": (589635, 4477468, 160), "UC": (589681, 4477457, 160), @@ -39,9 +31,6 @@ def __init__(self): "RACELINE_PASS": (589468.02, 4476993.07, -160), } - self.number_publisher = self.create_publisher(Float64, 'numbers', 1) - self.i = 0 - # for X11 matplotlib (direction included) self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1) @@ -51,43 +40,43 @@ def __init__(self): self.steering_subscriber = self.create_subscription( Float64, "buggy/input/steering", self.update_steering_angle, 1 ) - # To read from velocity - self.velocity_subscriber = self.create_subscription( - Float64, "velocity", self.update_velocity, 1 - ) + self.navsatfix_noisy_publisher = self.create_publisher( NavSatFix, "state/pose_navsat_noisy", 1 ) - - + # To read from velocity + self.velocity_subscriber = self.create_subscription( + Float64, "velocity", self.update_velocity, 1 + ) self.declare_parameter('velocity', 12) + if (self.get_namespace() == "/SC"): self.buggy_name = "SC" self.declare_parameter('pose', "Hill1_SC") - self.wheelbase = Simulator.WHEELBASE_SC + self.wheelbase = Utils.WHEELBASE_SC if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" self.declare_parameter('pose', "Hill1_NAND") - self.wheelbase = Simulator.WHEELBASE_NAND + self.wheelbase = Utils.WHEELBASE_NAND self.velocity = self.get_parameter("velocity").value init_pose_name = self.get_parameter("pose").value + self.init_pose = self.starting_poses[init_pose_name] self.e_utm, self.n_utm, self.heading = self.init_pose self.steering_angle = 0 # degrees - self.rate = 200 # Hz - self.pub_skip = 2 # publish every pub_skip ticks - self.pub_tick_count = 0 + self.rate = 100 # Hz + self.tick_count = 0 + self.interval = 2 # how frequently to publish self.lock = threading.Lock() - freq = 10 - timer_period = 1/freq # seconds + timer_period = 1/self.rate # seconds self.timer = self.create_timer(timer_period, self.loop) def update_steering_angle(self, data: Float64): @@ -98,14 +87,6 @@ def update_velocity(self, data: Float64): with self.lock: self.velocity = data.data - def get_steering_arc(self): - with self.lock: - steering_angle = self.steering_angle - if steering_angle == 0.0: - return np.inf - - return Simulator.WHEELBASE / np.tan(np.deg2rad(steering_angle)) - def dynamics(self, state, v): l = self.wheelbase _, _, theta, delta = state @@ -154,18 +135,13 @@ def publish(self): (lat, long) = utm.to_latlon( p.position.x, p.position.y, - Simulator.UTM_ZONE_NUM, - Simulator.UTM_ZONE_LETTER, + Utils.UTM_ZONE_NUM, + Utils.UTM_ZONE_LETTER, ) - nsf = NavSatFix() - nsf.latitude = lat - nsf.longitude = long - nsf.altitude = float(260) - nsf.header.stamp = time_stamp - lat_noisy = lat + np.random.normal(0, 1e-6) long_noisy = long + np.random.normal(0, 1e-6) + nsf_noisy = NavSatFix() nsf_noisy.latitude = lat_noisy nsf_noisy.longitude = long_noisy @@ -176,8 +152,8 @@ def publish(self): odom.header.stamp = time_stamp odom_pose = Pose() - odom_pose.position.x = float(long) - odom_pose.position.y = float(lat) + odom_pose.position.x = float(long_noisy) + odom_pose.position.y = float(lat_noisy) odom_pose.position.z = float(260) odom_pose.orientation.z = np.sin(np.deg2rad(self.heading) / 2) @@ -192,20 +168,10 @@ def publish(self): self.pose_publisher.publish(odom) def loop(self): - msg = Float64() - msg.data = float(self.i) - - self.number_publisher.publish(msg) - self.i += 1 - self.step() - if self.pub_tick_count == self.pub_skip: + if self.tick_count % self.interval == 0: self.publish() - self.pub_tick_count = 0 - else: - self.pub_tick_count += 1 - - + self.tick_count += 1 def main(args=None): diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util.py new file mode 100644 index 0000000..b5e78e1 --- /dev/null +++ b/rb_ws/src/buggy/buggy/util.py @@ -0,0 +1,8 @@ +class Utils: + UTM_EAST_ZERO = 589702.87 + UTM_NORTH_ZERO = 4477172.947 + UTM_ZONE_NUM = 17 + UTM_ZONE_LETTER = "T" + #TODO: make these values accurate + WHEELBASE_SC = 1.17 + WHEELBASE_NAND= 1.17 \ No newline at end of file From 295913880f50b6449dfdbc8b1c983914e356e1fb Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Wed, 18 Dec 2024 23:02:03 -0800 Subject: [PATCH 16/35] cleaned up constants class --- rb_ws/src/buggy/buggy/simulator/engine.py | 18 +++++++++++------- rb_ws/src/buggy/buggy/util.py | 2 +- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 912ec56..0d68462 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 import threading +import sys import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance @@ -8,17 +9,20 @@ from nav_msgs.msg import Odometry import numpy as np import utm -from util import Utils +sys.path.append("/rb_ws/src/buggy/buggy") +from util import Constants class Simulator(Node): + def __init__(self): super().__init__('sim_single') self.get_logger().info('INITIALIZED.') + self.starting_poses = { - "Hill1_NAND": (Utils.UTM_EAST_ZERO + 0, Utils.UTM_NORTH_ZERO + 0, -110), - "Hill1_SC": (Utils.UTM_EAST_ZERO + 20, Utils.UTM_NORTH_ZERO + 30, -110), + "Hill1_NAND": (Constants.UTM_EAST_ZERO + 0, Constants.UTM_NORTH_ZERO + 0, -110), + "Hill1_SC": (Constants.UTM_EAST_ZERO + 20, Constants.UTM_NORTH_ZERO + 30, -110), "WESTINGHOUSE": (589647, 4477143, -150), "UC_TO_PURNELL": (589635, 4477468, 160), "UC": (589681, 4477457, 160), @@ -56,12 +60,12 @@ def __init__(self): if (self.get_namespace() == "/SC"): self.buggy_name = "SC" self.declare_parameter('pose', "Hill1_SC") - self.wheelbase = Utils.WHEELBASE_SC + self.wheelbase = Constants.WHEELBASE_SC if (self.get_namespace() == "/NAND"): self.buggy_name = "NAND" self.declare_parameter('pose', "Hill1_NAND") - self.wheelbase = Utils.WHEELBASE_NAND + self.wheelbase = Constants.WHEELBASE_NAND self.velocity = self.get_parameter("velocity").value init_pose_name = self.get_parameter("pose").value @@ -135,8 +139,8 @@ def publish(self): (lat, long) = utm.to_latlon( p.position.x, p.position.y, - Utils.UTM_ZONE_NUM, - Utils.UTM_ZONE_LETTER, + Constants.UTM_ZONE_NUM, + Constants.UTM_ZONE_LETTER, ) lat_noisy = lat + np.random.normal(0, 1e-6) diff --git a/rb_ws/src/buggy/buggy/util.py b/rb_ws/src/buggy/buggy/util.py index b5e78e1..0bdfc8e 100644 --- a/rb_ws/src/buggy/buggy/util.py +++ b/rb_ws/src/buggy/buggy/util.py @@ -1,4 +1,4 @@ -class Utils: +class Constants: UTM_EAST_ZERO = 589702.87 UTM_NORTH_ZERO = 4477172.947 UTM_ZONE_NUM = 17 From 2aba16d0a676c4752676e64357907b2e75138dcb Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Sat, 21 Dec 2024 21:20:26 -0800 Subject: [PATCH 17/35] 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 0000000..db56fe5 --- /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 0000000..e69de29 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 0000000..e69de29 diff --git a/rb_ws/src/buggy/launch/controller.xml b/rb_ws/src/buggy/launch/controller.xml new file mode 100644 index 0000000..52f0ac7 --- /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 42318d0..cd28d34 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 18/35] 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 db56fe5..ae800e7 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 19/35] 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 ae800e7..7411572 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 20/35] 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 7411572..91fef61 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 a537b8839b41d717152c49a700542ee7c117297b Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 23 Dec 2024 22:16:24 -0800 Subject: [PATCH 21/35] rearranged subscribers/publishers --- rb_ws/src/buggy/buggy/simulator/engine.py | 40 +++++++++++------------ 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 0d68462..8d11998 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -35,28 +35,7 @@ def __init__(self): "RACELINE_PASS": (589468.02, 4476993.07, -160), } - # for X11 matplotlib (direction included) - self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1) - - # simulate the INS's outputs (noise included) - self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1) - - self.steering_subscriber = self.create_subscription( - Float64, "buggy/input/steering", self.update_steering_angle, 1 - ) - - self.navsatfix_noisy_publisher = self.create_publisher( - NavSatFix, "state/pose_navsat_noisy", 1 - ) - - # To read from velocity - self.velocity_subscriber = self.create_subscription( - Float64, "velocity", self.update_velocity, 1 - ) - - self.declare_parameter('velocity', 12) - if (self.get_namespace() == "/SC"): self.buggy_name = "SC" self.declare_parameter('pose', "Hill1_SC") @@ -83,6 +62,25 @@ def __init__(self): timer_period = 1/self.rate # seconds self.timer = self.create_timer(timer_period, self.loop) + self.steering_subscriber = self.create_subscription( + Float64, "buggy/input/steering", self.update_steering_angle, 1 + ) + + # To read from velocity + self.velocity_subscriber = self.create_subscription( + Float64, "velocity", self.update_velocity, 1 + ) + + # for X11 matplotlib (direction included) + self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1) + + # simulate the INS's outputs (noise included) + self.pose_publisher = self.create_publisher(Odometry, "nav/odom", 1) + + self.navsatfix_noisy_publisher = self.create_publisher( + NavSatFix, "state/pose_navsat_noisy", 1 + ) + def update_steering_angle(self, data: Float64): with self.lock: self.steering_angle = data.data From a5a6ab5600356ff992a914b7033981c4d7fe243c Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 23 Dec 2024 22:30:42 -0800 Subject: [PATCH 22/35] changed units to match clean_slate type (utm) --- rb_ws/src/buggy/buggy/simulator/engine.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/buggy/simulator/engine.py index 8d11998..efaf6db 100644 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/buggy/simulator/engine.py @@ -75,6 +75,7 @@ def __init__(self): self.plot_publisher = self.create_publisher(Pose, "sim_2d/utm", 1) # 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.navsatfix_noisy_publisher = self.create_publisher( @@ -154,13 +155,15 @@ def publish(self): odom.header.stamp = time_stamp odom_pose = Pose() - odom_pose.position.x = float(long_noisy) - odom_pose.position.y = float(lat_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) + # NOTE: autonsystem only cares about magnitude of velocity, so we don't need to split into components odom_twist = Twist() odom_twist.linear.x = float(velocity) From b10857fd578d50555a5f1d5165c53abd21bccb73 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Tue, 24 Dec 2024 18:33:36 -0800 Subject: [PATCH 23/35] 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 91fef61..051f4d0 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 e69de29..6944ccd 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 24/35] 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 e69de29..d8be46b 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 0000000..985755a --- /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 0000000..a7da7df --- /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 25/35] 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 051f4d0..bcc4652 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 6944ccd..ce152c0 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 26/35] 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 bcc4652..ee1bb2f 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 d8be46b..932538c 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 efaf6db..e03c2c6 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 985755a..b39e03d 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 a7da7df..85b996f 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 0398279..bc455d6 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 a44f005..a8a022e 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 27/35] 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 60a6406..e8edffc 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 ce152c0..f960de7 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 932538c..3407653 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 85b996f..cf9f662 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 bc455d6..2c62b1b 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 28/35] 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 ee1bb2f..af60dc0 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 29/35] 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 af60dc0..132efce 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 30/35] 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 132efce..700bef0 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 31/35] 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 700bef0..9d13593 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 e03c2c6..b3cb774 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 b39e03d..f12a4ff 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 cf9f662..72fbcae 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 32/35] fixed gitignore --- .gitignore | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index e8edffc..f3fcd37 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 From e53769d5182542c05477d7bf79ae0e4938d0fc33 Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Tue, 7 Jan 2025 16:17:52 -0500 Subject: [PATCH 33/35] Telematics Rewrite (#11) --- .../buggy/buggy/visualization/telematics.py | 63 +++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100644 rb_ws/src/buggy/buggy/visualization/telematics.py diff --git a/rb_ws/src/buggy/buggy/visualization/telematics.py b/rb_ws/src/buggy/buggy/visualization/telematics.py new file mode 100644 index 0000000..9d1fd80 --- /dev/null +++ b/rb_ws/src/buggy/buggy/visualization/telematics.py @@ -0,0 +1,63 @@ +#! /usr/bin/env python3 +# Runs the conversion script for all telematics data + +import rclpy +import utm +from nav_msgs.msg import Odometry +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix + + +class Telematics(Node): + """ + Converts subscribers and publishers that need to be reformated, so that they are readible. + """ + + def __init__(self): + """Generate all the subscribers and publishers that need to be reformatted. + """ + super().__init__('telematics') + + # Implements behavior of callback_args from rospy.Subscriber + def wrap_args(callback, callback_args): + return lambda msg: callback(msg, callback_args) + + self.self_publisher = self.create_publisher(NavSatFix, "/self/state_navsatfix", 10) + self.self_subscriber = self.create_subscription(Odometry, "/self/state", wrap_args(self.convert_buggystate, self.self_publisher), 1) + + self.other_publisher = self.create_publisher(NavSatFix, "/other/state_navsatfix", 10) + self.other_subscriber = self.create_subscription(Odometry, "/other/state", wrap_args(self.convert_buggystate, self.other_publisher), 1) + + # TODO Make this a static method? + def convert_buggystate(self, msg, publisher): + """Converts BuggyState/Odometry message to NavSatFix and publishes to specified publisher + + Args: + msg (Odometry): Buggy state to convert + publisher (Publisher): Publisher to send NavSatFix message to + """ + try: + y = msg.pose.pose.position.y + x = msg.pose.pose.position.x + lat, long = utm.to_latlon(x, y, 17, "T") + down = msg.pose.pose.position.z + new_msg = NavSatFix() + new_msg.header = msg.header + new_msg.latitude = lat + new_msg.longitude = long + new_msg.altitude = down + publisher.publish(new_msg) + + except Exception as e: + self.get_logger().warn( + "Unable to convert other buggy position to lon lat" + str(e) + ) + + +if __name__ == "__main__": + rclpy.init() + telem = Telematics() + rclpy.spin(telem) + + telem.destroy_node() + rclpy.shutdown() \ No newline at end of file From d3a09e801e7877be470838eb6c7bec9fad7c53f0 Mon Sep 17 00:00:00 2001 From: TiaSinghania <59597284+TiaSinghania@users.noreply.github.com> Date: Fri, 10 Jan 2025 12:13:42 -0800 Subject: [PATCH 34/35] serial port with updated packet structure (#13) --- .../src/buggy/buggy/buggy_state_converter.py | 55 ++-- rb_ws/src/buggy/buggy/serial/host_comm.py | 152 ++++++++-- .../src/buggy/buggy/serial/ros_to_bnyahaj.py | 272 +++++++++++------- 3 files changed, 315 insertions(+), 164 deletions(-) diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 48b1086..acc3d80 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -12,47 +12,51 @@ def __init__(self): super().__init__("buggy_state_converter") namespace = self.get_namespace() + if namespace == "/SC": + self.SC_raw_state_subscriber = self.create_subscription( + Odometry, "/raw_state", self.convert_SC_state_callback, 10 + ) - # Create publisher and subscriber for "self" namespace - self.self_raw_state_subscriber = self.create_subscription( - Odometry, "self/raw_state", self.self_raw_state_callback, 10 - ) - self.self_state_publisher = self.create_publisher(Odometry, "self/state", 10) + self.NAND_other_raw_state_subscriber = self.create_subscription( + Odometry, "/NAND_raw_state", self.convert_NAND_other_state_callback, 10 + ) - # Check if namespace is "SC" to add an "other" subscriber/publisher - if namespace == "/SC": - self.other_raw_state_subscriber = self.create_subscription( - Odometry, "other/raw_state", self.other_raw_state_callback, 10 + self.other_state_publisher = self.create_publisher(Odometry, "/other/state", 10) + + elif namespace == "/NAND": + self.NAND_raw_state_subscriber = self.create_subscription( + Odometry, "/raw_state", self.convert_NAND_state_callback, 10 ) - self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10) + + else: + self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}") + + self.self_state_publisher = self.create_publisher(Odometry, "/state", 10) # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( "epsg:4978", "epsg:32617", always_xy=True ) # TODO: Confirm UTM EPSG code, using EPSG:32617 for UTM Zone 17N - def self_raw_state_callback(self, msg): - """ Callback for processing self/raw_state messages and publishing to self/state """ - namespace = self.get_namespace() - - if namespace == "/SC": - converted_msg = self.convert_SC_state(msg) - elif namespace == "/NAND": - converted_msg = self.convert_NAND_state(msg) - else: - self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}") - return + def convert_SC_state_callback(self, msg): + """ Callback for processing SC/raw_state messages and publishing to self/state """ + converted_msg = self.convert_SC_state(msg) + self.self_state_publisher.publish(converted_msg) + def convert_NAND_state_callback(self, msg): + """ Callback for processing NAND/raw_state messages and publishing to self/state """ + converted_msg = self.convert_NAND_state(msg) self.self_state_publisher.publish(converted_msg) - def other_raw_state_callback(self, msg): - """ Callback for processing other/raw_state messages and publishing to other/state """ - # Convert the SC message and publish to other/state + + def convert_NAND_other_state_callback(self, msg): + """ Callback for processing SC/NAND_raw_state messages and publishing to other/state """ converted_msg = self.convert_NAND_other_state(msg) self.other_state_publisher.publish(converted_msg) + def convert_SC_state(self, msg): - """ + """ Converts self/raw_state in SC namespace to clean state units and structure Takes in ROS message in nav_msgs/Odometry format @@ -127,7 +131,6 @@ def convert_NAND_state(self, msg): converted_msg.twist.covariance = msg.twist.covariance # ---- 4. Linear Velocities in m/s ---- - # TODO: Check if scalar velocity is coming in from msg.twist.twist.linear.x # Convert scalar speed to velocity x/y components using heading (orientation.z) speed = msg.twist.twist.linear.x # m/s scalar velocity heading = msg.pose.pose.orientation.z # heading in radians diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/buggy/serial/host_comm.py index e98c23e..766247e 100644 --- a/rb_ws/src/buggy/buggy/serial/host_comm.py +++ b/rb_ws/src/buggy/buggy/serial/host_comm.py @@ -58,28 +58,104 @@ def update(self, b): MAX_PAYLOAD_LEN = 100 -MSG_TYPE_DEBUG = b'DB' -MSG_TYPE_ODOMETRY = b'OD' + +# firmware --> software +MSG_TYPE_NAND_DEBUG = b'ND' +MSG_TYPE_NAND_UKF = b'NU' +MSG_TYPE_NAND_GPS = b'NG' +MSG_TYPE_RADIO = b'RA' +MSG_TYPE_SC_DEBUG = b'SD' +MSG_TYPE_SC_SENSORS = b'SS' +MSG_TYPE_ROUNDTRIP_TIMESTAMP = b'RT' + +# software --> firmware MSG_TYPE_STEERING = b'ST' -MSG_TYPE_BRAKE = b'BR' MSG_TYPE_ALARM = b'AL' -MSG_TYPE_BNYATEL = b'BT' +MSG_TYPE_SOFTWARE_TIMESTAMP = b'TM' + + @dataclass -class Odometry: - x: float - y: float - radio_seqnum: int +class NANDDebugInfo: + # 64 bits + heading_rate: float # double + encoder_angle: float # double + # 32 bits + timestamp: int + rc_steering_angle: float + software_steering_angle: float + true_steering_angle: float + rfm69_timeout_num: int + # 8 bits + operator_ready: bool + brake_status: bool + auton_steer: bool + tx12_state: bool + stepper_alarm: int # unsigned char + rc_uplink_quality: int # uint8 + +@dataclass +class NANDUKF: + # 64 bits + easting: float # double + northing: float # double + theta: float # double + heading_rate: float # double + velocity: float # double + # 32 bits + timestamp: int + +@dataclass +class NANDRawGPS: + # 64 bits + easting: float # double + northing: float # double + # this is a 2D accuracy value + accuracy: float # double + gps_time: int # uint64 + # 32 bits + gps_seqnum: int + timestamp: int + # 8 bits + gps_fix: int # uint8 + +@dataclass +class Radio: + nand_east_gps: float + nand_north_gps: float gps_seqnum: int + nand_gps_fix: int # uint8 + +@dataclass +class SCDebugInfo: + # 64 bits + encoder_angle: float # double + # 32 bits + rc_steering_angle: float + software_steering_angle: float + true_steering_angle: float + missed_packets: int + timestamp: int + # 8 bits + tx12_state: bool + operator_ready: bool + stepper_alarm: int # unsigned char + brake_status: bool + auton_steer: bool + rc_uplink_quality: int # uint8 @dataclass -class BnyaTelemetry: - x: float - y: float - velocity: float # In METERS / SECOND - steering: float # In DEGREES - heading: float # In RADIANS - heading_rate: float # In RADIANS / SECOND +class SCSensors: + # 64 bits + velocity: float # double + # 32 bits + steering_angle: float + timestamp: int + +@dataclass +class RoundtripTimestamp: + returned_time: float # double + class IncompletePacket(Exception): pass @@ -115,9 +191,11 @@ def send_steering(self, angle: float): def send_alarm(self, status: int): self.send_packet_raw(MSG_TYPE_ALARM, struct.pack(' (Bnyahaj) Stepper Motor - (Bnyahaj) NAND Odom --> (ROS) NAND Odom topic - - NAND: - (ROS) Self Steering --> (Bnyahaj) Stepper Motor - (Bnyahaj) Self Odom from UKF --> (ROS) NAND Odom topic """ - def __init__(self, self_name, other_name, teensy_name): + def __init__(self, teensy_name): """ - self_name: Buggy namespace - other_name: Only requried by SC for passing buggy namespace teensy_name: required for communication, different for SC and NAND Initializes the subscribers, rates, and ros topics (including debug topics) """ + super().__init__('ROS_serial_translator') self.comms = Comms("/dev/" + teensy_name) + namespace = self.get_namespace() + if namespace == "/SC": + self.self_name = "SC" + else: + self.self_name = "NAND" + self.steer_angle = 0 self.alarm = 0 self.fresh_steer = False self.lock = Lock() self.create_subscription( - Float64, self_name + "/buggy/input/steering", self.set_steering, 1 + Float64, "/input/steering", self.set_steering, 1 ) - - self.create_subscription(Int8, self_name + "/debug/sanity_warning", self.set_alarm, 1) - - # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/83 - if other_name is None and self_name == "NAND": - self.odom_publisher = self.create_publisher( - ROSOdom, self_name + "/nav/odom", 1 - ) - else: - self.odom_publisher = self.create_publisher( - ROSOdom, other_name + "/nav/odom", 1 - ) + self.create_subscription(Int8, "/input/sanity_warning", self.set_alarm, 1) # upper bound of steering update rate, make sure auton sends slower than 500 Hz or update / 2ms self.steer_send_rate = self.create_rate(500) @@ -65,35 +47,88 @@ def __init__(self, self_name, other_name, teensy_name): # upper bound of reading data from Bnyahaj Serial, at 1ms self.read_rate = self.create_rate(1000) - # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/90 + # DEBUG MESSAGE PUBLISHERS: + self.heading_rate_publisher = self.create_publisher( + Float64, "/debug/heading_rate", 1 + ) + self.encoder_angle_publisher = self.create_publisher( + Float64, "/debug/encoder_angle", 1 + ) self.rc_steering_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/rc_steering_angle", 1 + Float64, "/debug/rc_steering_angle", 1 ) - self.steering_angle_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/steering_angle", 1 + self.software_steering_angle_publisher = self.create_publisher( + Float64, "/debug/software_steering_angle", 1 ) - self.battery_voltage_publisher = self.create_publisher( - Float64, self_name + "/buggy/debug/battery_voltage", 1 + self.true_steering_angle_publisher = self.create_publisher( + Float64, "/debug/true_steering_angle", 1 ) - self.operator_ready_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/operator_ready", 1 + self.rfm69_timeout_num_publisher = self.create_publisher( + Int32, "/debug/rfm_timeout_num", 1 ) - self.steering_alarm_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/steering_alarm", 1 + self.operator_ready_publisher = self.create_publisher( + Bool, "/debug/operator_ready", 1 ) self.brake_status_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/brake_status", 1 + Bool, "/debug/brake_status", 1 ) self.use_auton_steer_publisher = self.create_publisher( - Bool, self_name + "/buggy/debug/use_auton_steer", 1 + Bool, "/debug/use_auton_steer", 1 + ) + self.tx12_state_publisher = self.create_publisher( + Bool, "/debug/tx12_connected", 1 + ) + self.stepper_alarm_publisher = self.create_publisher( + UInt8, "/debug/steering_alarm", 1 ) self.rc_uplink_qual_publisher = self.create_publisher( - UInt8, self_name + "/buggy/debug/rc_uplink_quality", 1 + UInt8, "/debug/rc_uplink_quality", 1 ) - self.nand_fix_publisher = self.create_publisher( - UInt8, self_name + "/buggy/debug/nand_fix", 1 + self.nand_gps_seqnum_publisher = self.create_publisher( + Int32, "/debug/NAND_gps_seqnum", 1 ) + # SERIAL DEBUG PUBLISHERS + self.roundtrip_time_publisher = self.create_publisher( + Float64, "/debug/roundtrip_time", 1 + ) + + if self.self_name == "NAND": + # NAND POSITION PUBLISHERS + self.nand_ukf_odom_publisher = self.create_publisher( + Odometry, "/raw_state", 1 + ) + self.nand_gps_odom_publisher = self.create_publisher( + Odometry, "/debug/gps_odom", 1 + ) + + self.nand_gps_fix_publisher = self.create_publisher( + UInt8, "/debug/gps_fix", 1 + ) + self.nand_gps_acc_publisher = self.create_publisher( + Float64, "/debug/gps_accuracy", 1 + ) + + self.nand_gps_time_publisher = self.create_publisher( + UInt64, "/debug/gps_time", 1 + ) + + if self.self_name == "SC": + + # SC SENSOR PUBLISHERS + self.sc_velocity_publisher = self.create_publisher( + Float64, "/sensors/velocity", 1 + ) + self.sc_steering_angle_publisher = self.create_publisher( + Float64, "/sensors/steering_angle", 1 + ) + + # RADIO DATA PUBLISHER + self.observed_nand_odom_publisher = self.create_publisher( + Odometry, "/NAND_raw_state", 1 + ) + + def set_alarm(self, msg): """ alarm ros topic reader, locked so that only one of the setters runs at once @@ -106,18 +141,16 @@ def set_steering(self, msg): """ Steering Angle Updater, updates the steering angle locally if updated on ros stopic """ - self.get_logger().debug(f"Read steeering angle of: {msg.data}") - # print("Steering angle: " + str(msg.data)) - # print("SET STEERING: " + str(msg.data)) + self.get_logger().debug(f"Read steering angle of: {msg.data}") with self.lock: self.steer_angle = msg.data self.fresh_steer = True + def writer_thread(self): """ Sends ROS Topics to bnayhaj serial, only sends a steering angle when we receive a fresh one Will send steering and alarm node. - TODO: Does alarm node exist for NAND? """ self.get_logger().info("Starting sending alarm and steering to teensy!") @@ -130,76 +163,97 @@ def writer_thread(self): with self.lock: self.comms.send_alarm(self.alarm) + with self.lock: + self.comms.send_timestamp(time.time()) self.steer_send_rate.sleep() def reader_thread(self): - """ - Reads three different types of packets, that should have better ways of differntiating - Odometry -> (SC) NAND Odomotery - BnyaTelemetry -> (NAND) Self Odom - tuple -> (SC, maybe NAND?) Debug Info - """ self.get_logger().info("Starting reading odom from teensy!") while rclpy.ok(): packet = self.comms.read_packet() self.get_logger().debug("packet" + str(packet)) - if isinstance(packet, Odometry): + if isinstance(packet, NANDDebugInfo): + self.heading_rate_publisher.publish(packet.heading_rate) + self.encoder_angle_publisher.publish(packet.encoder_angle) + self.rc_steering_angle_publisher.publish(packet.rc_steering_angle) + self.software_steering_angle_publisher.publish(packet.software_steering_angle) + self.true_steering_angle_publisher.publish(packet.true_steering_angle) + self.rfm69_timeout_num_publisher.publish(packet.rfm69_timeout_num) + self.operator_ready_publisher.publish(packet.operator_ready) + self.brake_status_publisher.publish(packet.brake_status) + self.use_auton_steer_publisher.publish(packet.auton_steer) + self.tx12_state_publisher.publish(packet.tx12_state) + self.stepper_alarm_publisher.publish(packet.stepper_alarm) + self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality) + self.get_logger().debug(f'NAND Debug Timestamp: {packet.timestamp}') + elif isinstance(packet, NANDUKF): + odom = Odometry() + odom.pose.pose.position.x = packet.easting + odom.pose.pose.position.y = packet.northing + odom.pose.pose.orientation.z = packet.theta + + odom.twist.twist.linear.x = packet.velocity + odom.twist.twist.angular.z = packet.heading_rate + + self.nand_ukf_odom_publisher.publish(odom) + self.get_logger().debug(f'NAND UKF Timestamp: {packet.timestamp}') + + + elif isinstance(packet, NANDRawGPS): + odom = Odometry() + odom.pose.pose.position.x = packet.easting + odom.pose.pose.position.y = packet.northing + odom.pose.pose.orientation.z = 0 + odom.twist.twist.linear.x = 0 + odom.twist.twist.linear.y = 0 + odom.twist.twist.angular.z = 0 + + self.nand_gps_odom_publisher.publish(odom) + self.nand_gps_fix_publisher.publish(packet.gps_fix) + self.nand_gps_acc_publisher.publish(packet.accuracy) + self.nand_gps_seqnum_publisher.publish(packet.gps_seqnum) + self.nand_gps_time_publisher.publish(packet.gps_time) + self.get_logger().debug(f'NAND Raw GPS Timestamp: {packet.timestamp}') + + + # this packet is received on Short Circuit + elif isinstance(packet, Radio): + # Publish to odom topic x and y coord - odom = ROSOdom() - # convert to long lat - try: - # TODO check if the order of y and x here is accurate - lat, long = utm.to_latlon(packet.y, packet.x, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) - - elif isinstance(packet, BnyaTelemetry): - odom = ROSOdom() - - # TODO: Not mock rolled accurately (Needs to be Fact Checked) - try: - lat, long = utm.to_latlon(packet.x, packet.y, 17, "T") - odom.pose.pose.position.x = long - odom.pose.pose.position.y = lat - odom.twist.twist.angular.z = packet.heading_rate - heading = Rotation.from_euler('x', packet.heading, degrees=False).as_quat() - - odom.pose.pose.orientation.x = heading[0] - odom.pose.pose.orientation.y = heading[1] - odom.pose.pose.orientation.z = heading[2] - odom.pose.pose.orientation.w = heading[3] - - speed = packet.velocity - # TODO: fix this - # why this works: autonsystem only cares about magnitude of velocity - # so setting an arbitrary axis to the speed and leave other at 0 - # works. However, this should be done properly ASAP - odom.twist.twist.linear.x = speed - self.odom_publisher.publish(odom) - except Exception as e: - self.get_logger().warn( - "Unable to convert other buggy position to lon lat" + str(e) - ) - - elif isinstance( - packet, tuple - ): # Are there any other packet that is a tuple - self.rc_steering_angle_publisher.publish(Float64(data=packet[0])) - self.steering_angle_publisher.publish(Float64(data=packet[1])) - self.battery_voltage_publisher.publish(Float64(data=packet[2])) - self.operator_ready_publisher.publish(Bool(data=packet[3])) - self.steering_alarm_publisher.publish(Bool(data=packet[4])) - self.brake_status_publisher.publish(Bool(data=packet[5])) - self.use_auton_steer_publisher.publish(Bool(data=packet[6])) - self.rc_uplink_qual_publisher.publish(UInt8(data=packet[7])) - self.nand_fix_publisher.publish(UInt8(data=packet[8])) + odom = Odometry() + + odom.pose.pose.position.x = packet.nand_east_gps + odom.pose.pose.position.y = packet.nand_north_gps + self.observed_nand_odom_publisher.publish(odom) + self.nand_gps_seqnum_publisher.publish(packet.gps_seqnum) + + + + + elif isinstance(packet, SCDebugInfo): + self.encoder_angle_publisher.publish(packet.encoder_angle) + self.rc_steering_angle_publisher.publish(packet.rc_steering_angle) + self.software_steering_angle_publisher.publish(packet.software_steering_angle) + self.true_steering_angle_publisher.publish(packet.true_steering_angle) + self.operator_ready_publisher.publish(packet.operator_ready) + self.brake_status_publisher.publish(packet.brake_status) + self.use_auton_steer_publisher.publish(packet.auton_steer) + self.tx12_state_publisher.publish(packet.tx12_state) + self.stepper_alarm_publisher.publish(packet.stepper_alarm) + self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality) + self.get_logger().debug(f'SC Debug Timestamp: {packet.timestamp}') + + + elif isinstance(packet, SCSensors): + self.sc_velocity_publisher.publish(packet.velocity) + self.sc_steering_angle_publisher.publish(packet.steering_angle) + self.get_logger().debug(f'SC Sensors Timestamp: {packet.timestamp}') + + + elif isinstance(packet, RoundtripTimestamp): + self.roundtrip_time_publisher.publish(time.time() - packet.returned_time) self.read_rate.sleep() From 07d062c74415cc680488b1320a5fac98c0855628 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Fri, 10 Jan 2025 22:20:08 -0800 Subject: [PATCH 35/35] Path planner (#14) Co-authored-by: Jackack --- .gitignore | 11 +- Dockerfile | 12 +- rb_ws/src/buggy/CMakeLists.txt | 48 ++++ rb_ws/src/buggy/launch/controller.xml | 4 - rb_ws/src/buggy/launch/sim_2d_double.xml | 31 +++ rb_ws/src/buggy/launch/sim_2d_single.xml | 15 +- rb_ws/src/buggy/launch/watchdog.xml | 2 +- rb_ws/src/buggy/package.xml | 32 ++- rb_ws/src/buggy/resource/buggy | 0 .../buggy_state_converter.py | 0 .../controller/controller_node.py | 11 +- .../controller/controller_superclass.py | 2 +- .../controller/stanley_controller.py | 6 +- .../buggy/{buggy => scripts}/hello_world.py | 4 +- .../scripts/path_planner/path_planner.py | 247 ++++++++++++++++++ .../{buggy => scripts}/serial/host_comm.py | 0 .../serial/ros_to_bnyahaj.py | 6 +- .../{buggy => scripts}/simulator/engine.py | 15 +- .../{buggy => scripts}/util/constants.py | 0 .../src/buggy/{buggy => scripts}/util/pose.py | 0 .../{buggy => scripts}/util/trajectory.py | 11 +- .../watchdog/watchdog_node.py | 2 + rb_ws/src/buggy/setup.cfg | 4 - rb_ws/src/buggy/setup.py | 34 --- rb_ws/src/buggy/test/test_copyright.py | 25 -- rb_ws/src/buggy/test/test_flake8.py | 25 -- rb_ws/src/buggy/test/test_pep257.py | 23 -- 27 files changed, 400 insertions(+), 170 deletions(-) create mode 100644 rb_ws/src/buggy/CMakeLists.txt delete mode 100644 rb_ws/src/buggy/launch/controller.xml create mode 100644 rb_ws/src/buggy/launch/sim_2d_double.xml delete mode 100644 rb_ws/src/buggy/resource/buggy rename rb_ws/src/buggy/{buggy => scripts}/buggy_state_converter.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/controller/controller_node.py (95%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/controller/controller_superclass.py (96%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/controller/stanley_controller.py (97%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/hello_world.py (50%) mode change 100644 => 100755 create mode 100755 rb_ws/src/buggy/scripts/path_planner/path_planner.py rename rb_ws/src/buggy/{buggy => scripts}/serial/host_comm.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/serial/ros_to_bnyahaj.py (99%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/simulator/engine.py (94%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/util/constants.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/util/pose.py (100%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/util/trajectory.py (98%) mode change 100644 => 100755 rename rb_ws/src/buggy/{buggy => scripts}/watchdog/watchdog_node.py (98%) mode change 100644 => 100755 delete mode 100644 rb_ws/src/buggy/setup.cfg delete mode 100644 rb_ws/src/buggy/setup.py delete mode 100644 rb_ws/src/buggy/test/test_copyright.py delete mode 100644 rb_ws/src/buggy/test/test_flake8.py delete mode 100644 rb_ws/src/buggy/test/test_pep257.py diff --git a/.gitignore b/.gitignore index f3fcd37..88667fb 100644 --- a/.gitignore +++ b/.gitignore @@ -1,10 +1,15 @@ .DS_Store -rb_ws/bags .docker-compose.yml.un~ .python-requirements.txt.un~ docker-compose.yml~ *TEMP_DO_NOT_EDIT.txt -rb_ws/src/buggy/bags/ + +# BAGS +rb_ws/bags +rb_ws/src/buggy/bags/* *.bag +rb_ws/rosbag2/ + +# VISION .vision/ -vision/data/ \ No newline at end of file +vision/data/ diff --git a/Dockerfile b/Dockerfile index 6725e77..4ffeb8d 100644 --- a/Dockerfile +++ b/Dockerfile @@ -23,16 +23,8 @@ RUN pip3 install -r python-requirements.txt RUN echo 'source "/opt/ros/humble/setup.bash" --' >> ~/.bashrc && \ echo 'cd rb_ws' >> ~/.bashrc && \ echo 'colcon build --symlink-install' >> ~/.bashrc && \ - echo 'source install/local_setup.bash' >> ~/.bashrc -# RUN echo 'source "/opt/ros/humble/setup.bash" --' >> ~/.bashrc && \ -# echo 'cd rb_ws' >> ~/.bashrc && \ -# echo 'catkin_make >/dev/null' >> ~/.bashrc && \ -# echo 'source devel/setup.bash' >> ~/.bashrc - - - -# RUN echo "exec firefox" > ~/.xinitrc && chmod +x ~/.xinitrc -# CMD ["x11vnc", "-create", "-forever"] + echo 'source install/local_setup.bash' >> ~/.bashrc && \ + echo 'chmod -R +x src/buggy/scripts/' >> ~/.bashrc # add mouse to tmux RUN echo 'set -g mouse on' >> ~/.tmux.conf diff --git a/rb_ws/src/buggy/CMakeLists.txt b/rb_ws/src/buggy/CMakeLists.txt new file mode 100644 index 0000000..a8a5142 --- /dev/null +++ b/rb_ws/src/buggy/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.8) +project(buggy) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) + +# Install Launch Files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +# Install Python modules +# ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/hello_world.py + scripts/controller/controller_node.py + scripts/path_planner/path_planner.py + scripts/simulator/engine.py + scripts/watchdog/watchdog_node.py + scripts/buggy_state_converter.py + scripts/serial/ros_to_bnyahaj.py + DESTINATION lib/${PROJECT_NAME} +) + + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/TrajectoryMsg.msg" +) +ament_export_dependencies(rosidl_default_runtime) + +ament_package() \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/controller.xml b/rb_ws/src/buggy/launch/controller.xml deleted file mode 100644 index 52f0ac7..0000000 --- a/rb_ws/src/buggy/launch/controller.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2d_double.xml b/rb_ws/src/buggy/launch/sim_2d_double.xml new file mode 100644 index 0000000..b9d5ba6 --- /dev/null +++ b/rb_ws/src/buggy/launch/sim_2d_double.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/sim_2d_single.xml b/rb_ws/src/buggy/launch/sim_2d_single.xml index 2c62b1b..f0e7204 100755 --- a/rb_ws/src/buggy/launch/sim_2d_single.xml +++ b/rb_ws/src/buggy/launch/sim_2d_single.xml @@ -2,23 +2,14 @@ - + - - + + - - - - \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/watchdog.xml b/rb_ws/src/buggy/launch/watchdog.xml index 28c784d..5c74690 100644 --- a/rb_ws/src/buggy/launch/watchdog.xml +++ b/rb_ws/src/buggy/launch/watchdog.xml @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/rb_ws/src/buggy/package.xml b/rb_ws/src/buggy/package.xml index e1471e2..26879e3 100644 --- a/rb_ws/src/buggy/package.xml +++ b/rb_ws/src/buggy/package.xml @@ -2,18 +2,30 @@ buggy - 0.0.0 - TODO: Package description - root - TODO: License declaration + 2.0.0 + CMU's First Robotic Buggy + CMU's Robotic Club Officers + MIT + + ament_cmake + ament_cmake_python + + + rclcpp + rclpy + - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest ros2launch + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + - ament_python + ament_cmake - + \ No newline at end of file diff --git a/rb_ws/src/buggy/resource/buggy b/rb_ws/src/buggy/resource/buggy deleted file mode 100644 index e69de29..0000000 diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/scripts/buggy_state_converter.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/buggy_state_converter.py rename to rb_ws/src/buggy/scripts/buggy_state_converter.py diff --git a/rb_ws/src/buggy/buggy/controller/controller_node.py b/rb_ws/src/buggy/scripts/controller/controller_node.py old mode 100644 new mode 100755 similarity index 95% rename from rb_ws/src/buggy/buggy/controller/controller_node.py rename to rb_ws/src/buggy/scripts/controller/controller_node.py index 9d13593..3094ca8 --- a/rb_ws/src/buggy/buggy/controller/controller_node.py +++ b/rb_ws/src/buggy/scripts/controller/controller_node.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import threading import sys @@ -8,8 +10,9 @@ from std_msgs.msg import Float32, Float64, Bool from nav_msgs.msg import Odometry +from buggy.msg import TrajectoryMsg -sys.path.append("/rb_ws/src/buggy/buggy") +sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory from controller.stanley_controller import StanleyController @@ -55,12 +58,12 @@ def __init__(self): Float64, "input/steering", 1 ) self.heading_publisher = self.create_publisher( - Float32, "auton/debug/heading", 1 + Float32, "debug/heading", 1 ) # Subscribers 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.traj_subscriber = self.create_subscription(TrajectoryMsg, 'self/cur_traj', self.traj_listener, 1) self.lock = threading.Lock() @@ -139,7 +142,7 @@ def loop(self): 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))) + self.steer_publisher.publish(Float64(data=float(steering_angle_deg.item()))) diff --git a/rb_ws/src/buggy/buggy/controller/controller_superclass.py b/rb_ws/src/buggy/scripts/controller/controller_superclass.py old mode 100644 new mode 100755 similarity index 96% rename from rb_ws/src/buggy/buggy/controller/controller_superclass.py rename to rb_ws/src/buggy/scripts/controller/controller_superclass.py index f960de7..731c96e --- a/rb_ws/src/buggy/buggy/controller/controller_superclass.py +++ b/rb_ws/src/buggy/scripts/controller/controller_superclass.py @@ -4,7 +4,7 @@ from nav_msgs.msg import Odometry -sys.path.append("/rb_ws/src/buggy/buggy") +sys.path.append("/rb_ws/src/buggy/scripts") from util.trajectory import Trajectory class Controller(ABC): diff --git a/rb_ws/src/buggy/buggy/controller/stanley_controller.py b/rb_ws/src/buggy/scripts/controller/stanley_controller.py old mode 100644 new mode 100755 similarity index 97% rename from rb_ws/src/buggy/buggy/controller/stanley_controller.py rename to rb_ws/src/buggy/scripts/controller/stanley_controller.py index 3407653..ce9b811 --- a/rb_ws/src/buggy/buggy/controller/stanley_controller.py +++ b/rb_ws/src/buggy/scripts/controller/stanley_controller.py @@ -5,7 +5,7 @@ from geometry_msgs.msg import Pose as ROSPose from nav_msgs.msg import Odometry -sys.path.append("/rb_ws/src/buggy/buggy") +sys.path.append("/rb_ws/src/buggy/scripts") 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( - NavSatFix, "auton/debug/reference_navsat", 1 + NavSatFix, "controller/debug/reference_navsat", 1 ) self.debug_error_publisher = self.node.create_publisher( - ROSPose, "auton/debug/error", 1 + ROSPose, "controller/debug/stanley_error", 1 ) def compute_control(self, state_msg : Odometry, trajectory : Trajectory): diff --git a/rb_ws/src/buggy/buggy/hello_world.py b/rb_ws/src/buggy/scripts/hello_world.py old mode 100644 new mode 100755 similarity index 50% rename from rb_ws/src/buggy/buggy/hello_world.py rename to rb_ws/src/buggy/scripts/hello_world.py index 0a119aa..0a2d9ba --- a/rb_ws/src/buggy/buggy/hello_world.py +++ b/rb_ws/src/buggy/scripts/hello_world.py @@ -1,5 +1,7 @@ +#!/usr/bin/env python3 + def main(): - print('Hi from buggy. Hello again') + print('Hi from buggy.') if __name__ == '__main__': diff --git a/rb_ws/src/buggy/scripts/path_planner/path_planner.py b/rb_ws/src/buggy/scripts/path_planner/path_planner.py new file mode 100755 index 0000000..a42e6a1 --- /dev/null +++ b/rb_ws/src/buggy/scripts/path_planner/path_planner.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python3 + +import sys +from threading import Lock + +import numpy as np +import rclpy +from rclpy.node import Node + +from nav_msgs.msg import Odometry +from std_msgs.msg import Float64 +from geometry_msgs.msg import Pose +from buggy.msg import TrajectoryMsg + +sys.path.append("/rb_ws/src/buggy/scripts") +from util.trajectory import Trajectory + +class PathPlanner(Node): + """ + Class to generate new trajectory splices for SC autonomous system. + + Takes in a default trajectory and an inner curb trajectory. + + """ + + # move the curb towards the center of the course by CURB_MARGIN meters + # for a margin of safety + CURB_MARGIN = 1 #m + + # the offset is calculated as a mirrored sigmoid function of distance + OFFSET_SCALE_CROSS_TRACK = 2 #m + OFFSET_SCALE_ALONG_TRACK = 0.2 + ACTIVATE_OTHER_SCALE_ALONG_TRACK = 0.1 + OFFSET_SHIFT_ALONG_TRACK = 4 #m + + # number of meters ahead of the buggy to generate local trajectory for + LOCAL_TRAJ_LEN = 50#m + + # start generating local trajectory this many meters ahead of current position + LOOKAHEAD = 2#m + + # number of points to sample along the nominal trajectory + RESOLUTION = 150 + + # frequency to run in hz + FREQUENCY = 10 + + def __init__(self) -> None: + super().__init__('path_planner') + self.get_logger().info('INITIALIZED.') + + + #Parameters + self.declare_parameter("traj_name", "buggycourse_safe.json") + traj_name = self.get_parameter("traj_name").value + self.nominal_traj = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj_name) #TODO: Fixed filepath, not good + + self.declare_parameter("curb_name", "") + curb_name = self.get_parameter("curb_name").value + curb_name = None if curb_name == "" else curb_name + if curb_name is None: + self.left_curb = None + else: + self.left_curb = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + curb_name) #TODO: Fixed filepath, not good + + #Publishers + self.other_buggy_xtrack_publisher = self.create_publisher(Float64, "debug/other_buggy_xtrack", 10) + self.traj_publisher = self.create_publisher(TrajectoryMsg, "self/cur_traj", 10) + + #Subscribers + self.self_pose_subscriber = self.create_subscription(Odometry, 'self/state', self.self_pose_callback, 1) + self.other_pose_subscriber = self.create_subscription(Odometry, 'other/state', self.other_pose_callback, 1) + + self.timer = self.create_timer(1/self.FREQUENCY, self.timer_callback) + + # set up subscriber and callback for pose variables + self.self_pose = None + self.other_pose = None + + self.self_pose_lock = Lock() + self.other_pose_lock = Lock() + + def self_pose_callback(self, msg): + with self.self_pose_lock: + self.self_pose = msg.pose.pose + + def other_pose_callback(self, msg): + with self.other_pose_lock: + self.other_pose = msg.pose.pose + self.get_logger().debug("Received position of other buggy.") + + def timer_callback(self): + with self.self_pose_lock and self.other_pose_lock: + if (self.self_pose is not None) and (self.other_pose is not None): + self.compute_traj(self.self_pose, self.other_pose) + + def offset_func(self, dist): + """ + Args: + dist: (N, ) numpy array, distances between ego-buggy and obstacle, + along the trajectory + Returns: + (N, ) numpy array, offsets from nominal trajectory required to overtake, + defined by a sigmoid function + """ + + return self.OFFSET_SCALE_CROSS_TRACK / \ + (1 + np.exp(-(-self.OFFSET_SCALE_ALONG_TRACK * dist + + self.OFFSET_SHIFT_ALONG_TRACK))) + + def activate_other_crosstrack_func(self, dist): + """ + Args: + dist: (N, ) numpy array, distances between ego-buggy and obstacle, + along the trajectory + Returns: + (N, ) numpy array, multiplier used to weigh the cross-track distance of + the obstacle into the passing offset calculation. + """ + return 1 / \ + (1 + np.exp(-(-self.ACTIVATE_OTHER_SCALE_ALONG_TRACK * dist + + self.OFFSET_SHIFT_ALONG_TRACK))) + + + def compute_traj( + self, + self_pose: Pose, + other_pose: Pose, + ) -> None: + """ + draw trajectory starting at the current pose and ending at a fixed distance + ahead. For each trajectory point, calculate the required offset perpendicular to the nominal + trajectory. A sigmoid function of the distance along track to the other buggy is used to + weigh the other buggy's cross-track distance. This calculation produces a line that + allows the ego-buggy's trajectory to go through the other buggy. Since we want to pass + the other buggy at some constant distance to the left, another sigmoid function is multiplied + by that constant distance to produce a smooth trajectory that passes the other buggy. + + Finally, the trajectory is bounded to the left by the left curb (if it exists), and to the right + by the nominal trajectory. (we never pass on the right) + + passing offsets = + activate_other_crosstrack_func(distance to other buggy along track) * + other buggy cross track distance + + offset_func(distance to other buggy along track) + + trajectory = nominal trajectory + + left nominal trajectory unit normal vector * + clamp(passing offsets, 0, distance from nominal trajectory to left curb) + + Args: + other_pose (Pose): Pose containing NAND's easting (x), + northing(y), and heading (theta), in UTM + + other_speed (float): speed in m/s of NAND + Publishes: + Trajectory: list of x,y coords for ego-buggy to follow. + """ + # grab slice of nominal trajectory + nominal_idx = self.nominal_traj.get_closest_index_on_path(self_pose.position.x, self_pose.position.y) + nominal_dist_along = self.nominal_traj.get_distance_from_index(nominal_idx) + + nominal_slice = np.empty((self.RESOLUTION, 2)) + + # compute the distance along nominal trajectory between samples and the obstacle + nominal_slice_dists = np.linspace( + nominal_dist_along + self.LOOKAHEAD, + nominal_dist_along + self.LOOKAHEAD + self.LOCAL_TRAJ_LEN, + self.RESOLUTION) + + for i in range(self.RESOLUTION): + nominal_slice[i, :] = np.array(self.nominal_traj.get_position_by_distance( + nominal_slice_dists[i] + )) + + # get index of the other buggy along the trajetory and convert to distance + other_idx = self.nominal_traj.get_closest_index_on_path(other_pose.position.x, other_pose.position.y) + other_dist = self.nominal_traj.get_distance_from_index(other_idx) + nominal_slice_to_other_dist = np.abs(nominal_slice_dists - other_dist) + + passing_offsets = self.offset_func(nominal_slice_to_other_dist) + + # compute signed cross-track distance between NAND and nominal + nominal_to_other = np.array((other_pose.position.x, other_pose.position.y)) - \ + np.array(self.nominal_traj.get_position_by_index(other_idx)) + + # dot product with the unit normal to produce left-positive signed distance + other_normal = self.nominal_traj.get_unit_normal_by_index(np.array(other_idx.ravel())) + other_cross_track_dist = np.sum( + nominal_to_other * other_normal, axis=1) + + self.other_buggy_xtrack_publisher.publish(Float64(data=float(other_cross_track_dist.item()))) + + # here, use passing offsets to weight NAND's cross track signed distance: + # if the sample point is far from SC, the cross track distance doesn't matter + # if the sample point is near, we add cross track distance to the offset, + # such that the resulting offset is adjusted by position of NAND + + passing_offsets = passing_offsets + \ + self.activate_other_crosstrack_func(nominal_slice_to_other_dist) * other_cross_track_dist + + # clamp passing offset distances to distance to the curb + if self.left_curb is not None: + # grab slice of curb correponding to slice of nominal trajectory. + curb_idx = self.left_curb.get_closest_index_on_path(self_pose.position.x, self_pose.position.y) + curb_dist_along = self.left_curb.get_distance_from_index(curb_idx) + curb_idx_end = self.left_curb.get_closest_index_on_path(nominal_slice[-1, 0], nominal_slice[-1, 1]) + curb_dist_along_end = self.left_curb.get_distance_from_index(curb_idx_end) + curb_dists = np.linspace(curb_dist_along, curb_dist_along_end, self.RESOLUTION) + + curb_slice = np.empty((self.RESOLUTION, 2)) + for i in range(self.RESOLUTION): + curb_slice[i, :] = np.array(self.left_curb.get_position_by_distance( + curb_dists[i] + )) + + # compute distances from the sample points to the curb + nominal_slice_to_curb_dist = np.linalg.norm(curb_slice - nominal_slice, axis=1) + passing_offsets = np.minimum(passing_offsets, nominal_slice_to_curb_dist - self.CURB_MARGIN) + + # clamp negative passing offsets to zero, since we always pass on the left, + # the passing offsets should never pull SC to the right. + passing_offsets = np.maximum(0, passing_offsets) + + # shift the nominal slice by passing offsets + nominal_normals = self.nominal_traj.get_unit_normal_by_index( + self.nominal_traj.get_index_from_distance(nominal_slice_dists) + ) + positions = nominal_slice + (passing_offsets[:, None] * nominal_normals) + + local_traj = Trajectory(json_filepath=None, positions=positions) + self.traj_publisher.publish(local_traj.pack(self_pose.position.x, self_pose.position.y )) + + +def main(args=None): + rclpy.init(args=args) + # TODO: set file paths here + # At the time of writing the below snippet, config management is TBD + + path_planner = PathPlanner() + rclpy.spin(path_planner) + + path_planner.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/buggy/serial/host_comm.py b/rb_ws/src/buggy/scripts/serial/host_comm.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/serial/host_comm.py rename to rb_ws/src/buggy/scripts/serial/host_comm.py diff --git a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py old mode 100644 new mode 100755 similarity index 99% rename from rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py rename to rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index 974ef87..0efffb6 --- a/rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py @@ -1,11 +1,13 @@ +#!/usr/bin/env python3 + import argparse from threading import Lock import threading import rclpy -from rclpy import Node +from host_comm import * +from rclpy.node import Node from std_msgs.msg import Float64, Int8, Int32, UInt8, Bool, UInt64 -from host_comm import * from nav_msgs.msg import Odometry class Translator(Node): diff --git a/rb_ws/src/buggy/buggy/simulator/engine.py b/rb_ws/src/buggy/scripts/simulator/engine.py old mode 100644 new mode 100755 similarity index 94% rename from rb_ws/src/buggy/buggy/simulator/engine.py rename to rb_ws/src/buggy/scripts/simulator/engine.py index b3cb774..af2bdc5 --- a/rb_ws/src/buggy/buggy/simulator/engine.py +++ b/rb_ws/src/buggy/scripts/simulator/engine.py @@ -1,6 +1,7 @@ #! /usr/bin/env python3 import threading import sys +import time import rclpy from rclpy.node import Node from geometry_msgs.msg import Pose, Twist, PoseWithCovariance, TwistWithCovariance @@ -9,8 +10,9 @@ from nav_msgs.msg import Odometry import numpy as np import utm -sys.path.append("/rb_ws/src/buggy/buggy") -from rb_ws.src.buggy.buggy.util.constants import Constants + +sys.path.append("/rb_ws/src/buggy/scripts") +from util.constants import Constants class Simulator(Node): @@ -130,7 +132,7 @@ def publish(self): with self.lock: p.position.x = self.e_utm p.position.y = self.n_utm - p.position.z = self.heading + p.position.z = float(self.heading) velocity = self.velocity self.plot_publisher.publish(p) @@ -176,11 +178,18 @@ def loop(self): if self.tick_count % self.interval == 0: self.publish() self.tick_count += 1 + self.get_logger().debug("SIMULATED UTM: ({}, {}), HEADING: {}".format(self.e_utm, self.n_utm, self.heading)) def main(args=None): rclpy.init(args=args) sim = Simulator() + for _ in range(500): + time.sleep(0.01) + sim.publish() + + + sim.get_logger().info("STARTED PUBLISHING") rclpy.spin(sim) sim.destroy_node() diff --git a/rb_ws/src/buggy/buggy/util/constants.py b/rb_ws/src/buggy/scripts/util/constants.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/util/constants.py rename to rb_ws/src/buggy/scripts/util/constants.py diff --git a/rb_ws/src/buggy/buggy/util/pose.py b/rb_ws/src/buggy/scripts/util/pose.py old mode 100644 new mode 100755 similarity index 100% rename from rb_ws/src/buggy/buggy/util/pose.py rename to rb_ws/src/buggy/scripts/util/pose.py diff --git a/rb_ws/src/buggy/buggy/util/trajectory.py b/rb_ws/src/buggy/scripts/util/trajectory.py old mode 100644 new mode 100755 similarity index 98% rename from rb_ws/src/buggy/buggy/util/trajectory.py rename to rb_ws/src/buggy/scripts/util/trajectory.py index 72fbcae..1bbdfb4 --- a/rb_ws/src/buggy/buggy/util/trajectory.py +++ b/rb_ws/src/buggy/scripts/util/trajectory.py @@ -1,13 +1,14 @@ import json +import time # from buggy.msg import TrajectoryMsg import numpy as np from scipy.interpolate import Akima1DInterpolator, CubicSpline +from buggy.msg import TrajectoryMsg 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 @@ -347,10 +348,10 @@ 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] + traj.easting = list(self.positions[:, 0]) + traj.northing = list(self.positions[:, 1]) traj.time = time.time() traj.cur_idx = self.get_closest_index_on_path(x,y) return traj @@ -358,5 +359,5 @@ def get_closest_index_on_path( 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/watchdog/watchdog_node.py b/rb_ws/src/buggy/scripts/watchdog/watchdog_node.py old mode 100644 new mode 100755 similarity index 98% rename from rb_ws/src/buggy/buggy/watchdog/watchdog_node.py rename to rb_ws/src/buggy/scripts/watchdog/watchdog_node.py index b0338da..7c155e3 --- a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py +++ b/rb_ws/src/buggy/scripts/watchdog/watchdog_node.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python3 + import rclpy from rclpy.node import Node diff --git a/rb_ws/src/buggy/setup.cfg b/rb_ws/src/buggy/setup.cfg deleted file mode 100644 index c2b78e0..0000000 --- a/rb_ws/src/buggy/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/buggy -[install] -install_scripts=$base/lib/buggy diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py deleted file mode 100644 index a8a022e..0000000 --- a/rb_ws/src/buggy/setup.py +++ /dev/null @@ -1,34 +0,0 @@ -from glob import glob -import os - -from setuptools import find_packages, setup - -package_name = 'buggy' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join("share", package_name), glob("launch/*.xml")) - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='root', - maintainer_email='root@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'hello_world = buggy.hello_world:main', - 'sim_single = buggy.simulator.engine:main', - 'controller = buggy.controller.controller_node:main', - 'buggy_state_converter = buggy.buggy_state_converter:main', - 'watchdog = buggy.watchdog.watchdog_node:main', - ], - }, -) \ No newline at end of file diff --git a/rb_ws/src/buggy/test/test_copyright.py b/rb_ws/src/buggy/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/rb_ws/src/buggy/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/rb_ws/src/buggy/test/test_flake8.py b/rb_ws/src/buggy/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/rb_ws/src/buggy/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/rb_ws/src/buggy/test/test_pep257.py b/rb_ws/src/buggy/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/rb_ws/src/buggy/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings'