From 40336b908643f8b071e91342b5ffafe6f24766e7 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Thu, 7 Nov 2024 00:18:25 -0500 Subject: [PATCH] fixed up watchdog and added to launch files --- rb_ws/src/buggy/launch/nand-system.launch | 1 + rb_ws/src/buggy/launch/sc-system.launch | 2 +- rb_ws/src/buggy/launch/watchdog.launch | 2 +- rb_ws/src/buggy/scripts/watchdog/watchdog.py | 23 +++++++++++--------- 4 files changed, 16 insertions(+), 12 deletions(-) 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()