From e76f0ab95f2780483b7f9c4265b487287818aa20 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 --- .../config/wheel_slip_config.yaml | 5 ++ .../launch/wheel_slip_detector.launch.py | 14 ++++ .../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 | 77 +++++++++++++++++++ .../saltybot_wheel_slip_detector/setup.cfg | 4 + .../src/saltybot_wheel_slip_detector/setup.py | 21 +++++ .../test/__init__.py | 0 9 files changed, 139 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/config/wheel_slip_config.yaml create mode 100644 jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py 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/saltybot_wheel_slip_detector/wheel_slip_detector_node.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/setup.py 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/config/wheel_slip_config.yaml b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/config/wheel_slip_config.yaml new file mode 100644 index 0000000..7bae9fe --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/config/wheel_slip_config.yaml @@ -0,0 +1,5 @@ +wheel_slip_detector: + ros__parameters: + frequency: 10 + slip_threshold: 0.1 + slip_timeout: 0.5 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 new file mode 100644 index 0000000..6c383d3 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/launch/wheel_slip_detector.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +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")]), + ]) 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 new file mode 100644 index 0000000..a967d7b --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/saltybot_wheel_slip_detector/wheel_slip_detector_node.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 +from typing import Optional +import math +import rclpy +from rclpy.node import Node +from rclpy.timer import Timer +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from std_msgs.msg import Bool + +class WheelSlipDetectorNode(Node): + 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. Threshold: {self.slip_threshold} m/s, Timeout: {self.slip_timeout}s") + + def _on_cmd_vel(self, msg: Twist) -> None: + self.cmd_vel = msg + + def _on_odom(self, msg: Odometry) -> None: + self.actual_vel = msg.twist.twist + + def _timer_callback(self) -> None: + 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: + 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() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + 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 new file mode 100644 index 0000000..2ede727 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_wheel_slip_detector/setup.py @@ -0,0 +1,21 @@ +from setuptools import find_packages, setup +package_name = "saltybot_wheel_slip_detector" +setup( + name=package_name, + version="0.1.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", ["launch/wheel_slip_detector.launch.py"]), + ("share/" + package_name + "/config", ["config/wheel_slip_config.yaml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Seb", + maintainer_email="seb@vayrette.com", + 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"]}, +) \ 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