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