jetson/ros2_ws/src/saltybot_smooth_velocity/config/smooth_velocity_config.yaml

@@ -1,8 +1,27 @@
 smooth_velocity:
-  max_linear_accel: 0.5
+  # Acceleration limits
-  max_angular_accel: 1.0
+  max_linear_accel: 0.5      # m/s² - max linear acceleration
-  max_linear_decel: 0.8
+  max_angular_accel: 1.0     # rad/s² - max angular acceleration
-  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
+  scurve_duration: 0.2       # seconds - time to reach full acceleration
-  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,16 +1,17 @@
-"""Launch smooth velocity controller."""
+"""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")
-    node = Node(
+    smooth_node = Node(
         package="saltybot_smooth_velocity",
         executable="smooth_velocity_node",
         name="smooth_velocity_node",
         output="screen",
         parameters=[config_path],
     )
-    return LaunchDescription([node])
+    return LaunchDescription([smooth_node])

jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml

@@ -3,17 +3,25 @@
 <package format="3">
   <name>saltybot_smooth_velocity</name>
   <version>0.1.0</version>
-  <description>Smooth velocity controller with acceleration limiting and S-curve jerk reduction.</description>
+  <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,21 +1,30 @@
 #!/usr/bin/env python3
-"""Smooth velocity controller - Issue #455. Acceleration limiting + S-curve jerk reduction."""
+"""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
+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
@@ -24,79 +33,114 @@ 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_accel_limit(self.current_lin_vel, self.target_lin_vel, self.max_lin_accel, self.max_lin_decel)
+            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_accel_limit(self.current_ang_vel, self.target_ang_vel, self.max_ang_accel, self.max_ang_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)
 
-        profile = f"lin:{lin_vel:.3f},ang:{ang_vel:.3f}"
+        # 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:
-            time_var = self.scurve_time_lin if axis == "linear" else self.scurve_time_ang
+            # Smooth in/out using cosine curve
-            progress = min(time_var, self.scurve_duration) / self.scurve_duration
+            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
-        if (direction > 0 and new_vel >= target) or (direction < 0 and new_vel <= target):
+
-            if axis == "linear":
+        # Check if we've reached target
-                self.scurve_time_lin = 0.0
+        if direction > 0 and new_vel >= target:
-            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_accel_limit(self, current, target, max_accel, max_decel):
+    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
-        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 +1,2 @@
-[develop]\nscript_dir=$base/lib/saltybot_smooth_velocity\n
+[develop]
+script_dir=$base/lib/saltybot_smooth_velocity

jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py

@@ -1,4 +1,5 @@
 from setuptools import setup
+
 setup(
     name="saltybot_smooth_velocity",
     version="0.1.0",
@@ -13,7 +14,7 @@ setup(
     zip_safe=True,
     maintainer="sl-controls",
     maintainer_email="sl-controls@saltylab.local",
-    description="Smooth velocity controller with S-curve jerk reduction",
+    description="Smooth velocity controller with acceleration limiting and S-curve jerk reduction",
     license="MIT",
     tests_require=["pytest"],
     entry_points={