mock rolled ros2bnyahaj reading/writing

Co-authored-by: TiaSinghania <[email protected]> Co-authored-by: Gyanepsaa Singh <[email protected]> Co-authored-by: Tucker Shea <[email protected]> Co-authored-by: Alden-G878 <[email protected]> Co-authored-by: garrison2 <[email protected]> Co-authored-by: delaynie <[email protected]>
CMU-Robotics-Club · Jan 23, 2025 · 16542ba · 16542ba
1 parent cd50861
commit 16542ba
Show file tree

Hide file tree

Showing 8 changed files with 387 additions and 318 deletions.
diff --git a/rb_ws/src/buggy/CMakeLists.txt b/rb_ws/src/buggy/CMakeLists.txt
@@ -33,7 +33,8 @@ install(PROGRAMS
   scripts/simulator/engine.py
   scripts/watchdog/watchdog_node.py
   scripts/buggy_state_converter.py
-  scripts/serial/ros_to_bnyahaj.py
+  scripts/serial/ros_to_bnyahaj_reader.py
+  scripts/serial/ros_to_bnyahaj_writer.py
   scripts/visualization/telematics.py
   DESTINATION lib/${PROJECT_NAME}
 )

diff --git a/rb_ws/src/buggy/launch/debug_steer.xml b/rb_ws/src/buggy/launch/debug_steer.xml
@@ -1,4 +1,6 @@
 <launch>
     <node pkg="foxglove_bridge" exec="foxglove_bridge" name="foxglove" namespace="SC"/>
     <node name="debug_steer" pkg="buggy" exec="debug_steer.py" output="screen"/>
+    <node name="bnyahaj_writer" pkg="buggy" exec="ros_to_bnyahaj_writer.py" output="screen" respawn="true"  args="--self_name SC --other_name NAND --teensy_name ttyUSB0" namespace="SC"/>
+    <node name="bnyahaj_reader" pkg="buggy" exec="ros_to_bnyahaj_reader.py" output="screen" respawn="true"  args="--self_name SC --other_name NAND --teensy_name ttyUSB0" namespace="SC"/>
 </launch>
diff --git a/rb_ws/src/buggy/msg/SCDebugInfoMsg.msg b/rb_ws/src/buggy/msg/SCDebugInfoMsg.msg
@@ -0,0 +1,10 @@
+float64 encoder_angle
+float64 rc_steering_angle
+float64 software_steering_angle
+float64 true_steering_angle
+bool operator_ready
+bool brake_status
+bool use_auton_steer
+bool tx12_state
+uint8 stepper_alarm
+uint8 rc_uplink_qual
diff --git a/rb_ws/src/buggy/scripts/debug/debug_steer.py b/rb_ws/src/buggy/scripts/debug/debug_steer.py
@@ -2,6 +2,7 @@
 import argparse
 
 import rclpy
+from rclpy.node import Node
 from std_msgs.msg import Float64
 import numpy as np
 
@@ -41,6 +42,8 @@ def loop(self):
         self.steer_cmd = self.sin_steer(self.tick_count)
         msg = Float64()
         msg.data = self.steer_cmd
+        # if self.tick_count % 10 == 0:
+            # self.get_logger().info(f"SIN STEER: {self.steer_cmd}")
         self.steer_publisher.publish(msg)
 
         self.tick_count += 1

diff --git a/rb_ws/src/buggy/scripts/serial/host_comm.py b/rb_ws/src/buggy/scripts/serial/host_comm.py
@@ -1,3 +1,5 @@
+import sys
+
 import struct
 import time
 from dataclasses import dataclass
@@ -165,7 +167,7 @@ class ChecksumMismatch(Exception):
 
 class Comms:
     def __init__(self, path_to_port):
-        self.port = Serial(path_to_port, 1_000_000)
+        self.port = Serial(path_to_port, 1_000_000, exclusive=True)
         self.rx_buffer = b''
 
     def send_packet_raw(self, msg_type: bytes, payload: bytes):
@@ -263,23 +265,23 @@ def read_packet(self):
 
         msg_type, payload = packet
         if msg_type == MSG_TYPE_NAND_DEBUG:
-            data = struct.unpack('<ddIfffI????BB', payload)
+            data = struct.unpack('<ddIfffI????BBxxxx', payload)
             return NANDDebugInfo(*data)
 
         elif msg_type == MSG_TYPE_NAND_UKF:
-            data = struct.unpack('<dddddI', payload)
+            data = struct.unpack('<dddddIxxxx', payload)
             return NANDUKF(*data)
 
         elif msg_type == MSG_TYPE_NAND_GPS:
-            data = struct.unpack('<dddQIIB', payload)
+            data = struct.unpack('<dddQIIBxxxxxxx', payload)
             return NANDRawGPS(*data)
 
         elif msg_type == MSG_TYPE_RADIO:
-            data = struct.unpack('<ffIB', payload)
+            data = struct.unpack('<ffIBxxx', payload)
             return Radio(*data)
 
         elif msg_type == MSG_TYPE_SC_DEBUG:
-            data = struct.unpack('<dfffII??B??B', payload)
+            data = struct.unpack('<dfffII??B??Bxxxxxx', payload)
             return SCDebugInfo(*data)
 
         elif msg_type == MSG_TYPE_SC_SENSORS:
@@ -288,7 +290,7 @@ def read_packet(self):
 
         elif msg_type == MSG_TYPE_ROUNDTRIP_TIMESTAMP:
             time = struct.unpack('<d', payload)
-            return RoundtripTimestamp(time)
+            return RoundtripTimestamp(*time)
         else:
             print(f'Unknown packet type {msg_type}')
             return None