Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added a queue to the watchdog #115

Merged
merged 3 commits into from
Nov 14, 2024
Merged

Added a queue to the watchdog #115

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a58ac13
Added a queue to the watchdog
BruceMcRooster Nov 13, 2024
18e200d
Ensure no warnings when nothing has been reported
BruceMcRooster Nov 13, 2024
9aa0099
🚨Fix linting warning
BruceMcRooster Nov 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 21 additions & 5 deletions rb_ws/src/buggy/scripts/watchdog/watchdog.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,15 @@
#!/usr/bin/env python3

from collections import deque

import argparse

import rospy

from std_msgs.msg import Bool, Int8, Float64

STEERING_INSTRUCTION_MAX_LEN = 10

class Watchdog:

STEERING_DEVIANCE = 4 #deg
Expand All @@ -18,7 +22,8 @@ def __init__(self, self_name) -> None:
2 - ERROR
"""

self.commanded_steering = 0
self.steering_instructions = deque(maxlen=STEERING_INSTRUCTION_MAX_LEN)

self.inAutonSteer = False

rospy.Subscriber(
Expand All @@ -36,7 +41,7 @@ def __init__(self, self_name) -> None:

def set_input_steering(self, msg):
rospy.logdebug("Got input steering of: " + str(msg.data))
self.commanded_steering = msg.data
self.steering_instructions.append(msg.data)

def set_auton_steer(self, msg):
if (msg.data and not self.inAutonSteer):
Expand All @@ -49,16 +54,27 @@ def set_auton_steer(self, msg):
def check_stepper_steering(self, msg):
stepper_steer = msg.data
rospy.logdebug("Firmware's reported stepper degree: " + str(stepper_steer))
if (self.alarm < 2):
if self.alarm < 2:
self.alarm = 0
if abs(stepper_steer - self.commanded_steering) > Watchdog.STEERING_DEVIANCE:

steer_instruct_diff_min = 0
if len(self.steering_instructions) > 0:
# Finds the minimum difference between the stepper's reported angle and the last 10 steering instructions
steer_instruct_diff_min = min(
map(
lambda steer: abs(stepper_steer - steer),
self.steering_instructions
)
)

if steer_instruct_diff_min > 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:
elif steer_instruct_diff_min > 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)))
Expand Down
Loading