From 7c9e218eca33baae8c20163bb9aed6996a7239f5 Mon Sep 17 00:00:00 2001 From: Saransh Agrawal Date: Tue, 12 Nov 2024 17:25:47 -0500 Subject: [PATCH] Input/Debug Steering Deviation Sanity Alarm (#113) * added deviation sanity check * finish watchdog * fixed up watchdog and added to launch files * Revert "added deviation sanity check" This reverts commit 355cd2b0bf3949bed60a85fad620a9f204c2cee3. * pylint * updated telematics fix publisher * added debugs, allowed clean shutdown, and created proper alarms --------- Co-authored-by: Mehul Goel Co-authored-by: Buggy --- rb_ws/src/buggy/launch/nand-system.launch | 1 + rb_ws/src/buggy/launch/sc-system.launch | 1 + rb_ws/src/buggy/launch/watchdog.launch | 6 ++ .../buggy/scripts/serial/ros_to_bnyahaj.py | 7 +- .../buggy/scripts/visualization/telematics.py | 4 +- rb_ws/src/buggy/scripts/watchdog/watchdog.py | 87 +++++++++++++++++++ 6 files changed, 101 insertions(+), 5 deletions(-) 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/nand-system.launch b/rb_ws/src/buggy/launch/nand-system.launch index 21416a5e..f9a3f3d4 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 219c44b4..7a458ef3 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 00000000..a2302313 --- /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/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py index 800809df..1395061a 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/visualization/telematics.py b/rb_ws/src/buggy/scripts/visualization/telematics.py index a3e4893c..007aeb70 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) 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 00000000..33758618 --- /dev/null +++ b/rb_ws/src/buggy/scripts/watchdog/watchdog.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 + +import argparse + +import rospy + +from std_msgs.msg import Bool, Int8, Float64 + +class Watchdog: + + STEERING_DEVIANCE = 4 #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.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.logdebug("Got input steering of: " + str(msg.data)) + self.commanded_steering = msg.data + + 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.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: + 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: + 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 not rospy.is_shutdown(): + #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") + rospy.loginfo("INITIALIZED WATCHDOG NODE") + watchdog = Watchdog(self_name=self_name) + watchdog.loop() \ No newline at end of file