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)