From d7ede11de02fbad16301c4a3c0ad80104aae15f0 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 23 Oct 2024 23:37:57 -0400 Subject: [PATCH 01/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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/11] 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 86da93ce7d30b012e5861086ef337139fa2ecf4b Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 4 Dec 2024 16:54:55 -0500 Subject: [PATCH 10/11] 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 48a477e4adeccd56c78a64e489a1644406399ebb Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Sun, 15 Dec 2024 18:49:52 -0500 Subject: [PATCH 11/11] 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