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