From 9d36e1007d84ced3af1ccfe28a680a5ed42b692f Mon Sep 17 00:00:00 2001 From: sl-controls Date: Thu, 5 Mar 2026 09:18:58 -0500 Subject: [PATCH] feat: Add Issue #455 - Smooth velocity controller with jerk reduction MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implement acceleration-limited velocity controller with S-curve jerk limiting: - Subscribe to /cmd_vel_raw, publish smoothed /cmd_vel - Max linear acceleration: 0.5 m/s² - Max angular acceleration: 1.0 rad/s² - Deceleration: 0.8 m/s² (linear), 1.0 rad/s² (angular) - S-curve jerk limiting for smooth acceleration profiles (0.2s ramp) - E-stop immediate stop capability - Command priority system (e-stop > teleop > geofence > follow-me > nav2 > patrol) - Publish /saltybot/velocity_profile for monitoring - Configurable via smooth_velocity_config.yaml - 50Hz update rate (configurable) Co-Authored-By: Claude Haiku 4.5 --- .../config/smooth_velocity_config.yaml | 27 +++ .../launch/smooth_velocity.launch.py | 17 ++ .../src/saltybot_smooth_velocity/package.xml | 28 ++++ .../resource/saltybot_smooth_velocity | 0 .../saltybot_geofence/__init__.py | 0 .../saltybot_smooth_velocity/__init__.py | 0 .../smooth_velocity_node.cpython-314.pyc | Bin 0 -> 8505 bytes .../smooth_velocity_node.py | 156 ++++++++++++++++++ .../src/saltybot_smooth_velocity/setup.cfg | 2 + .../src/saltybot_smooth_velocity/setup.py | 25 +++ 10 files changed, 255 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/config/smooth_velocity_config.yaml create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/launch/smooth_velocity.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/resource/saltybot_smooth_velocity create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_geofence/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__pycache__/smooth_velocity_node.cpython-314.pyc create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/smooth_velocity_node.py create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py 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 new file mode 100644 index 0000000..2597d55 --- /dev/null +++ b/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 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 new file mode 100644 index 0000000..aaaf952 --- /dev/null +++ b/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]) diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml b/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml new file mode 100644 index 0000000..4bf276f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/package.xml @@ -0,0 +1,28 @@ + + + + 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). + + 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/resource/saltybot_smooth_velocity b/jetson/ros2_ws/src/saltybot_smooth_velocity/resource/saltybot_smooth_velocity new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_geofence/__init__.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_geofence/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__init__.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__pycache__/smooth_velocity_node.cpython-314.pyc b/jetson/ros2_ws/src/saltybot_smooth_velocity/saltybot_smooth_velocity/__pycache__/smooth_velocity_node.cpython-314.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e769ce92f2b39c61fb61a02d830c4743405d5290 GIT binary patch literal 8505 zcmcIJTWlLwc6Z1jIpRYvTMvq&II#uh1}iP8dKy*qD_&^ zosng;yI3PAcIhC1vu!`*{@7athz#^YEf%Oh_G9za?VlJ?p)&2Z0=htvKRNOu+iijN zoOzK_s8zP;4#4|3=brmG_uTX1OSK*+f$twbe3JZqGa>(u9pkgM&|qZF-cky`L3bUr$bKX)Y7aJr^#7S;^hoK5MN9PFoi&4 z%YA$-n@uGF5sSw~m?tJDGig4RTujQz^gJI+C-|xU_!7M>@;60#i>G2@DUM_MoY|$> zR8m?1T1nlOm@uaf@l#9LY=+7bFZN4vCd+4Ti(dv$%*eXK6A+;T$37M37O~ ziA+L#!YH=s<)kDBEsA|grmzXcaXFQV$)^V6I#1NfqIY(`4~-a!G55%IA~SGHmYG`F z+DtM`n335CImLu6z-3OhMHpyXu^ncite5S0C$b}Khmym)kZZ!6?2N#TM96E5?Cv7L z8l_R=dPHYB?lN~O{Ozeh{oGZrV>t=;q;kyQWhl;6?B5P;5Cm>_0>tcf*!(pUImJZC z4Sr)P2r_IK96lxu*RbM1k?QSKS7J+Jk=0WH^t$(y`b zh8eB!5m?DQkpU&dO~OY9%~uLMY66wrth8ws39X!JD21}2RO%OFcZ5_jEyk#z8ohaK zy6q9A0a3B^{8DNkZf9fa>QCSkla^PWewG zibq7_D2VCUY)VWhuBB`uCW~O0WiiJYb~@KKAfaKO&Bz1hfwD9+mrRK{w=oK>xqcEt zF6U1F0aqcJ2IC$}C39jT#J{aB$>Wk|c*QcBn+P6LY|>IzqzWepI93pp1~?WlMHI3z z8e0@)KzimyxuR>k9ZM~VibvR`mGU0MehLGh{41}yOLD3*k* z)W@laY%v@TO7Tr7F2j!jI7Lma9No_zBvFp&Gt!{2EDcCBZi<(%@7eqTgzE8?-&Y!(qEPlOMfIfkzV0xE3`xS9mEMNk{O%t4 z7tV9vQQ%tFy{C)Z%R6>b<0)|+1+HT~aB}Tbk$Yu}a~pucwTne=xB_t4h?O6#P8Yes z3W^hJU9gM^SozT!TjWlc`#)TrEpjKf{hcNMK*2w-e&WK0|6<8MR`8GIVPxA@8FO*N zf2riZTJT@pkNM^_Z5^eyzCv5y>a~rwGo`k3g|>5f_Jg{uKvyYnvJg1AK6r5>aH$ly zS_r7Cw43X%)ou9CmHd|r{>$3(zCg*>U-0#>E^hc0VWy4BdU!j@Wl3$4AI zjlD3t1DM>-mwYD*z7s!hdg}b;`3;{sN9$I1Z>js$Liek`=vluovwkDC(LIZEbn4f_ z7dZZ-rq$Y~Y-u=L7!I#bzg^@afaJSySo>Dsm{xamV(r^9>UOp7tunH+8%K6*>9xL) zx4?NM8DscWdvH3XNm&AA`c-LKo=yt+JeM%JV6l%n&6==^$QVSVA|df?V) zTxu7ji>Q9K;cKJZW5Bh`LxFyFgv?lm{~SUsfVPwGFbfbSOfm0}0SL2#mM?I;VqKKx zDNwAX9lP0yIXawW93FFfb;NGPGf$cMs2BxB(tfbK4VDwEWP?@f3nd~s97$eW1CD%R* zv06$56R}(w>Zl=HXd-A=?4a1dnkvj4`U1?ZI6z~e300UJ?Eyg0LQgB)LbnJ31fTe0cLw>S4-|wnw)f-g>n7a52w*#`!ch8ZrPR1U+rI zQTJ{=lqdz&*l4(Di65YBz!;Q)eIG#CeIV!pvJs`;Shs3O`mX?o9P-lfs6Q9s}!uQw~I$*ziy-i%cG zwAKg{W<8KVide87WV0CUbbpsrC{D{<&oJZ>s-$AMo>{*~zCsg?0n@7gO*Pp$Q@H=fHo)UllfuCv4i3taGTd)DTQ zFTU{^H=^ly^SuJ+@y0-7yLVT?@Igk+)c?`Wz6yJJ z#vxUhwMXnTqzs+5jx*D2))CECGq`=a7KL4qELUA)>a+bA?aMW^N$MA|g)M6O7xELT z-KkF*_s*VnyF)}*^*8Ysi_!I#jaGI+~95{$7d{iR&+++ z@2jvwSuWZdu~*K~XD|lptjoLu$X2zJ=-&_j!nh^8H5H5ZXkgFm!5W#AQc2@qeWg{{xeXP zcbKU-{V;m~hk0?{K8PC11N^Bc6t@W4G&?^R=3eH02FAh*GTT z$bW^#|AUUSWNi^!6&-1h-Z)f8I-)HR+kx+%Gw0P__Ivx@J+4}Q_wNnwefJ!^ENiY` zds$L)F$)1qknB7#K}i|=9aeJtL= zVg?EccB{#FS-`&qAf@kM0k7Pc_O^b6Gf^n+k*dPJ?G02H^=)r=SLaz1Cz{+12d7MZVZFc8J>U?~5kv&nXj`7*@YTE4o(G}v0cBv7bNQJrDW|9~va z1n-gTehEe8zkt@X~;a&rL3%Q(!!FQ$SkMnEf^u8tEVLu(BJ_bzMa27%p??$ zYGiAEa`nBdA70vIy*n10%?r852hR7M#Un3l964U(dY^NoruISY`?a5Ot-pKjAa4J2 z!q_~YbM750)c*zEtcKs@T7NI$i+I>^zLtDa>pI_V{lsfIA0VIj7;Lv&u^nhR|FRXO z8+RiJL1rT8p>se+zk|gh6pB+lZU}Ry6ukzeVP4EYa-H54AOR(XFbjq9p|~YEQ3jzp zr*0SqvJAWHCXD-^iTl^Y_iNJi sZ>0A*d%?oAtQ>t#uzo(uIvDRxBVpb7T+! 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() diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg new file mode 100644 index 0000000..be37112 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.cfg @@ -0,0 +1,2 @@ +[develop] +script_dir=$base/lib/saltybot_smooth_velocity diff --git a/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py b/jetson/ros2_ws/src/saltybot_smooth_velocity/setup.py new file mode 100644 index 0000000..28995de --- /dev/null +++ b/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", + ], + }, +)