From d7ede11de02fbad16301c4a3c0ad80104aae15f0 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 23 Oct 2024 23:37:57 -0400 Subject: [PATCH 01/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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/16] 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 12/16] 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 13/16] 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 14/16] 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 15/16] 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 16/16] 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)