Alarm serial (#76)

* send alarm node to serial

* fixed teensy name passing
added left curb to sc launch

* increased send rate to teensy,
added seqnum to odom

* fixed odometry dataclass

---------

Co-authored-by: Buggy <buggy@buggy.com>

rb_ws/src/buggy/launch/sc-main.launch

@@ -3,21 +3,22 @@
 <launch>
     <arg name="controller"      default="mpc" />
     <arg name="start_dist"      default="0.0" />
-    <arg name="path"      default="frew_test.json" />
+    <arg name="path"      default="buggycourse_safe_1.json" />
 
     <remap from="/SC/nav/odom" to="/nav/odom"/>
+    <node name="sc_rolling_sanity_check" pkg="buggy" type="rolling_sanity_check.py" output="screen" args="SC"/>
 
     <!-- ENABLE AUTON -->
     <!-- autonsystem args: controller start_dist path buggy_name is_sim -->
     <!-- Conditional Launch Files, depending on if NAND Exists or not -->
     <arg name="NAND_exist" default="false"/>
     <group if="$(arg NAND_exist)">
         <!-- Run the simulation with NAND -->
-        <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND" />
+        <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --other_name NAND --left_curb curb.json" />
         <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
     </group>
     <group unless="$(arg NAND_exist)">
-        <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC" />
+        <arg name="autonsystem_args" value="--controller $(arg controller) --dist 0.0 --traj $(arg path) --self_name SC --left_curb curb.json" />
         <node name="auton_system" pkg="buggy" type="autonsystem.py" output="screen" args="$(arg autonsystem_args)"/>
     </group>
 </launch>

rb_ws/src/buggy/scripts/serial/host_comm.py

@@ -68,6 +68,8 @@ def update(self, b):
 class Odometry:
     x: float
     y: float
+    radio_seqnum: int
+    gps_seqnum: int
 
 class IncompletePacket(Exception):
     pass
@@ -174,8 +176,8 @@ def read_packet(self):
         msg_type, payload = packet
         if msg_type == MSG_TYPE_ODOMETRY:
             # Odometry message
-            x, y = struct.unpack('<dd', payload)
-            return Odometry(x, y)
+            x, y, radio_seqnum, gps_seqnum = struct.unpack('<ddII', payload)
+            return Odometry(x, y, radio_seqnum, gps_seqnum)
         elif msg_type == MSG_TYPE_DEBUG:
             # Debug message
             debug = struct.unpack('<fff??B?BBxx', payload)

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

@@ -10,7 +10,7 @@
 import rospy
 
 #Ros Message Imports
-from std_msgs.msg import Float64, Bool, UInt8
+from std_msgs.msg import Float64, Bool, Int8, UInt8
 from nav_msgs.msg import Odometry as ROSOdom
 
 from host_comm import *
@@ -21,12 +21,16 @@ class Translator:
     def __init__(self, self_name, other_name, teensy_name):
         self.comms = Comms("/dev/" + teensy_name)
         self.steer_angle = 0
+        self.alarm = 0
         self.fresh_steer = False
         self.lock = Lock()
 
         rospy.Subscriber(self_name + "/buggy/input/steering", Float64, self.set_steering)
+        rospy.Subscriber(self_name + "/debug/sanity_warning", Int8, self.set_alarm)
+
         self.odom_publisher = rospy.Publisher(other_name + "/nav/odom", ROSOdom, queue_size=1)
-        self.steer_send_rate = rospy.Rate(100)
+        # upper bound of steering update rate, make sure auton sends slower than this
+        self.steer_send_rate = rospy.Rate(500)
         self.read_rate = rospy.Rate(1000)
 
         # DOES NAND GET ALL THIS DEBUG INFORMATION???
@@ -40,6 +44,10 @@ def __init__(self, self_name, other_name, teensy_name):
         self.rc_uplink_qual_publisher = rospy.Publisher(self_name + "/buggy/debug/rc_uplink_quality", UInt8, queue_size=1)
         self.nand_fix_publisher = rospy.Publisher(self_name + "/buggy/debug/nand_fix", UInt8, queue_size=1)
 
+    def set_alarm(self, msg):
+        with self.lock:
+            self.alarm = msg.data
+
     #Steering Angle Updater
     def set_steering(self, msg):
         #print("Steering angle: " + str(msg.data))
@@ -49,22 +57,25 @@ def set_steering(self, msg):
             self.fresh_steer = True
 
     def writer_thread(self):
-        print('Starting packet reading!')
+        print('Starting sending alarm and steering to teensy!')
         while True:
             if self.fresh_steer:
                 with self.lock:
                     self.comms.send_steering(self.steer_angle)
                     self.fresh_steer = False
+
+            with self.lock:
+                self.comms.send_alarm(self.alarm)
+
             self.steer_send_rate.sleep()
 
     def reader_thread(self):
-        print('Starting packet sending!')
+        print('Starting reading odom from teensy!')
         while True:
             packet = self.comms.read_packet()
-
             # print("trying to read odom")
             if isinstance(packet, Odometry):
-                # print("packet", packet.x, packet.y)
+                # print("packet", packet.radio_seqnum, packet.gps_seqnum)
                 #Publish to odom topic x and y coord
                 odom = ROSOdom()
                 # convert to long lat
@@ -111,5 +122,5 @@ def loop(self):
     teensy_name = args.teensy_name
 
     rospy.init_node("ros_bnyahaj")
-    translate = Translator(self_name, other_name)
+    translate = Translator(self_name, other_name, teensy_name)
     translate.loop()