fixed up watchdog and added to launch files

CMU-Robotics-Club · Nov 7, 2024 · 40336b9 · 40336b9
1 parent f6659b7
commit 40336b9
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 12 deletions.
diff --git a/rb_ws/src/buggy/launch/nand-system.launch b/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>
diff --git a/rb_ws/src/buggy/launch/sc-system.launch b/rb_ws/src/buggy/launch/sc-system.launch
@@ -14,5 +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" />
+    <node name="watchdog" pkg="buggy" type="watchdog.py" args = "--self_name SC" output="screen"/>
 </launch>
diff --git a/rb_ws/src/buggy/launch/watchdog.launch b/rb_ws/src/buggy/launch/watchdog.launch
@@ -2,5 +2,5 @@
 
 <launch>
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
-    <node name="watchdog" pkg="buggy" type="watchdog.py" args = "--self_name SC" />
+    <node name="watchdog" pkg="buggy" type="watchdog.py" args = "--self_name SC" output="screen"/>
 </launch>
diff --git a/rb_ws/src/buggy/scripts/watchdog/watchdog.py b/rb_ws/src/buggy/scripts/watchdog/watchdog.py
@@ -8,7 +8,7 @@
 
 class Watchdog:
 
-    STEERING_DEVIANCE = 2 #deg
+    STEERING_DEVIANCE = 4 #deg
 
     def __init__(self, self_name) -> None:
         self.alarm = 0 #Check this alarm value
@@ -28,17 +28,21 @@ def __init__(self, self_name) -> None:
             self_name + "/buggy/debug/steering_angle", Float64, self.check_stepper_steering
         )
         rospy.Subscriber(
-            self_name + "/buggy/debug/use_auton_steer", Bool, self.check_stepper_steering
+            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.loginfo("Got input steering of, "  + str(msg.data))
+        rospy.logdebug("Got input steering of: "  + str(msg.data))
         self.commanded_steering = msg.data
 
-    def set_auton_steer(self, msg):        
+    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.inAutonSteer = msg.data
 
     def check_stepper_steering(self, msg):
@@ -47,19 +51,19 @@ def check_stepper_steering(self, msg):
         if abs(stepper_steer - self.commanded_steering) > Watchdog.STEERING_DEVIANCE:
             self.alarm = 2 # ERROR
             if self.inAutonSteer:
-                rospy.logerror("STEPPER DEVIANCE of " +  str(abs(stepper_steer - self.commanded_steering)))
+                rospy.logerr("STEPPER DEVIANCE (DEGREES OFF): " +  str(abs(stepper_steer - self.commanded_steering)))
             else:
-                rospy.logdebug("Stepper Deviance of " + str((stepper_steer - self.commanded_steering)) + " but not in auton steer")
+                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: " + str(abs(stepper_steer - self.commanded_steering)))
+                rospy.logwarn("STEPPER POSSIBILY DEVIATING (DEGREES OFF):  " + str(abs(stepper_steer - self.commanded_steering)))
+                self.alarm = max(self.alarm, 1)
             else:
-                rospy.logdebug("Not in auton, but stepper possibly deviating " + str(abs(stepper_steer - self.commanded_steering)))
+                rospy.logdebug("(Non Auton) Stepper possibly deviating: " + str(abs(stepper_steer - self.commanded_steering)))
 
     def loop(self):
         while True:
-            print("HERE")
             #publish alarm
             ros_alarm = Int8()
             ros_alarm.data = self.alarm
@@ -75,7 +79,6 @@ def loop(self):
     args, _ = parser.parse_known_args()
     self_name = args.self_name
     rospy.init_node("watchdog")
-    print("INITIALIZED")
     rospy.loginfo("INITIALIZED WATCHDOG NODE")
     watchdog = Watchdog(self_name=self_name)
     watchdog.loop()