From b1a4eca5d01b8379344c217be0c8da2e1e68bd3b Mon Sep 17 00:00:00 2001
From: Mehul Goel <mehulgoel873@gmail.com>
Date: Sat, 16 Mar 2024 16:58:46 -0400
Subject: [PATCH] Debug publisher

---
 rb_ws/src/buggy/scripts/serial/host_comm.py   |  2 +-
 .../buggy/scripts/serial/ros_to_bnyahaj.py    | 28 ++++++++++++++++++-
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/serial/host_comm.py b/rb_ws/src/buggy/scripts/serial/host_comm.py
index 1568d04c..f0c8536a 100644
--- a/rb_ws/src/buggy/scripts/serial/host_comm.py
+++ b/rb_ws/src/buggy/scripts/serial/host_comm.py
@@ -175,7 +175,7 @@ def read_packet(self):
             # Debug message
             debug = struct.unpack('<fff??B?BBxx', payload)
             # print(debug)
-            return None
+            return debug
         else:
             return None
             # print(f'Unknown packet type {msg_type}')
diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
index 691b1550..88dde73d 100755
--- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
+++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
@@ -28,6 +28,18 @@ def __init__(self, self_name, other_name):
         self.steer_send_rate = rospy.Rate(100)
         self.read_rate = rospy.Rate(1000)
 
+        self.rc_steering_angle_publisher = rospy.Publisher(self_name + "/buggy/debug/rc_steering_angle", Float64, queue_size=1)
+        self.steering_angle_publisher = rospy.Publisher(self_name + "/buggy/debug/steering_angle", Float64, queue_size=1)
+        self.battery_voltage_publisher = rospy.Publisher(self_name + "/buggy/debug/battery_voltage", Float64, queue_size=1)
+        self.operator_ready_publisher = rospy.Publisher(self_name + "/buggy/debug/operator_ready", Bool, queue_size=1)
+        self.steering_alarm_publisher = rospy.Publisher(self_name + "/buggy/debug/steering_angle", Bool, queue_size=1)
+        self.brake_status_publisher = rospy.Publisher(self_name + "/buggy/debug/brake_status", Bool, queue_size=1)
+        self.use_auton_steer_publisher = rospy.Publisher(self_name + "/buggy/debug/use_auton_steer", Bool, queue_size=1)
+        self.rc_uplink_qual_publisher = rospy.Publisher(self_name + "/buggy/debug/rc_uplink_quality", Int8, queue_size=1)
+        self.nand_fix_publisher = rospy.Publisher(self_name + "/buggy/debug/nand_fix", Int8, queue_size=1)
+        self.padding_1_publisher = rospy.Publisher(self_name + "/buggy/debug/padding_1", Int8, queue_size=1)
+        self.padding_2_publisher = rospy.Publisher(self_name + "/buggy/debug/padding_2", Int8, queue_size=1)
+
     #Steering Angle Updater
     def set_steering(self, msg):
         #print("Steering angle: " + str(msg.data))
@@ -48,7 +60,7 @@ def reader_thread(self):
             packet = self.comms.read_packet()
 
             # print("trying to read odom")
-            if packet is not None:
+            if isinstance(packet, Odometry):
                 # print("packet", packet.x, packet.y)
                 #Publish to odom topic x and y coord
                 odom = ROSOdom()
@@ -58,6 +70,20 @@ def reader_thread(self):
                 odom.pose.pose.position.y = lat
 
                 self.odom_publisher.publish(odom)
+            elif isinstance(packet, tuple): #Are there any other packet that is a tuple
+                print(packet)
+                self.rc_steering_angle_publisher.publish(packet[0])
+                self.steering_angle_publisher.publish(packet[1])
+                self.battery_voltage_publisher.publish(packet[2])
+                self.operator_ready_publisher.publish(packet[3])
+                self.steering_alarm_publisher.publish(packet[4])
+                self.brake_status_publisher.publish(packet[5])
+                self.use_auton_steer_publisher.publish(packet[6])
+                self.rc_uplink_qual_publisher.publish(packet[7])
+                self.nand_fix_publisher.publish(packet[8])
+                self.padding_1_publisher.publish(packet[9][0])
+                self.padding_2_publisher.publish(packet[9][1])
+
 
             # # for debug
             # odom = ROSOdom()