seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

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

Browse Source 
# 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
This commit is contained in:
sl-jetson 2026-03-05 11:09:32 -05:00
commit b38f948844
6 changed files with 27 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
jetson/ros2_ws/src/saltybot_smooth_velocity/config/smooth_velocity_config.yaml
View File

@ -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

7
jetson/ros2_ws/src/saltybot_smooth_velocity/launch/smooth_velocity.launch.py
View File

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

10
jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml
View File

@ -3,25 +3,17 @@
<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>
<description>Smooth velocity controller with acceleration limiting and S-curve jerk reduction.</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>

74
jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/smooth_velocity_node.py
View File

@ -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):

3
jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg
View File

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

3
jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py
View File

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