From b6acf86b1b61ad6c7aad5684410ef3b53d9ed9b2 Mon Sep 17 00:00:00 2001 From: alex berg Date: Mon, 11 Nov 2024 10:29:47 -0600 Subject: [PATCH 1/4] Adding lifespan QOS to joynode yaml. NEEDS TO BE TESTED --- config/joy_node.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/config/joy_node.yaml b/config/joy_node.yaml index f3771b80..8fb44122 100644 --- a/config/joy_node.yaml +++ b/config/joy_node.yaml @@ -12,5 +12,8 @@ joy_node: durability: volatile history: keep_last reliability: reliable + lifespan: + sec: 0.5 + nsec: 0 sticky_buttons: false use_sim_time: false \ No newline at end of file From 77f4e3a2aa22934511a66401de7f91cf8651fb60 Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Fri, 10 Jan 2025 16:50:07 -0600 Subject: [PATCH 2/4] Decreased size of the message queue --- README.md | 2 +- config/joy_node.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 849e1daa..20ef4533 100644 --- a/README.md +++ b/README.md @@ -185,7 +185,7 @@ in another terminal to control the robot with your keyboard. Alternatively, you can run these nodes: ``` -ros2 run joy joy_node +ros2 run joy joy_node --ros-args --params-file config/joy_node.yaml ros2 run rovr_control main_control_node ``` to control the robot using a gamepad and our button bindings assigned in the main_control_node. diff --git a/config/joy_node.yaml b/config/joy_node.yaml index 8fb44122..629734d3 100644 --- a/config/joy_node.yaml +++ b/config/joy_node.yaml @@ -8,7 +8,7 @@ joy_node: qos_overrides: /parameter_events: publisher: - depth: 1000 + depth: 10 durability: volatile history: keep_last reliability: reliable From 692f7552ca92a1dd0761ed41333978011c53460f Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Fri, 10 Jan 2025 17:15:55 -0600 Subject: [PATCH 3/4] Added a joystick timeout watchdog to the main_control_node for extra safety --- .../rovr_control/main_control_node.py | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 5048b80a..1adb2ff5 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -6,6 +6,7 @@ # Import the ROS 2 module import rclpy +import time from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle from rclpy.callback_groups import ReentrantCallbackGroup @@ -154,6 +155,15 @@ def __init__(self) -> None: self.nav2 = BasicNavigator() # Instantiate the BasicNavigator class + # Add watchdog parameters + self.declare_parameter("watchdog_timeout", 0.5) # Timeout in seconds + self.watchdog_timeout = self.get_parameter("watchdog_timeout").value + self.last_joy_timestamp = time.time() + + # Create timer for watchdog + self.watchdog_timer = self.create_timer(0.1, self.watchdog_callback) # Check every 0.1 seconds + self.connection_active = True + # ----- !! BLOCKING WHILE LOOP !! ----- # while not self.cli_lift_zero.wait_for_service(timeout_sec=1): self.get_logger().warn("Waiting for the lift/zero service to be available (BLOCKING)") @@ -213,6 +223,9 @@ def get_result_callback(self, future: Future): async def joystick_callback(self, msg: Joy) -> None: """This method is called whenever a joystick message is received.""" + # Update watchdog timestamp + self.last_joy_timestamp = time.time() + # PUT TELEOP CONTROLS BELOW # if self.state == states["Teleop"]: @@ -330,6 +343,21 @@ async def joystick_callback(self, msg: Joy) -> None: for index in range(len(buttons)): buttons[index] = msg.buttons[index] + def watchdog_callback(self): + """Check if we've received joystick messages recently""" + current_time = time.time() + time_since_last_joy = current_time - self.last_joy_timestamp + + # If we haven't received a message in watchdog_timeout seconds + if time_since_last_joy > self.watchdog_timeout: + if self.connection_active: # Only trigger once when connection is lost + self.get_logger().warn(f"No joystick messages received for {time_since_last_joy:.2f} seconds! Stopping the robot.") + self.stop_all_subsystems() # Stop all robot movement! (safety feature) + self.connection_active = False + elif not self.connection_active: + self.get_logger().warn("Joystick messages received! Functionality of the robot has been restored.") + self.connection_active = True + def main(args=None) -> None: rclpy.init(args=args) From 3b77c36ff1dd4e4bf1f89b844917657409a796ed Mon Sep 17 00:00:00 2001 From: Anthony Brogni Date: Fri, 10 Jan 2025 17:20:08 -0600 Subject: [PATCH 4/4] auto format to pass the linter --- src/rovr_control/rovr_control/main_control_node.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/rovr_control/rovr_control/main_control_node.py b/src/rovr_control/rovr_control/main_control_node.py index 1adb2ff5..327856cd 100644 --- a/src/rovr_control/rovr_control/main_control_node.py +++ b/src/rovr_control/rovr_control/main_control_node.py @@ -351,7 +351,9 @@ def watchdog_callback(self): # If we haven't received a message in watchdog_timeout seconds if time_since_last_joy > self.watchdog_timeout: if self.connection_active: # Only trigger once when connection is lost - self.get_logger().warn(f"No joystick messages received for {time_since_last_joy:.2f} seconds! Stopping the robot.") + self.get_logger().warn( + f"No joystick messages received for {time_since_last_joy:.2f} seconds! Stopping the robot." + ) self.stop_all_subsystems() # Stop all robot movement! (safety feature) self.connection_active = False elif not self.connection_active: