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

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

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

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

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