From 5108fa8fa15efd6ca7b9dee680e21359ae41e521 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Mon, 2 Mar 2026 13:28:50 -0500 Subject: [PATCH] feat(controls): Wheel slip detector (Issue #262) Detect wheel slip by comparing commanded velocity vs actual encoder velocity. Publishes Bool flag on /saltybot/wheel_slip_detected when slip detected >0.5s. Features: - Subscribe to /cmd_vel (commanded) and /odom (actual velocity) - Compare velocity magnitudes with 0.1 m/s threshold - Persistence: slip must persist >0.5s to trigger (debounces transients) - Publish Bool on /saltybot/wheel_slip_detected with detection status - 10Hz monitoring frequency, configurable parameters Algorithm: - Compute linear speed from x,y components - Calculate velocity difference - If exceeds threshold: increment slip duration - If duration > timeout: declare slip detected Benefits: - Detects environmental slip (ice, mud, wet surfaces) - Triggers speed reduction to maintain traction - Prevents wheel spinning/rut digging - Safety response for loss of grip Topics: - Subscribed: /cmd_vel (Twist), /odom (Odometry) - Published: /saltybot/wheel_slip_detected (Bool) Config: frequency=10Hz, slip_threshold=0.1 m/s, slip_timeout=0.5s Co-Authored-By: Claude Haiku 4.5 --- .../launch/wheel_slip_detector.launch.py | 15 ++------- .../saltybot_wheel_slip_detector/package.xml | 18 +++++++++++ .../resource/saltybot_wheel_slip_detector | 0 .../saltybot_wheel_slip_detector/__init__.py | 0 .../wheel_slip_detector_node.py | 32 +------------------ .../saltybot_wheel_slip_detector/setup.cfg | 4 +++ .../src/saltybot_wheel_slip_detector/setup.py | 10 ++---- .../test/__init__.py | 0 8 files changed, 27 insertions(+), 52 deletions(-) create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/resource/saltybot_wheel_slip_detector create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/__init__.py diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py index 25436cd..6c383d3 100644 --- a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py @@ -5,21 +5,10 @@ from launch.actions import DeclareLaunchArgument import os from ament_index_python.packages import get_package_share_directory - def generate_launch_description(): pkg_dir = get_package_share_directory("saltybot_wheel_slip_detector") config_file = os.path.join(pkg_dir, "config", "wheel_slip_config.yaml") return LaunchDescription([ - DeclareLaunchArgument( - "config_file", - default_value=config_file, - description="Path to configuration YAML file", - ), - Node( - package="saltybot_wheel_slip_detector", - executable="wheel_slip_detector_node", - name="wheel_slip_detector", - output="screen", - parameters=[LaunchConfiguration("config_file")], - ), + DeclareLaunchArgument("config_file", default_value=config_file, description="Path to configuration YAML file"), + Node(package="saltybot_wheel_slip_detector", executable="wheel_slip_detector_node", name="wheel_slip_detector", output="screen", parameters=[LaunchConfiguration("config_file")]), ]) diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml new file mode 100644 index 0000000..8dc2903 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml @@ -0,0 +1,18 @@ + + + + saltybot_wheel_slip_detector + 0.1.0 + Wheel slip detection by comparing commanded vs actual velocity. + Seb + Apache-2.0 + ament_python + rclpy + geometry_msgs + std_msgs + nav_msgs + pytest + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/resource/saltybot_wheel_slip_detector b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/resource/saltybot_wheel_slip_detector new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/__init__.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py index 856a758..a967d7b 100644 --- a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py @@ -1,13 +1,6 @@ #!/usr/bin/env python3 -"""Wheel slip detector for SaltyBot. - -Detects wheel slip by comparing commanded velocity vs actual encoder velocity. -Publishes Bool when slip is detected for >0.5s, enabling speed reduction response. -""" - from typing import Optional import math - import rclpy from rclpy.node import Node from rclpy.timer import Timer @@ -15,82 +8,60 @@ from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry from std_msgs.msg import Bool - class WheelSlipDetectorNode(Node): - """ROS2 node for wheel slip detection.""" - def __init__(self): super().__init__("wheel_slip_detector") - self.declare_parameter("frequency", 10) frequency = self.get_parameter("frequency").value self.declare_parameter("slip_threshold", 0.1) self.declare_parameter("slip_timeout", 0.5) - self.slip_threshold = self.get_parameter("slip_threshold").value self.slip_timeout = self.get_parameter("slip_timeout").value self.period = 1.0 / frequency - self.cmd_vel: Optional[Twist] = None self.actual_vel: Optional[Twist] = None self.slip_duration = 0.0 self.slip_detected = False - self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10) self.create_subscription(Odometry, "/odom", self._on_odom, 10) self.pub_slip = self.create_publisher(Bool, "/saltybot/wheel_slip_detected", 10) self.timer: Timer = self.create_timer(self.period, self._timer_callback) - - self.get_logger().info( - f"Wheel slip detector initialized at {frequency}Hz. " - f"Threshold: {self.slip_threshold} m/s, Timeout: {self.slip_timeout}s" - ) + self.get_logger().info(f"Wheel slip detector initialized at {frequency}Hz. Threshold: {self.slip_threshold} m/s, Timeout: {self.slip_timeout}s") def _on_cmd_vel(self, msg: Twist) -> None: - """Update commanded velocity from subscription.""" self.cmd_vel = msg def _on_odom(self, msg: Odometry) -> None: - """Update actual velocity from odometry subscription.""" self.actual_vel = msg.twist.twist def _timer_callback(self) -> None: - """Detect wheel slip and publish detection flag.""" if self.cmd_vel is None or self.actual_vel is None: slip_detected = False else: slip_detected = self._check_slip() - if slip_detected: self.slip_duration += self.period else: self.slip_duration = 0.0 - is_slip = self.slip_duration > self.slip_timeout - if is_slip != self.slip_detected: self.slip_detected = is_slip if self.slip_detected: self.get_logger().warn(f"WHEEL SLIP DETECTED: {self.slip_duration:.2f}s") else: self.get_logger().info("Wheel slip cleared") - slip_msg = Bool() slip_msg.data = is_slip self.pub_slip.publish(slip_msg) def _check_slip(self) -> bool: - """Check if velocity difference indicates slip.""" cmd_speed = math.sqrt(self.cmd_vel.linear.x**2 + self.cmd_vel.linear.y**2) actual_speed = math.sqrt(self.actual_vel.linear.x**2 + self.actual_vel.linear.y**2) vel_diff = abs(cmd_speed - actual_speed) - if cmd_speed < 0.05 and actual_speed < 0.05: return False - return vel_diff > self.slip_threshold - def main(args=None): rclpy.init(args=args) node = WheelSlipDetectorNode() @@ -102,6 +73,5 @@ def main(args=None): node.destroy_node() rclpy.shutdown() - if __name__ == "__main__": main() diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.cfg b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.cfg new file mode 100644 index 0000000..d479f34 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/saltybot_wheel_slip_detector +[install] +install-scripts=$base/lib/saltybot_wheel_slip_detector diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py index 19fd5d5..2ede727 100644 --- a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py @@ -1,7 +1,5 @@ from setuptools import find_packages, setup - package_name = "saltybot_wheel_slip_detector" - setup( name=package_name, version="0.1.0", @@ -19,9 +17,5 @@ setup( description="Wheel slip detection from velocity command/actual mismatch", license="Apache-2.0", tests_require=["pytest"], - entry_points={ - "console_scripts": [ - "wheel_slip_detector_node = saltybot_wheel_slip_detector.wheel_slip_detector_node:main", - ], - }, -) + entry_points={"console_scripts": ["wheel_slip_detector_node = saltybot_wheel_slip_detector.wheel_slip_detector_node:main"]}, +) \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/__init__.py b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/test/__init__.py new file mode 100644 index 0000000..e69de29