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 <noreply@anthropic.com>

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

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")]),
+    ])

jetson/ros2_ws/src/saltybot_wheel_slip_detector/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_wheel_slip_detector</name>
+  <version>0.1.0</version>
+  <description>Wheel slip detection by comparing commanded vs actual velocity.</description>
+  <maintainer email="seb@vayrette.com">Seb</maintainer>
+  <license>Apache-2.0</license>
+  <buildtool_depend>ament_python</buildtool_depend>
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <test_depend>pytest</test_depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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()

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

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"]},
+)