added NAND-self and SC-other functionality

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