From d7ede11de02fbad16301c4a3c0ad80104aae15f0 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 23 Oct 2024 23:37:57 -0400 Subject: [PATCH 01/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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/18] 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 a537b8839b41d717152c49a700542ee7c117297b Mon Sep 17 00:00:00 2001 From: TiaSinghania Date: Mon, 23 Dec 2024 22:16:24 -0800 Subject: [PATCH 17/18] 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 18/18] 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)