diff --git a/python-requirements.txt b/python-requirements.txt index 66b300e..d75cc06 100644 --- a/python-requirements.txt +++ b/python-requirements.txt @@ -5,6 +5,7 @@ numpy osqp pandas pymap3d +pyproj pyserial scipy setuptools==58.2.0 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..48b1086 --- /dev/null +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +import numpy as np +import pyproj +from scipy.spatial.transform import Rotation + +class BuggyStateConverter(Node): + def __init__(self): + super().__init__("buggy_state_converter") + + 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) + + # Check if namespace is "SC" to add an "other" subscriber/publisher + if namespace == "/SC": + self.other_raw_state_subscriber = self.create_subscription( + Odometry, "other/raw_state", self.other_raw_state_callback, 10 + ) + self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10) + + # Initialize pyproj Transformer for ECEF -> UTM conversion for /SC + self.ecef_to_utm_transformer = pyproj.Transformer.from_crs( + "epsg:4978", "epsg:32617", always_xy=True + ) # TODO: Confirm UTM EPSG code, using EPSG:32617 for UTM Zone 17N + + def self_raw_state_callback(self, msg): + """ Callback for processing self/raw_state messages and publishing to self/state """ + namespace = self.get_namespace() + + if namespace == "/SC": + converted_msg = self.convert_SC_state(msg) + elif namespace == "/NAND": + converted_msg = self.convert_NAND_state(msg) + else: + self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}") + return + + 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_NAND_other_state(msg) + self.other_state_publisher.publish(converted_msg) + + def convert_SC_state(self, msg): + """ + Converts self/raw_state in SC namespace to clean state units and structure + + Takes in ROS message in nav_msgs/Odometry format + Assumes that the SC namespace is using ECEF coordinates and quaternion orientation + """ + + 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 + 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 = 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 + + # 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 + 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) ---- + 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.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 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 = msg.pose.pose.position.z # UTM Altitude + + # ---- 2. Orientation in Radians ---- + converted_msg.pose.pose.orientation.x = msg.pose.pose.orientation.x + 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) ---- + converted_msg.pose.covariance = msg.pose.covariance + converted_msg.twist.covariance = msg.twist.covariance + + # ---- 4. Linear Velocities in m/s ---- + # TODO: Check if scalar velocity is coming in from msg.twist.twist.linear.x + # Convert scalar speed to velocity x/y components using heading (orientation.z) + speed = msg.twist.twist.linear.x # m/s scalar velocity + heading = msg.pose.pose.orientation.z # heading in radians + + # 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 # 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, defaults to 0.0) + + # ---- 2. Orientation in Radians ---- + 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 + + # ---- 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 + + # ---- 5. Angular Velocities ---- + 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 + + +def main(args=None): + rclpy.init(args=args) + + # Create the BuggyStateConverter node and spin it + state_converter = BuggyStateConverter() + rclpy.spin(state_converter) + + # Shutdown when done + state_converter.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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 @@ + + diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index 42318d0..08a05de 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -26,7 +26,8 @@ 'console_scripts': [ 'hello_world = buggy.hello_world:main', 'sim_single = buggy.simulator.engine:main', - 'watchdog = buggy.watchdog.watchdog_node:main' + 'buggy_state_converter = buggy.buggy_state_converter:main', + 'watchdog = buggy.watchdog.watchdog_node:main', ], }, ) \ No newline at end of file