diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index 3d5c4420..e0e39d46 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