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() + + + +