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

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

jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml

@@ -0,0 +1,28 @@
+<?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_smooth_velocity</name>
+  <version>0.1.0</version>
+  <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>
+</package>

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

jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg

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

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