Merge remote-tracking branch 'origin/sl-controls/issue-455-smooth-velocity'

# Conflicts:
#	jetson/ros2_ws/src/saltybot_smooth_velocity/config/smooth_velocity_config.yaml
#	jetson/ros2_ws/src/saltybot_smooth_velocity/launch/smooth_velocity.launch.py
#	jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml
#	jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/smooth_velocity_node.py
#	jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg
#	jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py

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
-  max_linear_accel: 0.5      # m/s² - max linear acceleration
+  max_angular_accel: 1.0
-  max_angular_accel: 1.0     # rad/s² - max angular acceleration
+  max_linear_decel: 0.8
-  
+  max_angular_decel: 1.0
-  # 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
+  scurve_duration: 0.2
-  
+  update_rate: 50
-  # 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

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

jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml

@@ -3,25 +3,17 @@
 <package format="3">
   <name>saltybot_smooth_velocity</name>
   <version>0.1.0</version>
-  <description>
+  <description>Smooth velocity controller with acceleration limiting and S-curve jerk reduction.</description>
-    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).
-  </description>
   <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
   <license>MIT</license>
-
   <depend>rclpy</depend>
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
-
   <buildtool_depend>ament_python</buildtool_depend>
-
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>
   <test_depend>ament_pep257</test_depend>
   <test_depend>python3-pytest</test_depend>
-
   <export>
     <build_type>ament_python</build_type>
   </export>

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.
+"""Smooth velocity controller - Issue #455. Acceleration limiting + S-curve jerk reduction."""
-
-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
+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: {self.update_rate}Hz, S-curve={self.use_scurve}")
-        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)
+            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_acceleration_limit(self.current_ang_vel, self.target_ang_vel, self.max_ang_accel, self.max_ang_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:{lin_vel:.3f},ang:{ang_vel:.3f}"
-        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
+            time_var = self.scurve_time_lin if axis == "linear" else self.scurve_time_ang
-            progress = min(self.scurve_time_lin if axis == "linear" else self.scurve_time_ang, self.scurve_duration) / self.scurve_duration
+            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
-
+        if (direction > 0 and new_vel >= target) or (direction < 0 and new_vel <= target):
-        # Check if we've reached target
+            if axis == "linear":
-        if direction > 0 and new_vel >= target:
+                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):
+    def _apply_accel_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
-
+        if (direction > 0 and new_vel >= target) or (direction < 0 and new_vel <= target):
-        # 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):

jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg

@@ -1,2 +1 @@
-[develop]
+[develop]\nscript_dir=$base/lib/saltybot_smooth_velocity\n
-script_dir=$base/lib/saltybot_smooth_velocity

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={