From 355cd2b0bf3949bed60a85fad620a9f204c2cee3 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Wed, 6 Nov 2024 18:19:18 -0500 Subject: [PATCH 1/7] added deviation sanity check --- .../buggy/scripts/serial/ros_to_bnyahaj.py | 54 ++++++++++++++++++- 1 file changed, 53 insertions(+), 1 deletion(-) diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index 800809d..1019c1b 100755 --- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py @@ -42,14 +42,27 @@ def __init__(self, self_name, other_name, teensy_name): """ self.comms = Comms("/dev/" + teensy_name) self.steer_angle = 0 + self.debug_steer = 0 + self.alarm = 0 + self.alarm_counter = 0 + self.fresh_steer = False + self.fresh_debug_steer = False + self.lock = Lock() + self.SANITY_THRESHOLD = 2 # Threshold for deviation in steering + self.ALARM_THRESHOLD = 3 # Threshold for the number of consecutive alarms + self.DEVIATION_ALARM_STATUS_CODE = 501 # Deviation alarm status code + rospy.Subscriber( self_name + "/buggy/input/steering", Float64, self.set_steering ) rospy.Subscriber(self_name + "/debug/sanity_warning", Int8, self.set_alarm) + rospy.Subscriber( + self_name + "/buggy/debug/steering", Float64, self.set_debug_steering + ) # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/83 if other_name is None and self_name == "NAND": @@ -104,7 +117,7 @@ def set_alarm(self, msg): def set_steering(self, msg): """ - Steering Angle Updater, updates the steering angle locally if updated on ros stopic + Steering Angle Updater, updates the steering angle locally if updated on ros topic """ rospy.loginfo(f"Read steeering angle of: {msg.data}") # print("Steering angle: " + str(msg.data)) @@ -113,6 +126,14 @@ def set_steering(self, msg): self.steer_angle = msg.data self.fresh_steer = True + def set_debug_steering(self, msg): + """ + Debug Steering Angle Updater, updates debug steering angle locally if updated on ros topic + """ + with self.lock: + self.debug_steer = msg.data + self.fresh_debug_steer = True + def writer_thread(self): """ Sends ROS Topics to bnayhaj serial, only sends a steering angle when we receive a fresh one @@ -126,6 +147,37 @@ def writer_thread(self): self.comms.send_steering(self.steer_angle) self.fresh_steer = False + # Sanity check between debug steering and input steering + if self.fresh_debug_steer: + with self.lock: + # Check for deviation between input and debug steering + deviation = abs(self.steer_angle - self.debug_steer) + if deviation > self.SANITY_THRESHOLD: + # Increment the alarm counter if deviation exceeds threshold + self.alarm_counter += 1 + rospy.logwarn( + "Sanity check failed. Deviation between input and debug steering is: " + + str(deviation) + + ". Incrementing alarm counter to: " + + str(self.alarm_counter) + + "." + ) + else: + # Reset alarm counter if steering is within threshold + self.alarm_counter = 0 + + # Check if alarm counter exceeds the maximum allowed + if self.alarm_counter > self.ALARM_THRESHOLD: + self.alarm = self.ALARM_STATUS_CODE + rospy.logerr( + "Alarm status set to " + + str(self.DEVIATION_ALARM_STATUS_CODE) + + "due to repeated sanity check failures." + ) + + # Reset the flag to avoid reprocessing until a new message arrives + self.fresh_debug_steer = False + with self.lock: self.comms.send_alarm(self.alarm) From f6659b7e8ab7c83966d41f09944e2b4dcfbaebd0 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Wed, 6 Nov 2024 23:46:42 -0500 Subject: [PATCH 2/7] finish watchdog --- rb_ws/src/buggy/launch/sc-system.launch | 1 + rb_ws/src/buggy/launch/watchdog.launch | 6 ++ rb_ws/src/buggy/scripts/watchdog/watchdog.py | 85 ++++++++++++++++++++ 3 files changed, 92 insertions(+) create mode 100644 rb_ws/src/buggy/launch/watchdog.launch create mode 100755 rb_ws/src/buggy/scripts/watchdog/watchdog.py diff --git a/rb_ws/src/buggy/launch/sc-system.launch b/rb_ws/src/buggy/launch/sc-system.launch index 219c44b..3720e8a 100644 --- a/rb_ws/src/buggy/launch/sc-system.launch +++ b/rb_ws/src/buggy/launch/sc-system.launch @@ -14,4 +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 new file mode 100644 index 0000000..d7547d0 --- /dev/null +++ b/rb_ws/src/buggy/launch/watchdog.launch @@ -0,0 +1,6 @@ + + + + + + \ 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 new file mode 100755 index 0000000..a02b3d1 --- /dev/null +++ b/rb_ws/src/buggy/scripts/watchdog/watchdog.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +import argparse + +import rospy + +from std_msgs.msg import Bool, Int8, Float64 + +class Watchdog: + + STEERING_DEVIANCE = 2 #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.check_stepper_steering + ) + + 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)) + self.commanded_steering = msg.data + + def set_auton_steer(self, msg): + 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 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))) + else: + rospy.logdebug("Stepper Deviance of " + str((stepper_steer - self.commanded_steering)) + " but not in auton steer") + + 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))) + else: + rospy.logdebug("Not in auton, but 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 + 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") + print("INITIALIZED") + rospy.loginfo("INITIALIZED WATCHDOG NODE") + watchdog = Watchdog(self_name=self_name) + watchdog.loop() + + + + From 40336b908643f8b071e91342b5ffafe6f24766e7 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Thu, 7 Nov 2024 00:18:25 -0500 Subject: [PATCH 3/7] 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() From 74d80f4c3a57eaaeadf96984c3929c064662cb9a Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Thu, 7 Nov 2024 00:18:31 -0500 Subject: [PATCH 4/7] Revert "added deviation sanity check" This reverts commit 355cd2b0bf3949bed60a85fad620a9f204c2cee3. --- .../buggy/scripts/serial/ros_to_bnyahaj.py | 54 +------------------ 1 file changed, 1 insertion(+), 53 deletions(-) diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index 1019c1b..800809d 100755 --- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py +++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py @@ -42,27 +42,14 @@ def __init__(self, self_name, other_name, teensy_name): """ self.comms = Comms("/dev/" + teensy_name) self.steer_angle = 0 - self.debug_steer = 0 - self.alarm = 0 - self.alarm_counter = 0 - self.fresh_steer = False - self.fresh_debug_steer = False - self.lock = Lock() - self.SANITY_THRESHOLD = 2 # Threshold for deviation in steering - self.ALARM_THRESHOLD = 3 # Threshold for the number of consecutive alarms - self.DEVIATION_ALARM_STATUS_CODE = 501 # Deviation alarm status code - rospy.Subscriber( self_name + "/buggy/input/steering", Float64, self.set_steering ) rospy.Subscriber(self_name + "/debug/sanity_warning", Int8, self.set_alarm) - rospy.Subscriber( - self_name + "/buggy/debug/steering", Float64, self.set_debug_steering - ) # ISSUE: https://github.com/CMU-Robotics-Club/RoboBuggy2/issues/83 if other_name is None and self_name == "NAND": @@ -117,7 +104,7 @@ def set_alarm(self, msg): def set_steering(self, msg): """ - Steering Angle Updater, updates the steering angle locally if updated on ros topic + Steering Angle Updater, updates the steering angle locally if updated on ros stopic """ rospy.loginfo(f"Read steeering angle of: {msg.data}") # print("Steering angle: " + str(msg.data)) @@ -126,14 +113,6 @@ def set_steering(self, msg): self.steer_angle = msg.data self.fresh_steer = True - def set_debug_steering(self, msg): - """ - Debug Steering Angle Updater, updates debug steering angle locally if updated on ros topic - """ - with self.lock: - self.debug_steer = msg.data - self.fresh_debug_steer = True - def writer_thread(self): """ Sends ROS Topics to bnayhaj serial, only sends a steering angle when we receive a fresh one @@ -147,37 +126,6 @@ def writer_thread(self): self.comms.send_steering(self.steer_angle) self.fresh_steer = False - # Sanity check between debug steering and input steering - if self.fresh_debug_steer: - with self.lock: - # Check for deviation between input and debug steering - deviation = abs(self.steer_angle - self.debug_steer) - if deviation > self.SANITY_THRESHOLD: - # Increment the alarm counter if deviation exceeds threshold - self.alarm_counter += 1 - rospy.logwarn( - "Sanity check failed. Deviation between input and debug steering is: " - + str(deviation) - + ". Incrementing alarm counter to: " - + str(self.alarm_counter) - + "." - ) - else: - # Reset alarm counter if steering is within threshold - self.alarm_counter = 0 - - # Check if alarm counter exceeds the maximum allowed - if self.alarm_counter > self.ALARM_THRESHOLD: - self.alarm = self.ALARM_STATUS_CODE - rospy.logerr( - "Alarm status set to " - + str(self.DEVIATION_ALARM_STATUS_CODE) - + "due to repeated sanity check failures." - ) - - # Reset the flag to avoid reprocessing until a new message arrives - self.fresh_debug_steer = False - with self.lock: self.comms.send_alarm(self.alarm) From 19b8a92e890ff74267ca311512ae7df9840f3e19 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Thu, 7 Nov 2024 00:20:11 -0500 Subject: [PATCH 5/7] pylint --- rb_ws/src/buggy/scripts/watchdog/watchdog.py | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/rb_ws/src/buggy/scripts/watchdog/watchdog.py b/rb_ws/src/buggy/scripts/watchdog/watchdog.py index a87d728..83df05a 100755 --- a/rb_ws/src/buggy/scripts/watchdog/watchdog.py +++ b/rb_ws/src/buggy/scripts/watchdog/watchdog.py @@ -17,7 +17,7 @@ def __init__(self, self_name) -> None: 1 - WARNING 2 - ERROR """ - + self.commanded_steering = 0 self.inAutonSteer = False @@ -33,12 +33,12 @@ def __init__(self, self_name) -> None: 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): + + def set_auton_steer(self, msg): if (msg.data and not self.inAutonSteer): rospy.loginfo("ENTERED AUTON") if (not msg.data and self.inAutonSteer): @@ -54,7 +54,7 @@ def check_stepper_steering(self, msg): 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))) @@ -68,7 +68,7 @@ def loop(self): ros_alarm = Int8() ros_alarm.data = self.alarm self.alarm_publisher.publish(ros_alarm) - + self.alarm_publish_rate.sleep() if __name__ == "__main__": @@ -81,8 +81,4 @@ def loop(self): rospy.init_node("watchdog") rospy.loginfo("INITIALIZED WATCHDOG NODE") watchdog = Watchdog(self_name=self_name) - watchdog.loop() - - - - + watchdog.loop() \ No newline at end of file From b56619ef09925af21d5135db575f8ee503f50e14 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Thu, 7 Nov 2024 12:23:56 -0500 Subject: [PATCH 6/7] updated telematics fix publisher --- rb_ws/src/buggy/scripts/visualization/telematics.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/scripts/visualization/telematics.py b/rb_ws/src/buggy/scripts/visualization/telematics.py index a3e4893..007aeb7 100755 --- a/rb_ws/src/buggy/scripts/visualization/telematics.py +++ b/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) From 6f724ee3f87451f242205282aba282245adf3d6a Mon Sep 17 00:00:00 2001 From: Buggy Date: Thu, 7 Nov 2024 22:58:36 -0500 Subject: [PATCH 7/7] added debugs, allowed clean shutdown, and created proper alarms --- rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py | 7 ++++--- rb_ws/src/buggy/scripts/watchdog/watchdog.py | 9 ++++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index 800809d..1395061 100755 --- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py +++ b/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): diff --git a/rb_ws/src/buggy/scripts/watchdog/watchdog.py b/rb_ws/src/buggy/scripts/watchdog/watchdog.py index 83df05a..3375861 100755 --- a/rb_ws/src/buggy/scripts/watchdog/watchdog.py +++ b/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