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