serial port with updated packet structure

rb_ws/src/buggy/buggy/buggy_state_converter.py

@@ -12,47 +12,51 @@ def __init__(self):
         super().__init__("buggy_state_converter")
 
         namespace = self.get_namespace()
+        if namespace == "/SC":
+            self.SC_raw_state_subscriber = self.create_subscription(
+                Odometry, "/raw_state", self.convert_SC_state_callback, 10
+            )
 
-        # 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)
+            self.NAND_other_raw_state_subscriber = self.create_subscription(
+                Odometry, "/NAND_raw_state", self.convert_NAND_other_state_callback, 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)
+
+        elif namespace == "/NAND":
+            self.NAND_raw_state_subscriber = self.create_subscription(
+                Odometry, "/raw_state", self.convert_NAND_state_callback, 10
             )
-            self.other_state_publisher = self.create_publisher(Odometry, "other/state", 10)
+
+        else:
+            self.get_logger().warn(f"Namespace not recognized for buggy state conversion: {namespace}")
+
+        self.self_state_publisher = self.create_publisher(Odometry, "/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
+    def convert_SC_state_callback(self, msg):
+        """ Callback for processing SC/raw_state messages and publishing to self/state """
+        converted_msg = self.convert_SC_state(msg)
+        self.self_state_publisher.publish(converted_msg)
 
+    def convert_NAND_state_callback(self, msg):
+        """ Callback for processing NAND/raw_state messages and publishing to self/state """
+        converted_msg = self.convert_NAND_state(msg)
         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
+
+    def convert_NAND_other_state_callback(self, msg):
+        """ Callback for processing SC/NAND_raw_state messages and publishing 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
@@ -127,7 +131,6 @@ def convert_NAND_state(self, msg):
         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

rb_ws/src/buggy/buggy/serial/ros_to_bnyahaj.py

@@ -4,7 +4,7 @@
 import rclpy
 from rclpy import Node
 
-from std_msgs.msg import Float64, Int8, Int32, UInt8, Bool
+from std_msgs.msg import Float64, Int8, Int32, UInt8, Bool, UInt64
 from host_comm import *
 from nav_msgs.msg import Odometry
 
@@ -26,7 +26,7 @@ def __init__(self, teensy_name):
 
         self.comms = Comms("/dev/" + teensy_name)
         namespace = self.get_namespace()
-        if namespace == "\SC":
+        if namespace == "/SC":
             self.self_name = "SC"
         else:
             self.self_name = "NAND"
@@ -85,36 +85,46 @@ def __init__(self, teensy_name):
             UInt8, self_name + "/debug/rc_uplink_quality", 1
         )
 
-        # NAND POSITION PUBLISHERS
-        self.nand_ukf_odom_publisher = self.create_publisher(
-            Odometry, "NAND/nav/odom", 1
-        )
-        self.nand_gps_odom_publisher = self.create_publisher(
-            Odometry, "NAND/debug/gps_odom", 1
-        )
-        self.observed_nand_odom_publisher = self.create_publisher(
-            Odometry, "SC/nav/NAND_odom", 1
-        )
-        self.nand_gps_fix_publisher = self.create_publisher(
-            UInt8, "NAND/debug/gps_fix", 1
-        )
-        self.nand_gps_acc_publisher = self.create_publisher(
-            Float64, "NAND/debug/gps_accuracy", 1
-        )
-
-        # SC SENSOR PUBLISHERS
-        self.sc_velocity_publisher = self.create_publisher(
-            Float64, "SC/sensors/velocity", 1
-        )
-        self.sc_steering_angle_publisher = self.create_publisher(
-            Float64, "SC/sensors/steering_angle", 1
-        )
-
         # SERIAL DEBUG PUBLISHERS
         self.roundtrip_time_publisher = self.create_publisher(
             Float64, self_name + "/debug/roundtrip_time", 1
         )
 
+        if self.self_name == "NAND":
+            # NAND POSITION PUBLISHERS
+            self.nand_ukf_odom_publisher = self.create_publisher(
+                Odometry, "/raw_state", 1
+            )
+            self.nand_gps_odom_publisher = self.create_publisher(
+                Odometry, "/debug/gps_odom", 1
+            )
+
+            self.nand_gps_fix_publisher = self.create_publisher(
+                UInt8, "/debug/gps_fix", 1
+            )
+            self.nand_gps_acc_publisher = self.create_publisher(
+                Float64, "/debug/gps_accuracy", 1
+            )
+            self.nand_gps_seqnum_publisher = self.create_publisher(
+                Int32, "/debug/gps_seqnum", 1
+            )
+            self.nand_gps_time_publisher = self.create_publisher(
+                UInt64, "/debug/gps_time", 1
+            )
+
+        if self.self_name == "SC":
+            # SC SENSOR PUBLISHERS
+            self.sc_velocity_publisher = self.create_publisher(
+                Float64, "SC/sensors/velocity", 1
+            )
+            self.sc_steering_angle_publisher = self.create_publisher(
+                Float64, "SC/sensors/steering_angle", 1
+            )
+
+            self.observed_nand_odom_publisher = self.create_publisher(
+                    Odometry, "/NAND_raw_state", 1
+                )
+
 
     def set_alarm(self, msg):
         """
@@ -129,8 +139,6 @@ def set_steering(self, msg):
         Steering Angle Updater, updates the steering angle locally if updated on ros stopic
         """
         self.get_logger().debug(f"Read steering angle of: {msg.data}")
-        # print("Steering angle: " + str(msg.data))
-        # print("SET STEERING: " + str(msg.data))
         with self.lock:
             self.steer_angle = msg.data
             self.fresh_steer = True
@@ -176,24 +184,22 @@ def reader_thread(self):
                 self.tx12_state_publisher.publish(packet.tx12_state)
                 self.stepper_alarm_publisher.publish(packet.stepper_alarm)
                 self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality)
-
+                self.get_logger().debug(f'NAND Debug Timestamp: {packet.timestamp}')
             elif isinstance(packet, NANDUKF):
                 odom = Odometry()
-
-                # NOTE: this data is published as an Odometry object per the BuggyState specs: https://github.com/CMU-Robotics-Club/robobuggy-software/wiki/BuggyState#buggystate-definition-as-a-navodom-ros-msg
                 odom.pose.pose.position.x = packet.easting
                 odom.pose.pose.position.y = packet.northing
                 odom.pose.pose.orientation.z = packet.theta
 
-                # why this works: autonsystem only cares about magnitude of velocity so setting an arbitrary axis to the speed and leave other at 0 works.
                 odom.twist.twist.linear.x = packet.velocity
                 odom.twist.twist.angular.z = packet.heading_rate
 
                 self.nand_ukf_odom_publisher.publish(odom)
+                self.get_logger().debug(f'NAND UKF Timestamp: {packet.timestamp}')
+
 
             elif isinstance(packet, NANDRawGPS):
                 odom = Odometry()
-                # NOTE: this data is published as an Odometry object per the BuggyState specs: https://github.com/CMU-Robotics-Club/robobuggy-software/wiki/BuggyState#buggystate-definition-as-a-navodom-ros-msg
                 odom.pose.pose.position.x = packet.easting
                 odom.pose.pose.position.y = packet.northing
                 odom.pose.pose.orientation.z = 0
@@ -204,7 +210,12 @@ def reader_thread(self):
                 self.nand_gps_odom_publisher.publish(odom)
                 self.nand_gps_fix_publisher.publish(packet.gps_fix)
                 self.nand_gps_acc_publisher.publish(packet.accuracy)
+                self.nand_gps_seqnum_publisher.publish(packet.gps_seqnum)
+                self.nand_gps_time_publisher.publish(packet.gps_time)
+                self.get_logger().debug(f'NAND Raw GPS Timestamp: {packet.timestamp}')
+
 
+            # this packet is received on Short Circuit
             elif isinstance(packet, Radio):
 
                 # Publish to odom topic x and y coord
@@ -213,6 +224,9 @@ def reader_thread(self):
                 odom.pose.pose.position.x = packet.nand_east_gps
                 odom.pose.pose.position.y = packet.nand_north_gps
                 self.observed_nand_odom_publisher.publish(odom)
+                self.get_logger().debug(f'Radio Received NAND GPS Seqnum: {packet.gps_seqnum}')
+
+
 
             elif isinstance(packet, SCDebugInfo):
                 self.encoder_angle_publisher.publish(packet.encoder_angle)
@@ -225,10 +239,14 @@ def reader_thread(self):
                 self.tx12_state_publisher.publish(packet.tx12_state)
                 self.stepper_alarm_publisher.publish(packet.stepper_alarm)
                 self.rc_uplink_qual_publisher.publish(packet.rc_uplink_quality)
+                self.get_logger().debug(f'SC Debug Timestamp: {packet.timestamp}')
+
 
             elif isinstance(packet, SCSensors):
                 self.sc_velocity_publisher.publish(packet.velocity)
                 self.sc_steering_angle_publisher.publish(packet.steering_angle)
+                self.get_logger().debug(f'SC Sensors Timestamp: {packet.timestamp}')
+
 
             elif isinstance(packet, RoundtripTimestamp):
                 self.roundtrip_time_publisher.publish(time.time() - packet.returned_time)