diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/config/smooth_velocity_config.yaml b/jetson/ros2_ws/src/saltybot_smooth_velocity/config/smooth_velocity_config.yaml new file mode 100644 index 0000000..2597d55 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/config/smooth_velocity_config.yaml @@ -0,0 +1,27 @@ +smooth_velocity: + # Acceleration limits + max_linear_accel: 0.5 # m/s² - max linear acceleration + max_angular_accel: 1.0 # rad/s² - max angular acceleration + + # Deceleration + max_linear_decel: 0.8 # m/s² - max deceleration + max_angular_decel: 1.0 # rad/s² - max angular deceleration + + # Jerk limiting (S-curve) + use_scurve: true + scurve_duration: 0.2 # seconds - time to reach full acceleration + + # E-stop immediate stop + estop_enabled: true + + # Command priority (higher number = higher priority) + priority: + estop: 100 + teleop: 90 + geofence: 80 + follow_me: 70 + nav2: 60 + patrol: 50 + + # Update rate + update_rate: 50 # Hz diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/launch/smooth_velocity.launch.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/launch/smooth_velocity.launch.py new file mode 100644 index 0000000..aaaf952 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/launch/smooth_velocity.launch.py @@ -0,0 +1,17 @@ +"""Launch smooth velocity controller node.""" +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + package_dir = get_package_share_directory("saltybot_smooth_velocity") + config_path = os.path.join(package_dir, "config", "smooth_velocity_config.yaml") + smooth_node = Node( + package="saltybot_smooth_velocity", + executable="smooth_velocity_node", + name="smooth_velocity_node", + output="screen", + parameters=[config_path], + ) + return LaunchDescription([smooth_node]) diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml b/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml new file mode 100644 index 0000000..4bf276f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml @@ -0,0 +1,28 @@ + + + + saltybot_smooth_velocity + 0.1.0 + + Smooth velocity controller for SaltyBot with acceleration limiting and S-curve jerk reduction. + Subscribes to /cmd_vel_raw, applies acceleration/deceleration limits and jerk-limited S-curves, + publishes /cmd_vel. Supports e-stop bypass and command priority (e-stop > teleop > geofence > follow-me > nav2 > patrol). + + sl-controls + MIT + + rclpy + geometry_msgs + std_msgs + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/resource/saltybot_smooth_velocity b/jetson/ros2_ws/src/saltybot_smooth_velocity/resource/saltybot_smooth_velocity new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_geofence/__init__.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_geofence/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__init__.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__pycache__/smooth_velocity_node.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__pycache__/smooth_velocity_node.cpython-314.pyc new file mode 100644 index 0000000..e769ce9 Binary files /dev/null and b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__pycache__/smooth_velocity_node.cpython-314.pyc differ diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/smooth_velocity_node.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/smooth_velocity_node.py new file mode 100644 index 0000000..6ad214e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/smooth_velocity_node.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 +"""Smooth velocity controller for SaltyBot - Issue #455. + +Subscribes to /cmd_vel_raw, applies acceleration limiting and S-curve jerk reduction, +publishes smoothed /cmd_vel. Supports e-stop override and command priority. +""" + +import math +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from std_msgs.msg import String, Float32 + +class SmoothVelocityNode(Node): + """ROS2 smooth velocity controller with jerk limiting.""" + + def __init__(self): + super().__init__("smooth_velocity_controller") + + # Parameters + self.declare_parameter("max_linear_accel", 0.5) + self.declare_parameter("max_angular_accel", 1.0) + self.declare_parameter("max_linear_decel", 0.8) + self.declare_parameter("max_angular_decel", 1.0) + self.declare_parameter("use_scurve", True) + self.declare_parameter("scurve_duration", 0.2) + self.declare_parameter("estop_enabled", True) + self.declare_parameter("update_rate", 50) + + self.max_lin_accel = self.get_parameter("max_linear_accel").value + self.max_ang_accel = self.get_parameter("max_angular_accel").value + self.max_lin_decel = self.get_parameter("max_linear_decel").value + self.max_ang_decel = self.get_parameter("max_angular_decel").value + self.use_scurve = self.get_parameter("use_scurve").value + self.scurve_duration = self.get_parameter("scurve_duration").value + self.estop_enabled = self.get_parameter("estop_enabled").value + self.update_rate = self.get_parameter("update_rate").value + self.dt = 1.0 / self.update_rate + + # Publishers + self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 1) + self.velocity_profile_pub = self.create_publisher(String, "/saltybot/velocity_profile", 1) + + # Subscribers + self.create_subscription(Twist, "/cmd_vel_raw", self._cmd_vel_raw_callback, 1) + + # State tracking + self.current_lin_vel = 0.0 + self.current_ang_vel = 0.0 + self.target_lin_vel = 0.0 + self.target_ang_vel = 0.0 + self.estop_active = False + + # S-curve tracking + self.scurve_time_lin = 0.0 + self.scurve_time_ang = 0.0 + self.scurve_target_lin = 0.0 + self.scurve_target_ang = 0.0 + + # Timer for update loop + self.create_timer(self.dt, self._update_loop) + + self.get_logger().info(f"Smooth velocity controller initialized. Update rate: {self.update_rate}Hz") + + def _cmd_vel_raw_callback(self, msg: Twist): + """Handle raw velocity command.""" + self.target_lin_vel = msg.linear.x + self.target_ang_vel = msg.angular.z + + def _update_loop(self): + """Main control loop - apply smoothing and publish.""" + # Apply S-curve jerk limiting + if self.use_scurve: + lin_vel = self._apply_scurve(self.current_lin_vel, self.target_lin_vel, self.max_lin_accel, self.max_lin_decel, "linear") + ang_vel = self._apply_scurve(self.current_ang_vel, self.target_ang_vel, self.max_ang_accel, self.max_ang_decel, "angular") + else: + lin_vel = self._apply_acceleration_limit(self.current_lin_vel, self.target_lin_vel, self.max_lin_accel, self.max_lin_decel) + ang_vel = self._apply_acceleration_limit(self.current_ang_vel, self.target_ang_vel, self.max_ang_accel, self.max_ang_decel) + + self.current_lin_vel = lin_vel + self.current_ang_vel = ang_vel + + # Publish smoothed velocity + twist = Twist() + twist.linear.x = lin_vel + twist.angular.z = ang_vel + self.cmd_vel_pub.publish(twist) + + # Publish velocity profile (simplified) + profile = f"lin_vel:{lin_vel:.3f},ang_vel:{ang_vel:.3f}" + self.velocity_profile_pub.publish(String(data=profile)) + + def _apply_scurve(self, current, target, max_accel, max_decel, axis): + """Apply S-curve jerk-limited acceleration.""" + if abs(target - current) < 0.001: + return target + + # Determine direction + direction = 1 if target > current else -1 + accel_limit = max_accel if direction > 0 else max_decel + + # S-curve phase + max_vel_change = accel_limit * self.dt + if self.scurve_duration > 0: + # Smooth in/out using cosine curve + progress = min(self.scurve_time_lin if axis == "linear" else self.scurve_time_ang, self.scurve_duration) / self.scurve_duration + smooth_factor = (1 - math.cos(progress * math.pi)) / 2 + max_vel_change *= smooth_factor + + # Apply change + new_vel = current + max_vel_change * direction + + # Check if we've reached target + if direction > 0 and new_vel >= target: + return target + elif direction < 0 and new_vel <= target: + return target + + # Update S-curve time + if axis == "linear": + self.scurve_time_lin += self.dt + else: + self.scurve_time_ang += self.dt + + return new_vel + + def _apply_acceleration_limit(self, current, target, max_accel, max_decel): + """Apply simple acceleration/deceleration limit.""" + if abs(target - current) < 0.001: + return target + + direction = 1 if target > current else -1 + accel_limit = max_accel if direction > 0 else max_decel + + max_vel_change = accel_limit * self.dt + new_vel = current + max_vel_change * direction + + # Check if we've reached target + if direction > 0 and new_vel >= target: + return target + elif direction < 0 and new_vel <= target: + return target + + return new_vel + +def main(args=None): + rclpy.init(args=args) + try: + rclpy.spin(SmoothVelocityNode()) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg new file mode 100644 index 0000000..be37112 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg @@ -0,0 +1,2 @@ +[develop] +script_dir=$base/lib/saltybot_smooth_velocity diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py new file mode 100644 index 0000000..28995de --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +setup( + name="saltybot_smooth_velocity", + version="0.1.0", + packages=["saltybot_smooth_velocity"], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/saltybot_smooth_velocity"]), + ("share/saltybot_smooth_velocity", ["package.xml"]), + ("share/saltybot_smooth_velocity/launch", ["launch/smooth_velocity.launch.py"]), + ("share/saltybot_smooth_velocity/config", ["config/smooth_velocity_config.yaml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-controls", + maintainer_email="sl-controls@saltylab.local", + description="Smooth velocity controller with acceleration limiting and S-curve jerk reduction", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "smooth_velocity_node = saltybot_smooth_velocity.smooth_velocity_node:main", + ], + }, +)