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 219c44b..7a458ef 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..a230231
--- /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 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/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)
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..3375861
--- /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