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 index 2597d55..63b1336 100644 --- 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 @@ -1,27 +1,8 @@ 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) + max_linear_accel: 0.5 + max_angular_accel: 1.0 + max_linear_decel: 0.8 + max_angular_decel: 1.0 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 + scurve_duration: 0.2 + update_rate: 50 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 index aaaf952..fc76cd5 100644 --- 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 @@ -1,17 +1,16 @@ -"""Launch smooth velocity controller node.""" +"""Launch smooth velocity controller.""" 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( + node = Node( package="saltybot_smooth_velocity", executable="smooth_velocity_node", name="smooth_velocity_node", output="screen", parameters=[config_path], ) - return LaunchDescription([smooth_node]) + return LaunchDescription([node]) diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml b/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml index 4bf276f..a81e5c4 100644 --- a/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml @@ -3,25 +3,17 @@ 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). - + Smooth velocity controller with acceleration limiting and S-curve jerk reduction. 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/saltybot_smooth_velocity/smooth_velocity_node.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/smooth_velocity_node.py index 6ad214e..63eab3c 100644 --- 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 @@ -1,30 +1,21 @@ #!/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. -""" +"""Smooth velocity controller - Issue #455. Acceleration limiting + S-curve jerk reduction.""" import math import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist -from std_msgs.msg import String, Float32 +from std_msgs.msg import String 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 @@ -33,114 +24,79 @@ class SmoothVelocityNode(Node): 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") + self.get_logger().info(f"Smooth velocity controller: {self.update_rate}Hz, S-curve={self.use_scurve}") 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) + lin_vel = self._apply_accel_limit(self.current_lin_vel, self.target_lin_vel, self.max_lin_accel, self.max_lin_decel) + ang_vel = self._apply_accel_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}" + profile = f"lin:{lin_vel:.3f},ang:{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 + time_var = self.scurve_time_lin if axis == "linear" else self.scurve_time_ang + progress = min(time_var, 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: + if (direction > 0 and new_vel >= target) or (direction < 0 and new_vel <= target): + if axis == "linear": + self.scurve_time_lin = 0.0 + else: + self.scurve_time_ang = 0.0 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.""" + def _apply_accel_limit(self, current, target, max_accel, max_decel): 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: + if (direction > 0 and new_vel >= target) or (direction < 0 and new_vel <= target): return target - elif direction < 0 and new_vel <= target: - return target - return new_vel def main(args=None): diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg index be37112..257f482 100644 --- a/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg @@ -1,2 +1 @@ -[develop] -script_dir=$base/lib/saltybot_smooth_velocity +[develop]\nscript_dir=$base/lib/saltybot_smooth_velocity\n \ No newline at end of file diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py index 28995de..e0c0d9b 100644 --- a/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py @@ -1,5 +1,4 @@ from setuptools import setup - setup( name="saltybot_smooth_velocity", version="0.1.0", @@ -14,7 +13,7 @@ setup( zip_safe=True, maintainer="sl-controls", maintainer_email="sl-controls@saltylab.local", - description="Smooth velocity controller with acceleration limiting and S-curve jerk reduction", + description="Smooth velocity controller with S-curve jerk reduction", license="MIT", tests_require=["pytest"], entry_points={