Input/Debug Steering Deviation Sanity Alarm (#113)

* added deviation sanity check

* finish watchdog

* fixed up watchdog and added to launch files

* Revert "added deviation sanity check"

This reverts commit 355cd2b.

* pylint

* updated telematics fix publisher

* added debugs, allowed clean shutdown, and created proper alarms

---------

Co-authored-by: Mehul Goel <[email protected]>
Co-authored-by: Buggy <[email protected]>

rb_ws/src/buggy/launch/nand-system.launch

@@ -6,4 +6,5 @@
 
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="telematics" pkg="buggy" type="telematics.py" />
+    <node name="watchdog" pkg="buggy" type="watchdog.py" args = "--self_name NAND" output="screen"/>
 </launch>

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

@@ -14,4 +14,5 @@
 
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
     <node name="telematics" pkg="buggy" type="telematics.py" />
+    <node name="watchdog" pkg="buggy" type="watchdog.py" args = "--self_name SC" output="screen"/>
 </launch>

rb_ws/src/buggy/launch/watchdog.launch

@@ -0,0 +1,6 @@
+<!-- roslaunch buggy main.launch -->
+
+<launch>
+    <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
+    <node name="watchdog" pkg="buggy" type="watchdog.py" args = "--self_name SC" output="screen"/>
+</launch>

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

@@ -100,13 +100,14 @@ def set_alarm(self, msg):
         alarm ros topic reader, locked so that only one of the setters runs at once
         """
         with self.lock:
+            rospy.logdebug(f"Reading alarm of {msg.data}")
             self.alarm = msg.data
 
     def set_steering(self, msg):
         """
         Steering Angle Updater, updates the steering angle locally if updated on ros stopic
         """
-        rospy.loginfo(f"Read steeering angle of: {msg.data}")
+        rospy.logdebug(f"Read steeering angle of: {msg.data}")
         # print("Steering angle: " + str(msg.data))
         # print("SET STEERING: " + str(msg.data))
         with self.lock:
@@ -120,7 +121,7 @@ def writer_thread(self):
         TODO: Does alarm node exist for NAND?
         """
         rospy.loginfo("Starting sending alarm and steering to teensy!")
-        while True:
+        while not rospy.is_shutdown():
             if self.fresh_steer:
                 with self.lock:
                     self.comms.send_steering(self.steer_angle)
@@ -139,7 +140,7 @@ def reader_thread(self):
         tuple -> (SC, maybe NAND?) Debug Info
         """
         rospy.loginfo("Starting reading odom from teensy!")
-        while True:
+        while not rospy.is_shutdown():
             packet = self.comms.read_packet()
 
             if isinstance(packet, Odometry):

rb_ws/src/buggy/scripts/visualization/telematics.py

@@ -97,8 +97,8 @@ def republish_fixinfo(self, msg, publishers):
         else:
             fix_string += "FIX_RTK_FIXED"
 
-        fix_string += "\nsbas_used: "  + str(msg.sbas_used)
-        fix_string += "\ndngss_used: " + str(msg.dngss_used)
+        # fix_string += "\nsbas_used: "  + str(msg.sbas_used)
+        # fix_string += "\ndngss_used: " + str(msg.dngss_used)
         publishers[0].publish(fix_string)
         publishers[1].publish(fix_type)
 

rb_ws/src/buggy/scripts/watchdog/watchdog.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+
+import argparse
+
+import rospy
+
+from std_msgs.msg import Bool, Int8, Float64
+
+class Watchdog:
+
+    STEERING_DEVIANCE = 4 #deg
+
+    def __init__(self, self_name) -> None:
+        self.alarm = 0 #Check this alarm value
+        """
+        0 - OK
+        1 - WARNING
+        2 - ERROR        
+        """
+
+        self.commanded_steering = 0
+        self.inAutonSteer = False
+
+        rospy.Subscriber(
+            self_name + "/buggy/input/steering", Float64, self.set_input_steering
+        )
+        rospy.Subscriber(
+            self_name + "/buggy/debug/steering_angle", Float64, self.check_stepper_steering
+        )
+        rospy.Subscriber(
+            self_name + "/buggy/debug/use_auton_steer", Bool, self.set_auton_steer
+        )
+
+        self.alarm_publisher = rospy.Publisher(self_name + "/debug/sanity_warning", Int8, queue_size=1)
+        self.alarm_publish_rate = rospy.Rate(100) #10ms per alarm
+
+    def set_input_steering(self, msg):
+        rospy.logdebug("Got input steering of: "  + str(msg.data))
+        self.commanded_steering = msg.data
+
+    def set_auton_steer(self, msg):
+        if (msg.data and not self.inAutonSteer):
+            rospy.loginfo("ENTERED AUTON")
+        if (not msg.data and self.inAutonSteer):
+            rospy.logwarn("EXITED AUTON")
+            self.alarm = 0 #No alarm if not in auton
+        self.inAutonSteer = msg.data
+
+    def check_stepper_steering(self, msg):
+        stepper_steer = msg.data
+        rospy.logdebug("Firmware's reported stepper degree: " + str(stepper_steer))
+        if (self.alarm < 2):
+            self.alarm = 0
+        if abs(stepper_steer - self.commanded_steering) > Watchdog.STEERING_DEVIANCE:
+            if self.inAutonSteer:
+                self.alarm = 2 # ERROR
+                rospy.logerr("STEPPER DEVIANCE (DEGREES OFF): " +  str(abs(stepper_steer - self.commanded_steering)))
+            else:
+                rospy.logdebug("(Non Auton) Stepper Deviance of: " + str(abs(stepper_steer - self.commanded_steering)))
+
+        elif abs(stepper_steer - self.commanded_steering) > Watchdog.STEERING_DEVIANCE//2:
+            if self.inAutonSteer:
+                self.alarm = max(self.alarm, 1)
+                rospy.logwarn("STEPPER POSSIBILY DEVIATING (DEGREES OFF):  " + str(abs(stepper_steer - self.commanded_steering)))
+            else:
+                rospy.logdebug("(Non Auton) Stepper possibly deviating: " + str(abs(stepper_steer - self.commanded_steering)))
+
+    def loop(self):
+        while not rospy.is_shutdown():
+            #publish alarm
+            ros_alarm = Int8()
+            ros_alarm.data = self.alarm
+            self.alarm_publisher.publish(ros_alarm)
+
+            self.alarm_publish_rate.sleep()
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--self_name", type=str, help="name of ego-buggy", required=True
+    )
+    args, _ = parser.parse_known_args()
+    self_name = args.self_name
+    rospy.init_node("watchdog")
+    rospy.loginfo("INITIALIZED WATCHDOG NODE")
+    watchdog = Watchdog(self_name=self_name)
+    watchdog.loop()