added debugs, allowed clean shutdown, and created proper alarms

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/watchdog/watchdog.py

@@ -43,27 +43,30 @@ def set_auton_steer(self, msg):
             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:
-            self.alarm = 2 # ERROR
             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:
-                rospy.logwarn("STEPPER POSSIBILY DEVIATING (DEGREES OFF):  " + str(abs(stepper_steer - self.commanded_steering)))
                 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 True:
+        while not rospy.is_shutdown():
             #publish alarm
             ros_alarm = Int8()
             ros_alarm.data = self.alarm