diff --git a/rb_ws/src/buggy/launch/nand-system.launch b/rb_ws/src/buggy/launch/nand-system.launch
index 21416a5..f9a3f3d 100644
--- a/rb_ws/src/buggy/launch/nand-system.launch
+++ b/rb_ws/src/buggy/launch/nand-system.launch
@@ -6,4 +6,5 @@
+
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/sc-system.launch b/rb_ws/src/buggy/launch/sc-system.launch
index 3720e8a..7a458ef 100644
--- a/rb_ws/src/buggy/launch/sc-system.launch
+++ b/rb_ws/src/buggy/launch/sc-system.launch
@@ -14,5 +14,5 @@
-
+
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/watchdog.launch b/rb_ws/src/buggy/launch/watchdog.launch
index d7547d0..a230231 100644
--- a/rb_ws/src/buggy/launch/watchdog.launch
+++ b/rb_ws/src/buggy/launch/watchdog.launch
@@ -2,5 +2,5 @@
-
+
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/watchdog/watchdog.py b/rb_ws/src/buggy/scripts/watchdog/watchdog.py
index a02b3d1..a87d728 100755
--- 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()