From 92a062fd36a3461f408fe015d4c10d51d3ccbad6 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 13 Apr 2026 16:41:30 +0000 Subject: [PATCH 1/5] Add ROS 2 Jazzy pixi TurtleBot3 diff-drive tuning tutorial Co-authored-by: Mateusz Sadowski --- README.md | 4 + .../.gitignore | 2 + .../README.md | 151 ++++++++++++++ .../config/diff_drive_tuner_replay.yaml | 15 ++ .../config/diff_drive_tuner_sim.yaml | 15 ++ ...s_turtlebot3_gazebo.launch.cpython-312.pyc | Bin 0 -> 2701 bytes .../headless_turtlebot3_gazebo.launch.py | 72 +++++++ .../pixi.toml | 36 ++++ .../diff_drive_tuner.cpython-312.pyc | Bin 0 -> 10538 bytes .../scripts/diff_drive_tuner.py | 187 ++++++++++++++++++ .../scripts/run_mcap_player.sh | 43 ++++ .../scripts/setup_foxglove_mcap_player.sh | 24 +++ 12 files changed, 549 insertions(+) create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_replay.yaml create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_sim.yaml create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/__pycache__/headless_turtlebot3_gazebo.launch.cpython-312.pyc create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/headless_turtlebot3_gazebo.launch.py create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/__pycache__/diff_drive_tuner.cpython-312.pyc create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_tuner.py create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/run_mcap_player.sh create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/setup_foxglove_mcap_player.sh diff --git a/README.md b/README.md index c587800..0d64298 100644 --- a/README.md +++ b/README.md @@ -89,6 +89,10 @@ Below is a list of all tutorials available in this repository: - 📝 Tutorial for using ROS Client Library for Rust - 🔗 [Related Blog Post](https://foxglove.dev/blog/first-steps-using-rust-with-ros2) +## ROS Tutorials +### [ROS 2 Jazzy TurtleBot3 Diff-Drive Tuning with Pixi](ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md) +- 📝 Headless Gazebo simulation, MCAP recording, and live diff-drive tuning with Foxglove + ## Jupyter Notebooks ### [Analyze Your Robotics Data with Jupyter Notebooks](jupyter_notebooks/data_platform/README.md) - 📝 Load and analyze data in Jupyter Notebooks using Foxglove Data Management diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore new file mode 100644 index 0000000..72ba203 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore @@ -0,0 +1,2 @@ +bags/ +external/ diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md new file mode 100644 index 0000000..5deb657 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md @@ -0,0 +1,151 @@ +--- +title: "ROS 2 Jazzy TurtleBot3 Diff-Drive Tuning with Pixi" +short_description: "Headless Gazebo simulation, MCAP recording, and live diff-drive tuning with Foxglove" +--- + +# ROS 2 Jazzy + Pixi: TurtleBot3 diff-drive tuning workflow + +This tutorial creates an end-to-end, **pixi-only** workflow for: + +1. Launching a **headless Gazebo** simulation with TurtleBot3 (Burger) on ROS 2 Jazzy +2. Recording an MCAP bag from the simulation +3. Replaying that bag with [`foxglove_mcap_player`](https://github.com/botsandus/foxglove_mcap_player) +4. Tuning diff-drive behavior live from Foxglove by updating ROS parameters + +## What this tutorial contains + +- `pixi.toml`: ROS 2 Jazzy environment and repeatable tasks +- `launch/headless_turtlebot3_gazebo.launch.py`: Gazebo server-only launch (no GUI) +- `scripts/diff_drive_tuner.py`: runtime diff-drive filter and tuner node +- `config/diff_drive_tuner_sim.yaml`: tuning params for simulation (`/cmd_vel_raw` -> `/cmd_vel`) +- `config/diff_drive_tuner_replay.yaml`: tuning params for replay (`/cmd_vel` -> `/cmd_vel_tuned`) +- `scripts/setup_foxglove_mcap_player.sh`: clones/builds `foxglove_mcap_player` +- `scripts/run_mcap_player.sh`: runs player against a chosen MCAP file + +## 1) Enter the tutorial directory and install dependencies + +```bash +cd ros/ros2_jazzy_turtlebot3_diffdrive_pixi +pixi install +``` + +The pixi project uses RoboStack Jazzy packages from: + +- `https://prefix.dev/robostack-jazzy` +- `https://prefix.dev/conda-forge` + +## 2) Start headless TurtleBot3 Gazebo sim + +Terminal A: + +```bash +pixi run sim +``` + +This launches Gazebo in server mode (`-s`) with no GUI client. + +## 3) Start the diff-drive tuner for simulation + +Terminal B: + +```bash +pixi run tune-sim +``` + +The tuner subscribes to `/cmd_vel_raw` and publishes tuned velocity commands to `/cmd_vel`. +Tuneable parameters are namespaced under `/diff_drive_tuner`. + +## 4) Drive the robot and record MCAP + +Terminal C (drive): + +```bash +pixi run teleop +``` + +Or publish a continuous circle command: + +```bash +pixi run drive-circle +``` + +Terminal D (record): + +```bash +pixi run bag-record +``` + +This creates `bags/tb3_/` with MCAP storage and captures: + +- `/clock` +- `/cmd_vel_raw` +- `/cmd_vel` +- `/odom` +- `/tf`, `/tf_static` +- `/joint_states` +- `/scan` + +Stop recording with `Ctrl+C`. + +## 5) Start Foxglove bridge (optional live graph introspection) + +Terminal E: + +```bash +pixi run foxglove-bridge +``` + +Open Foxglove and connect to `ws://:8765`. + +## 6) Build and run foxglove_mcap_player for replay + +First-time setup (clones and builds the player): + +```bash +pixi run player-setup +``` + +Replay a bag with Foxglove websocket server + ROS republish: + +```bash +MCAP_FILE="$(ls -d bags/tb3_* | tail -n 1)/tb3_0.mcap" pixi run mcap-play +``` + +You can set `PLAYER_HOST` and `PLAYER_PORT` if needed: + +```bash +PLAYER_HOST=0.0.0.0 PLAYER_PORT=8766 MCAP_FILE=... pixi run mcap-play +``` + +## 7) Tune diff-drive during replay in Foxglove + +Terminal F: + +```bash +pixi run tune-replay +``` + +Replay mode subscribes to `/cmd_vel` and publishes tuned commands to `/cmd_vel_tuned`. +This keeps original and tuned command streams separate for comparison. + +In Foxglove, add panels for: + +- `/cmd_vel` (original) +- `/cmd_vel_tuned` (after tuning) +- `/odom` and `/tf` + +Adjust ROS parameters on `/diff_drive_tuner` live: + +- `linear_scale` +- `angular_scale` +- `max_linear_speed` +- `max_angular_speed` +- `linear_accel_limit` +- `angular_accel_limit` +- `cmd_timeout` + +## Notes + +- Default robot model is `burger` via pixi activation env (`TURTLEBOT3_MODEL`). +- If your bag contains a different file name than `tb3_0.mcap`, point `MCAP_FILE` to the actual path. +- `foxglove_mcap_player` is not currently packaged in RoboStack Jazzy, so this tutorial builds it from source in `external/`. diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_replay.yaml b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_replay.yaml new file mode 100644 index 0000000..6ee4fe1 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_replay.yaml @@ -0,0 +1,15 @@ +diff_drive_tuner: + ros__parameters: + use_sim_time: true + input_topic: "/cmd_vel" + output_topic: "/cmd_vel_tuned" + linear_scale: 1.0 + angular_scale: 1.0 + linear_bias: 0.0 + angular_bias: 0.0 + max_linear_speed: 0.22 + max_angular_speed: 2.84 + linear_accel_limit: 0.4 + angular_accel_limit: 1.2 + cmd_timeout: 0.5 + publish_rate_hz: 30.0 diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_sim.yaml b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_sim.yaml new file mode 100644 index 0000000..bcc91b9 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_sim.yaml @@ -0,0 +1,15 @@ +diff_drive_tuner: + ros__parameters: + use_sim_time: true + input_topic: "/cmd_vel_raw" + output_topic: "/cmd_vel" + publish_rate_hz: 30.0 + linear_scale: 1.0 + angular_scale: 1.0 + linear_bias: 0.0 + angular_bias: 0.0 + max_linear_speed: 0.22 + max_angular_speed: 2.84 + linear_accel_limit: 0.6 + angular_accel_limit: 3.5 + cmd_timeout: 0.5 diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/__pycache__/headless_turtlebot3_gazebo.launch.cpython-312.pyc b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/__pycache__/headless_turtlebot3_gazebo.launch.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..efd9b24b06973661aec7150f63b90dfa3f560803 GIT binary patch literal 2701 zcmb_eO-vg{6yEjEdc9s79PEHG5F(YNPQ#Bv{tihhk`O}U{3tY0WmQ@&-oY$c?^-jv zBv_{Mp%>Ijh!Z{Zz^zo|kYm(Sj+J_Gw;z=B1^xMy0=YnIYvd$qB+|K5^0Z|H{%%@`KU1L8xMBf4hj zb85kYGW7y;5SSL!^G(R?fg$6s*lO-t_Py8sfD+uI{*t!rH_2&mn!>gG3V67o7B}eO@+~gs;f7n>kQr&~RfMOJ zme0IL5>iBuB?YJXUiL|5>VN9^dt7c{74-1qY|ue zO7fi%=`fQnVimp;t_YP#MXW?Cu}Vjwbuwp|%+WhIPSVxXz^q}8O9i=12rNJYNrke6 z)FS3IyU<|nu>my&mE)jTGM6NKHe&Z!v=Av#YEd%PBEZ}esbnBBE;(b+Srjm#MT-I`AyYPi zRI+9@70mqg{}8 zv_e6ibU3J*pokzXW0_8zZ0m2rPU}04wV}7?LeqaLYe8ot3OlLhmOGH93Qj0anU~%Q zNzCOoL(`f!LqrtYmxB{koIvF2A)rEl>{3szA@X%$8U_Ft0mRAwgb;Te01iX=2SwCo-ESU-Z99+$Dhyks=|6WsM{jNDMqQk8 zG;mBhvw6Cfy6|i2^2^ladg{~taIl}4PwmH;?$N_rC+gx=*LZjT_R+pWcDuRiU1C@4 z|6Ls17JKbqYMau@ZSnZFnBI+t!@{0FB67P-B*^WNcrlq6Op^hpW%_WDR9yk&G}|wf zk>)cA*$D|_WSO01IOZIH5jjtvCdf%hJ2h!m;^R&6;bS5>2Ol|!n`o{>)+{nr({hyA zc}|bhj|?29CO;wF8~zfhT9yW`z;QCAj~kRB-L9Wy**%V7g+G~VJIqJFG2kTwc9>f` T%-LP;63f+w&hIm1v{n5J)YqF! literal 0 HcmV?d00001 diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/headless_turtlebot3_gazebo.launch.py b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/headless_turtlebot3_gazebo.launch.py new file mode 100644 index 0000000..fb8938b --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/headless_turtlebot3_gazebo.launch.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description() -> LaunchDescription: + turtlebot3_gazebo_dir = get_package_share_directory("turtlebot3_gazebo") + ros_gz_sim_dir = get_package_share_directory("ros_gz_sim") + launch_dir = os.path.join(turtlebot3_gazebo_dir, "launch") + + default_world = os.path.join(turtlebot3_gazebo_dir, "worlds", "empty_world.world") + + use_sim_time = LaunchConfiguration("use_sim_time") + x_pose = LaunchConfiguration("x_pose") + y_pose = LaunchConfiguration("y_pose") + world = LaunchConfiguration("world") + + declare_use_sim_time = DeclareLaunchArgument("use_sim_time", default_value="true") + declare_x_pose = DeclareLaunchArgument("x_pose", default_value="0.0") + declare_y_pose = DeclareLaunchArgument("y_pose", default_value="0.0") + declare_world = DeclareLaunchArgument("world", default_value=default_world) + + set_gz_resource_path = AppendEnvironmentVariable( + "GZ_SIM_RESOURCE_PATH", + os.path.join(turtlebot3_gazebo_dir, "models"), + ) + + gz_server = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, "launch", "gz_sim.launch.py") + ), + launch_arguments={ + "gz_args": ["-r -s -v2 ", world], + "on_exit_shutdown": "true", + }.items(), + ) + + robot_state_publisher = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, "robot_state_publisher.launch.py") + ), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ) + + spawn_turtlebot = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, "spawn_turtlebot3.launch.py") + ), + launch_arguments={ + "x_pose": x_pose, + "y_pose": y_pose, + }.items(), + ) + + return LaunchDescription( + [ + declare_use_sim_time, + declare_x_pose, + declare_y_pose, + declare_world, + set_gz_resource_path, + gz_server, + robot_state_publisher, + spawn_turtlebot, + ] + ) diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml new file mode 100644 index 0000000..49d4d00 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml @@ -0,0 +1,36 @@ +[workspace] +name = "ros2-jazzy-turtlebot3-diffdrive-pixi" +channels = ["https://prefix.dev/robostack-jazzy", "https://prefix.dev/conda-forge"] +platforms = ["linux-64"] + +[activation.env] +TURTLEBOT3_MODEL = "burger" + +[dependencies] +python = ">=3.12,<3.13" +git = "*" +colcon-common-extensions = "*" +lz4-c = "*" +zstd = "*" +ros-jazzy-ros-base = "*" +ros-jazzy-rclpy = "*" +ros-jazzy-nav-msgs = "*" +ros-jazzy-geometry-msgs = "*" +ros-jazzy-tf2-ros = "*" +ros-jazzy-ros-gz-sim = "*" +ros-jazzy-rosbag2-storage-mcap = "*" +ros-jazzy-foxglove-bridge = "*" +ros-jazzy-teleop-twist-keyboard = "*" +ros-jazzy-turtlebot3-gazebo = "*" +ros-jazzy-turtlebot3-simulations = "*" + +[tasks] +sim = "bash -lc 'ros2 launch \"$PIXI_PROJECT_ROOT/launch/headless_turtlebot3_gazebo.launch.py\"'" +tune-sim = "python ./scripts/diff_drive_tuner.py --ros-args --params-file ./config/diff_drive_tuner_sim.yaml" +tune-replay = "python ./scripts/diff_drive_tuner.py --ros-args --params-file ./config/diff_drive_tuner_replay.yaml" +teleop = "ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/cmd_vel_raw" +drive-circle = "ros2 topic pub --rate 10 /cmd_vel_raw geometry_msgs/msg/Twist '{linear: {x: 0.2}, angular: {z: 0.4}}'" +bag-record = "bash -lc 'mkdir -p bags && ros2 bag record -s mcap -o bags/tb3_$(date +%Y%m%d_%H%M%S) /clock /cmd_vel_raw /cmd_vel /odom /tf /tf_static /joint_states /scan'" +foxglove-bridge = "ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=8765 address:=0.0.0.0" +player-setup = "bash ./scripts/setup_foxglove_mcap_player.sh" +mcap-play = { cmd = "bash -lc ': \"${MCAP_FILE:?Set MCAP_FILE to a .mcap path}\"; bash ./scripts/run_mcap_player.sh \"$MCAP_FILE\" \"${PLAYER_PORT:-8766}\" \"${PLAYER_HOST:-127.0.0.1}\"'", depends-on = ["player-setup"] } diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/__pycache__/diff_drive_tuner.cpython-312.pyc b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/__pycache__/diff_drive_tuner.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..44899090b4787f89783bc8f742eb8a382e9e90a8 GIT binary patch literal 10538 zcmb_CTWlLwb~Ahq-;^xb)YFtGKPXx9LsI;Z-*o_p`P=bV{;bGhskq(8KL5&cOEMg0jIT4IWojb*4jq*#ijV^o|T zr)jJkVuo=8l*X7bZW=ccm?>tCTgEMw%s4~pmY6MWAGbr@$~jmD{xC-&9ml+HRL3*!W*gXgzd6jxZj|B~i5x7GZO$3cRE&y-C3>Qhj{(isz+voPrCi!V$CLH1R^GN~! zheA`~bb2lX;}K)r&7{~LVxtoiEFZnag=V5}ME463J~|@``w<*c;bJPm@qIIMia8Ps z$7lFD;P1o-ZHMeGwdQbVX8+2#II{Gkysszk*q1Zy+W-uuSR*N(=Mth~5ySi>Cn`1= zbcp3*VtA71LRXVV=GU0-2}MFt0ntG9ek?FA8klP)2Ele#6ku zJP$n5zzcd~4SIvv3}gQVAd+C=jNzL?(8Ox13{EgX9VF=Iy$E!~lOP>2 zIUD9#9yULPP~kjecd3Ha`{3M)bw|O|m~j=H^$%W>oUM1yuUYMx!JnreTwk%atEkRB z*}&iQWDY;LE;-tBrVGo5A77XHFF=j22Tl?A>&a5AZQO#&FF_(4HBPe>93xkTnxI+Z z+s<(#YvN2S%^D_5tQj`BnRT%iC@lnT<*Wo}VHtoiaExr6y$QInG#n-qYX_(kdf=gQ z0n9pKV4YBRXQ(&manDsM=vLgrumy&3XI#S#z&oJ(SVZ5b(86(&b^A5se@VkO27sCt zVAXzQ#^-4(MZ;Q50i-gq2G*!;?(e06Io71Hu-2M1%ob>Dti2{5P)apvK)y9;+^nZ2 zjhC&fNmI{mt4Y(qHrAxs&h9ADu&`nwTwjt%3lB!ArLQLI14?bzQly0nPNlR0PP-yN zu|o@0m5Q@Ni(D+&WU+<2j3m|tEmSjV%L6T!-l~I^TW@WHmPcD$Ku^>KB*AX&(dwmAqa-`Ew*=Q}&Z6+E?JNHN8Z0Hsj3-RGu#hFZr zB^;yS6lW}&;KF=Jh=gNYQJJovQe5H0WGbe?6-SZkW;87DJ{Xqb)Cdryc<~a@IL&Yz zJNfJPFnxYXsY8@TED+o$|BmT@Pbu}qF5yT7h8c^-qhhh+m#36%TBmZXDiPCZ#etI( zqj3)AHK{_MoKie9shhE=@H$Lcn z%rSN|9GQm4d-1SYUB87bPxu#yyon~)eKKZXhzlhjnX$$}J+G=+P|fHrnWrvPgGQd2202NTp# zx)$ds9xtMx5e{-9IU5rAQ_xiE@N5%(BE*wP@pX;`%X|bd{3xQ0K&n~hMY9sOc2Vu7 zn=5x`33nd43%ei#YtvnSe|lm1op{Dr(3VQyl)}=v&zQZcHK|Vsy6FP>4`|dB2-s~? z<>G`sm{f~gZ~_+jRiwN09I?}@;n4c7&T|@`!$VKes4)+OVLBd{dOdyYwaj^*~81s|Z`^gQU2>vu`;bMDGE$iZV$@Yssu7~s}C4e!im zO!_%q6ql}k#stZ#@i@#)j*)I)_rb7X>F6n-s7%$CtNJpfVQ&LQH`Gd;&{?`;1pd0A zR>Do`23iRQT&kEPMgCPx62jjk>W0|75jg0ESc$I#$5#ce`Sf?pWjstktD9>TJgUHT z9u}6q17_$Hx~~=KttJt@B$8M-_0_3xOKaN@IDxB0+|C&hokNnKU#nIxm0MmtaA2p} zOZ~LkycH#^&%<2vd0Smg3p~%SJ*8AuJkKHqG@O1=nR)X(Q*B;lI@JOcRYnqmxu^-A z-lf$`<1U8~JACBv3zZbu0NdR*UmpreUuXizMw8Kux~@Xr7(e*`kcS~M3GI?W$O z=uymYck;(D8^Y`~X2_BF{QMcL9mnhhW~c@ELy#%fqTUbMO6r~*WyKE0bkW{Y99Y*L z>r&$(8qQs8YG?y1Dn>A%1XO{btkk61GN{(l{z_$4dG%ZcD&ZR30-&@SeGkXq9e?=h zyRR+{%U#b%@N0bLaf5vHk_5lTOBs7%d(*?|cc&l5-;FP^^6sM&{I(x`JS-m{li;^~ zEaUjIuWfNEJ0SNANj*ck?xDxmGTWvrWZ5rCoXTsP(XFrDLFcGj=r_Jh6h&{14|~^e^`QF^Wl8mk<8gA4%Y+YUmRI$ z-Ba*3$leag+aY`RO5VM)w_Ead7rbo*j*#pj$=kbOHMO}jSDxCb_F!3$pyUn8-X6)@ zQ}DJEI6|^#C2t?}Xm@AEo;oQ|!jo`??&!)G@7ovb_nZaaZm2pI9QRzHn(x~dY%iMykcy9w_c-2{PyQ%stUqp7m~&JSFg2EriO*hySE z9yJvYgQ1AOdm-)g$5VpnzsdPe`VWA2Vi!{2wF<&SDyBd*I4mR+YR4e<>4q%rDs?&G zKfv!pSc`V7iymr;1Soot!dK>^m9W6zT9D>n1B#$YF(n{)t;UFPFY#v)Tr`Y+0(Hf# z?&NaIh#v+-x}#Ffe8(62DUb;m>_t7(@)gx!tIwP-cpC1f7t*=53zFx;$Ag~^{c$Ka zeq-hGjkO)!g`Hi6_Q3C^-kVx{HQSnNdFH9v=&S#VGS=0vGnBVc?RZJ@T>3cm>Fqz> z&fR!z<+;}?JGSLo4p;5ix8mtr9{lH_e;E2Ou+o3N*stYC72?Gkc~5sX_50h8Za==7 z-+x-89W0{_z45e*W zeM6RFsnxDiZCk=Ms6VnMM}yX@21nhW5K9+s)k6cZ+#+oaj=B+AlcOHXs=-k|!ZkUX zw>U=KXsOB3vPF)%Pg9elb&DMB+G5w_sGCkT2jpFZC9? zv^w;!G7@-ifp-BU&?S0Y9cXu}(DEGSgZiu-RIHQvy!<`Aa6WkVRix_Fo92VpUPan8 zp9&l{fh%686$xf(Qb90Fd<{qv(DrHdQu!8+_h6~5)v8l(8gI3TzI)!7hEoQ;2Q*Hl zQgbX}ROrHx1TlQi81IKVkGp{H#0*!F--Q{jOPLL#8x_PxAg08Zs3v&a3&9p1AIc9x zrWgg0S6moQ)g#r!P*BVh5VjRnkBK-)=r*ZdQprIoIYT7$LYyH8eFM+roE8j&5Un=? z77s+&i^dGRNkqBxC{Cq8BZug?`sNwlz$@E|J<5Un7 zEj4v#+=b?Lxp|+|yf5SVbL&oYdk#tdL(3=f{xhI7(39-#d~3 z7Q)(B#yU56G2b~VcRnX|K9_HOUY%25F9yJdq~K8Q`1AST3v%!!Dfm)8@G_$PYhA%? zm)w0^>OP)3@nXLFs@#2D>b{=u8b@?T8S`r6-qE^vO>WySwe8OxIFoNXE4N*g+AijI zjAooq9G?3}7mns2K<8N=ko(U|{pVNwV47~bKesTK^K=o{aJS^$y(Iqa++WYFdJh(S zt+KCI^7VeeEnkxdN2I}#kL&ZkXEVe1M$6c_m%HRc=cGgDR{ZBG*bz^&>-WJ&!By`7 zVRu0C9r&ll$0qs6sB~oX;{o~dE7IjxayLSG-)kD*p@pHGqqE@j%FcGl*}f?L?)H1P zSDihD?alJ`J<|3)Sz|UP?LGE*_+Q8VWh}q_a>n#4$9IiSROG0(H;Nxo{9xRBs;JyZ|LH{i#K~H6i>3L7}lMk&`(q0n2I5s!tKr5~+RP%Jl+;_<)+w$_jP%mdkmZ1mC2bqcDv6VGp8 zxiQdYewJQ8Nt=%>n*W=Ef(U{`9FZXga*d5AN_GMDF zxG{X6gzF257#pxhDwD$9rx+E9SmCweOf;d?UFPO)CgJO!(FA-1#;0aP#i`vv3HT07 zVT9LHBAc8|;FnLF%JjG}d<;u^!xg)?>sogu@Jl_!NvUSO=eM&B4?sdE7|+ zJY-7UB$tG*;P^Rw2`uzMMh=ux4^o2*79=wPUz({1JVg-xL~-_^Ja}~X6psoh*&5`; zkz-hgkHPRckvowVk6%AMqSQ@}A9Q@n99K{GN&X#Z;&u`;kbw+n`fCeCxBiK8enADk zpbmXOwR}PCTDQ=2?*lkoP%Nnh9yMq^9^AlUow3j(^di8p%$n8m@h-Le@cagWKdpb! nKp%NbX)goU% literal 0 HcmV?d00001 diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_tuner.py b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_tuner.py new file mode 100644 index 0000000..e6ef4d6 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_tuner.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python3 + +from typing import Optional + +import rclpy +from geometry_msgs.msg import Twist +from rcl_interfaces.msg import SetParametersResult +from rclpy.node import Node +from rclpy.parameter import Parameter + + +def clamp(value: float, minimum: float, maximum: float) -> float: + return max(minimum, min(maximum, value)) + + +def move_toward(current: float, target: float, max_delta: float) -> float: + if max_delta <= 0.0: + return target + delta = target - current + if abs(delta) <= max_delta: + return target + return current + max_delta if delta > 0.0 else current - max_delta + + +class DiffDriveTuner(Node): + def __init__(self) -> None: + super().__init__("diff_drive_tuner") + + self.declare_parameter("input_topic", "/cmd_vel_raw") + self.declare_parameter("output_topic", "/cmd_vel") + self.declare_parameter("linear_scale", 1.0) + self.declare_parameter("angular_scale", 1.0) + self.declare_parameter("linear_bias", 0.0) + self.declare_parameter("angular_bias", 0.0) + self.declare_parameter("max_linear_speed", 0.4) + self.declare_parameter("max_angular_speed", 1.8) + self.declare_parameter("linear_accel_limit", 0.8) + self.declare_parameter("angular_accel_limit", 2.5) + self.declare_parameter("cmd_timeout", 0.5) + self.declare_parameter("publish_rate_hz", 30.0) + + self._sub = None + self._pub = None + self._timer = None + + self._input_topic = "" + self._output_topic = "" + self._linear_scale = 1.0 + self._angular_scale = 1.0 + self._linear_bias = 0.0 + self._angular_bias = 0.0 + self._max_linear = 0.4 + self._max_angular = 1.8 + self._linear_accel_limit = 0.8 + self._angular_accel_limit = 2.5 + self._cmd_timeout = 0.5 + + self._last_msg_time: Optional[float] = None + self._last_publish_time: Optional[float] = None + self._last_output = Twist() + + self._load_params_and_rewire_topics() + self.add_on_set_parameters_callback(self._on_set_parameters) + + self.get_logger().info( + f"DiffDrive tuner started. input={self._input_topic} output={self._output_topic}" + ) + + def _on_cmd_vel(self, msg: Twist) -> None: + now = self._now_s() + self._last_msg_time = now + self._publish_smoothed(msg, now) + + def _on_timer(self) -> None: + now = self._now_s() + if self._cmd_timeout <= 0.0 or self._last_msg_time is None: + return + if (now - self._last_msg_time) < self._cmd_timeout: + return + zero = Twist() + self._publish_smoothed(zero, now) + + def _publish_smoothed(self, msg: Twist, now: float) -> None: + raw_linear = msg.linear.x * self._linear_scale + self._linear_bias + raw_angular = msg.angular.z * self._angular_scale + self._angular_bias + + target_linear = clamp(raw_linear, -self._max_linear, self._max_linear) + target_angular = clamp(raw_angular, -self._max_angular, self._max_angular) + + dt = 0.0 + if self._last_publish_time is not None: + dt = max(0.0, now - self._last_publish_time) + + if dt > 0.0: + target_linear = move_toward( + self._last_output.linear.x, + target_linear, + self._linear_accel_limit * dt, + ) + target_angular = move_toward( + self._last_output.angular.z, + target_angular, + self._angular_accel_limit * dt, + ) + + out = Twist() + out.linear.x = target_linear + out.angular.z = target_angular + self._pub.publish(out) + self._last_publish_time = now + self._last_output = out + + def _on_set_parameters(self, params: list[Parameter]) -> SetParametersResult: + for param in params: + if param.name in {"max_linear_speed", "max_angular_speed"} and param.value <= 0.0: + return SetParametersResult( + successful=False, + reason=f"{param.name} must be > 0", + ) + if param.name in {"linear_accel_limit", "angular_accel_limit"} and param.value < 0.0: + return SetParametersResult( + successful=False, + reason=f"{param.name} must be >= 0", + ) + if param.name == "publish_rate_hz" and param.value <= 0.0: + return SetParametersResult( + successful=False, + reason="publish_rate_hz must be > 0", + ) + if param.name == "cmd_timeout" and param.value < 0.0: + return SetParametersResult( + successful=False, + reason="cmd_timeout must be >= 0", + ) + + self._load_params_and_rewire_topics() + return SetParametersResult(successful=True) + + def _load_params_and_rewire_topics(self) -> None: + previous_input = self._input_topic + previous_output = self._output_topic + + self._input_topic = str(self.get_parameter("input_topic").value) + self._output_topic = str(self.get_parameter("output_topic").value) + self._linear_scale = float(self.get_parameter("linear_scale").value) + self._angular_scale = float(self.get_parameter("angular_scale").value) + self._linear_bias = float(self.get_parameter("linear_bias").value) + self._angular_bias = float(self.get_parameter("angular_bias").value) + self._max_linear = float(self.get_parameter("max_linear_speed").value) + self._max_angular = float(self.get_parameter("max_angular_speed").value) + self._linear_accel_limit = float(self.get_parameter("linear_accel_limit").value) + self._angular_accel_limit = float(self.get_parameter("angular_accel_limit").value) + self._cmd_timeout = float(self.get_parameter("cmd_timeout").value) + publish_rate = float(self.get_parameter("publish_rate_hz").value) + + if self._pub is None or previous_output != self._output_topic: + if self._pub is not None: + self.destroy_publisher(self._pub) + self._pub = self.create_publisher(Twist, self._output_topic, 10) + + if self._sub is None or previous_input != self._input_topic: + if self._sub is not None: + self.destroy_subscription(self._sub) + self._sub = self.create_subscription(Twist, self._input_topic, self._on_cmd_vel, 10) + + if self._timer is not None: + self.destroy_timer(self._timer) + self._timer = self.create_timer(1.0 / publish_rate, self._on_timer) + + def _now_s(self) -> float: + return self.get_clock().now().nanoseconds / 1e9 + + +def main() -> None: + rclpy.init() + node = DiffDriveTuner() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/run_mcap_player.sh b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/run_mcap_player.sh new file mode 100644 index 0000000..9a9ddf2 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/run_mcap_player.sh @@ -0,0 +1,43 @@ +#!/usr/bin/env bash +set -euo pipefail + +PROJECT_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +PLAYER_REPO_DIR="${PROJECT_ROOT}/external/foxglove_mcap_player" +PLAYER_WORKSPACE_DIR="${PLAYER_REPO_DIR}/ros2_ws" + +MCAP_FILE="${MCAP_FILE:-}" +PORT="${PLAYER_PORT:-8766}" +HOST="${PLAYER_HOST:-127.0.0.1}" + +if [[ $# -ge 1 ]]; then + MCAP_FILE="$1" +fi +if [[ $# -ge 2 ]]; then + PORT="$2" +fi +if [[ $# -ge 3 ]]; then + HOST="$3" +fi + +if [[ -z "${MCAP_FILE}" ]]; then + echo "Usage: $0 [port] [host]" + echo "Or set MCAP_FILE (and optional PLAYER_PORT/PLAYER_HOST) in the environment." + exit 1 +fi + +if [[ ! -f "${MCAP_FILE}" ]]; then + echo "MCAP file not found: ${MCAP_FILE}" + exit 1 +fi + +if [[ ! -d "${PLAYER_WORKSPACE_DIR}" ]]; then + echo "foxglove_mcap_player workspace not found. Run: pixi run player-setup" + exit 1 +fi + +source "${PLAYER_WORKSPACE_DIR}/install/setup.bash" + +ros2 run foxglove_mcap_player foxglove_mcap_player --ros-args \ + -p file:="${MCAP_FILE}" \ + -p port:="${PORT}" \ + -p host:="${HOST}" diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/setup_foxglove_mcap_player.sh b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/setup_foxglove_mcap_player.sh new file mode 100644 index 0000000..4b41e6a --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/setup_foxglove_mcap_player.sh @@ -0,0 +1,24 @@ +#!/usr/bin/env bash + +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +ROOT_DIR="$(cd "${SCRIPT_DIR}/.." && pwd)" +PLAYER_DIR="${ROOT_DIR}/external/foxglove_mcap_player" +PLAYER_WS="${PLAYER_DIR}/ros2_ws" + +mkdir -p "${ROOT_DIR}/external" + +if [[ ! -d "${PLAYER_DIR}/.git" ]]; then + git clone --depth=1 https://github.com/botsandus/foxglove_mcap_player.git "${PLAYER_DIR}" +fi + +if [[ ! -d "${PLAYER_WS}" ]]; then + echo "Expected workspace missing: ${PLAYER_WS}" + exit 1 +fi + +cd "${PLAYER_WS}" +colcon build --packages-select foxglove_mcap_player + +echo "foxglove_mcap_player built at: ${PLAYER_WS}" From 7246e55dbfd2534a9be6c6e294b2bac6c6f3849f Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 13 Apr 2026 16:42:00 +0000 Subject: [PATCH 2/5] Ignore and remove generated pycache artifacts Co-authored-by: Mateusz Sadowski --- ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore index 72ba203..2f30ea7 100644 --- a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore @@ -1,2 +1,4 @@ bags/ external/ +**/__pycache__/ +*.pyc From e3aa43fb9ee1ef4c2559f475ac2af095993c38d1 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 13 Apr 2026 16:42:18 +0000 Subject: [PATCH 3/5] Remove tracked pycache files from tutorial Co-authored-by: Mateusz Sadowski --- ...ess_turtlebot3_gazebo.launch.cpython-312.pyc | Bin 2701 -> 0 bytes .../diff_drive_tuner.cpython-312.pyc | Bin 10538 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/__pycache__/headless_turtlebot3_gazebo.launch.cpython-312.pyc delete mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/__pycache__/diff_drive_tuner.cpython-312.pyc diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/__pycache__/headless_turtlebot3_gazebo.launch.cpython-312.pyc b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/launch/__pycache__/headless_turtlebot3_gazebo.launch.cpython-312.pyc deleted file mode 100644 index efd9b24b06973661aec7150f63b90dfa3f560803..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2701 zcmb_eO-vg{6yEjEdc9s79PEHG5F(YNPQ#Bv{tihhk`O}U{3tY0WmQ@&-oY$c?^-jv zBv_{Mp%>Ijh!Z{Zz^zo|kYm(Sj+J_Gw;z=B1^xMy0=YnIYvd$qB+|K5^0Z|H{%%@`KU1L8xMBf4hj zb85kYGW7y;5SSL!^G(R?fg$6s*lO-t_Py8sfD+uI{*t!rH_2&mn!>gG3V67o7B}eO@+~gs;f7n>kQr&~RfMOJ zme0IL5>iBuB?YJXUiL|5>VN9^dt7c{74-1qY|ue zO7fi%=`fQnVimp;t_YP#MXW?Cu}Vjwbuwp|%+WhIPSVxXz^q}8O9i=12rNJYNrke6 z)FS3IyU<|nu>my&mE)jTGM6NKHe&Z!v=Av#YEd%PBEZ}esbnBBE;(b+Srjm#MT-I`AyYPi zRI+9@70mqg{}8 zv_e6ibU3J*pokzXW0_8zZ0m2rPU}04wV}7?LeqaLYe8ot3OlLhmOGH93Qj0anU~%Q zNzCOoL(`f!LqrtYmxB{koIvF2A)rEl>{3szA@X%$8U_Ft0mRAwgb;Te01iX=2SwCo-ESU-Z99+$Dhyks=|6WsM{jNDMqQk8 zG;mBhvw6Cfy6|i2^2^ladg{~taIl}4PwmH;?$N_rC+gx=*LZjT_R+pWcDuRiU1C@4 z|6Ls17JKbqYMau@ZSnZFnBI+t!@{0FB67P-B*^WNcrlq6Op^hpW%_WDR9yk&G}|wf zk>)cA*$D|_WSO01IOZIH5jjtvCdf%hJ2h!m;^R&6;bS5>2Ol|!n`o{>)+{nr({hyA zc}|bhj|?29CO;wF8~zfhT9yW`z;QCAj~kRB-L9Wy**%V7g+G~VJIqJFG2kTwc9>f` T%-LP;63f+w&hIm1v{n5J)YqF! diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/__pycache__/diff_drive_tuner.cpython-312.pyc b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/__pycache__/diff_drive_tuner.cpython-312.pyc deleted file mode 100644 index 44899090b4787f89783bc8f742eb8a382e9e90a8..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10538 zcmb_CTWlLwb~Ahq-;^xb)YFtGKPXx9LsI;Z-*o_p`P=bV{;bGhskq(8KL5&cOEMg0jIT4IWojb*4jq*#ijV^o|T zr)jJkVuo=8l*X7bZW=ccm?>tCTgEMw%s4~pmY6MWAGbr@$~jmD{xC-&9ml+HRL3*!W*gXgzd6jxZj|B~i5x7GZO$3cRE&y-C3>Qhj{(isz+voPrCi!V$CLH1R^GN~! zheA`~bb2lX;}K)r&7{~LVxtoiEFZnag=V5}ME463J~|@``w<*c;bJPm@qIIMia8Ps z$7lFD;P1o-ZHMeGwdQbVX8+2#II{Gkysszk*q1Zy+W-uuSR*N(=Mth~5ySi>Cn`1= zbcp3*VtA71LRXVV=GU0-2}MFt0ntG9ek?FA8klP)2Ele#6ku zJP$n5zzcd~4SIvv3}gQVAd+C=jNzL?(8Ox13{EgX9VF=Iy$E!~lOP>2 zIUD9#9yULPP~kjecd3Ha`{3M)bw|O|m~j=H^$%W>oUM1yuUYMx!JnreTwk%atEkRB z*}&iQWDY;LE;-tBrVGo5A77XHFF=j22Tl?A>&a5AZQO#&FF_(4HBPe>93xkTnxI+Z z+s<(#YvN2S%^D_5tQj`BnRT%iC@lnT<*Wo}VHtoiaExr6y$QInG#n-qYX_(kdf=gQ z0n9pKV4YBRXQ(&manDsM=vLgrumy&3XI#S#z&oJ(SVZ5b(86(&b^A5se@VkO27sCt zVAXzQ#^-4(MZ;Q50i-gq2G*!;?(e06Io71Hu-2M1%ob>Dti2{5P)apvK)y9;+^nZ2 zjhC&fNmI{mt4Y(qHrAxs&h9ADu&`nwTwjt%3lB!ArLQLI14?bzQly0nPNlR0PP-yN zu|o@0m5Q@Ni(D+&WU+<2j3m|tEmSjV%L6T!-l~I^TW@WHmPcD$Ku^>KB*AX&(dwmAqa-`Ew*=Q}&Z6+E?JNHN8Z0Hsj3-RGu#hFZr zB^;yS6lW}&;KF=Jh=gNYQJJovQe5H0WGbe?6-SZkW;87DJ{Xqb)Cdryc<~a@IL&Yz zJNfJPFnxYXsY8@TED+o$|BmT@Pbu}qF5yT7h8c^-qhhh+m#36%TBmZXDiPCZ#etI( zqj3)AHK{_MoKie9shhE=@H$Lcn z%rSN|9GQm4d-1SYUB87bPxu#yyon~)eKKZXhzlhjnX$$}J+G=+P|fHrnWrvPgGQd2202NTp# zx)$ds9xtMx5e{-9IU5rAQ_xiE@N5%(BE*wP@pX;`%X|bd{3xQ0K&n~hMY9sOc2Vu7 zn=5x`33nd43%ei#YtvnSe|lm1op{Dr(3VQyl)}=v&zQZcHK|Vsy6FP>4`|dB2-s~? z<>G`sm{f~gZ~_+jRiwN09I?}@;n4c7&T|@`!$VKes4)+OVLBd{dOdyYwaj^*~81s|Z`^gQU2>vu`;bMDGE$iZV$@Yssu7~s}C4e!im zO!_%q6ql}k#stZ#@i@#)j*)I)_rb7X>F6n-s7%$CtNJpfVQ&LQH`Gd;&{?`;1pd0A zR>Do`23iRQT&kEPMgCPx62jjk>W0|75jg0ESc$I#$5#ce`Sf?pWjstktD9>TJgUHT z9u}6q17_$Hx~~=KttJt@B$8M-_0_3xOKaN@IDxB0+|C&hokNnKU#nIxm0MmtaA2p} zOZ~LkycH#^&%<2vd0Smg3p~%SJ*8AuJkKHqG@O1=nR)X(Q*B;lI@JOcRYnqmxu^-A z-lf$`<1U8~JACBv3zZbu0NdR*UmpreUuXizMw8Kux~@Xr7(e*`kcS~M3GI?W$O z=uymYck;(D8^Y`~X2_BF{QMcL9mnhhW~c@ELy#%fqTUbMO6r~*WyKE0bkW{Y99Y*L z>r&$(8qQs8YG?y1Dn>A%1XO{btkk61GN{(l{z_$4dG%ZcD&ZR30-&@SeGkXq9e?=h zyRR+{%U#b%@N0bLaf5vHk_5lTOBs7%d(*?|cc&l5-;FP^^6sM&{I(x`JS-m{li;^~ zEaUjIuWfNEJ0SNANj*ck?xDxmGTWvrWZ5rCoXTsP(XFrDLFcGj=r_Jh6h&{14|~^e^`QF^Wl8mk<8gA4%Y+YUmRI$ z-Ba*3$leag+aY`RO5VM)w_Ead7rbo*j*#pj$=kbOHMO}jSDxCb_F!3$pyUn8-X6)@ zQ}DJEI6|^#C2t?}Xm@AEo;oQ|!jo`??&!)G@7ovb_nZaaZm2pI9QRzHn(x~dY%iMykcy9w_c-2{PyQ%stUqp7m~&JSFg2EriO*hySE z9yJvYgQ1AOdm-)g$5VpnzsdPe`VWA2Vi!{2wF<&SDyBd*I4mR+YR4e<>4q%rDs?&G zKfv!pSc`V7iymr;1Soot!dK>^m9W6zT9D>n1B#$YF(n{)t;UFPFY#v)Tr`Y+0(Hf# z?&NaIh#v+-x}#Ffe8(62DUb;m>_t7(@)gx!tIwP-cpC1f7t*=53zFx;$Ag~^{c$Ka zeq-hGjkO)!g`Hi6_Q3C^-kVx{HQSnNdFH9v=&S#VGS=0vGnBVc?RZJ@T>3cm>Fqz> z&fR!z<+;}?JGSLo4p;5ix8mtr9{lH_e;E2Ou+o3N*stYC72?Gkc~5sX_50h8Za==7 z-+x-89W0{_z45e*W zeM6RFsnxDiZCk=Ms6VnMM}yX@21nhW5K9+s)k6cZ+#+oaj=B+AlcOHXs=-k|!ZkUX zw>U=KXsOB3vPF)%Pg9elb&DMB+G5w_sGCkT2jpFZC9? zv^w;!G7@-ifp-BU&?S0Y9cXu}(DEGSgZiu-RIHQvy!<`Aa6WkVRix_Fo92VpUPan8 zp9&l{fh%686$xf(Qb90Fd<{qv(DrHdQu!8+_h6~5)v8l(8gI3TzI)!7hEoQ;2Q*Hl zQgbX}ROrHx1TlQi81IKVkGp{H#0*!F--Q{jOPLL#8x_PxAg08Zs3v&a3&9p1AIc9x zrWgg0S6moQ)g#r!P*BVh5VjRnkBK-)=r*ZdQprIoIYT7$LYyH8eFM+roE8j&5Un=? z77s+&i^dGRNkqBxC{Cq8BZug?`sNwlz$@E|J<5Un7 zEj4v#+=b?Lxp|+|yf5SVbL&oYdk#tdL(3=f{xhI7(39-#d~3 z7Q)(B#yU56G2b~VcRnX|K9_HOUY%25F9yJdq~K8Q`1AST3v%!!Dfm)8@G_$PYhA%? zm)w0^>OP)3@nXLFs@#2D>b{=u8b@?T8S`r6-qE^vO>WySwe8OxIFoNXE4N*g+AijI zjAooq9G?3}7mns2K<8N=ko(U|{pVNwV47~bKesTK^K=o{aJS^$y(Iqa++WYFdJh(S zt+KCI^7VeeEnkxdN2I}#kL&ZkXEVe1M$6c_m%HRc=cGgDR{ZBG*bz^&>-WJ&!By`7 zVRu0C9r&ll$0qs6sB~oX;{o~dE7IjxayLSG-)kD*p@pHGqqE@j%FcGl*}f?L?)H1P zSDihD?alJ`J<|3)Sz|UP?LGE*_+Q8VWh}q_a>n#4$9IiSROG0(H;Nxo{9xRBs;JyZ|LH{i#K~H6i>3L7}lMk&`(q0n2I5s!tKr5~+RP%Jl+;_<)+w$_jP%mdkmZ1mC2bqcDv6VGp8 zxiQdYewJQ8Nt=%>n*W=Ef(U{`9FZXga*d5AN_GMDF zxG{X6gzF257#pxhDwD$9rx+E9SmCweOf;d?UFPO)CgJO!(FA-1#;0aP#i`vv3HT07 zVT9LHBAc8|;FnLF%JjG}d<;u^!xg)?>sogu@Jl_!NvUSO=eM&B4?sdE7|+ zJY-7UB$tG*;P^Rw2`uzMMh=ux4^o2*79=wPUz({1JVg-xL~-_^Ja}~X6psoh*&5`; zkz-hgkHPRckvowVk6%AMqSQ@}A9Q@n99K{GN&X#Z;&u`;kbw+n`fCeCxBiK8enADk zpbmXOwR}PCTDQ=2?*lkoP%Nnh9yMq^9^AlUow3j(^di8p%$n8m@h-Le@cagWKdpb! nKp%NbX)goU% From baf19dc4c82bf3af6c91643cb16c467ea0362d5a Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 13 Apr 2026 16:44:53 +0000 Subject: [PATCH 4/5] Regenerate README with tutorial metadata Co-authored-by: Mateusz Sadowski --- README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 0d64298..f665957 100644 --- a/README.md +++ b/README.md @@ -89,13 +89,13 @@ Below is a list of all tutorials available in this repository: - 📝 Tutorial for using ROS Client Library for Rust - 🔗 [Related Blog Post](https://foxglove.dev/blog/first-steps-using-rust-with-ros2) -## ROS Tutorials -### [ROS 2 Jazzy TurtleBot3 Diff-Drive Tuning with Pixi](ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md) -- 📝 Headless Gazebo simulation, MCAP recording, and live diff-drive tuning with Foxglove - ## Jupyter Notebooks ### [Analyze Your Robotics Data with Jupyter Notebooks](jupyter_notebooks/data_platform/README.md) - 📝 Load and analyze data in Jupyter Notebooks using Foxglove Data Management - 🔗 [Related Blog Post](https://foxglove.dev/blog/analyze-your-robotics-data-with-jupyter-notebooks) +## Ros +### [ROS 2 Jazzy TurtleBot3 Diff-Drive Tuning with Pixi](ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md) +- 📝 Headless Gazebo simulation, MCAP recording, and live diff-drive tuning with Foxglove + Join the Foxglove [Discord](https://discord.gg/UEuytgVkks) and follow [our blog](https://foxglove.dev/blog) for more ideas on how to integrate Foxglove into your robotics development workflows. \ No newline at end of file From 5cbfb03330cb0b67aca3da63eec4a86cae741013 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Mon, 13 Apr 2026 17:26:37 +0000 Subject: [PATCH 5/5] Refocus tutorial on diff-drive wheel modifier calibration Co-authored-by: Mateusz Sadowski --- README.md | 2 +- .../README.md | 48 +++-- .../config/diff_drive_calibrator_replay.yaml | 9 + .../config/diff_drive_calibrator_sim.yaml | 9 + .../config/diff_drive_tuner_replay.yaml | 15 -- .../config/diff_drive_tuner_sim.yaml | 15 -- .../pixi.toml | 4 +- .../scripts/diff_drive_calibrator.py | 144 ++++++++++++++ .../scripts/diff_drive_tuner.py | 187 ------------------ 9 files changed, 192 insertions(+), 241 deletions(-) create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_calibrator_replay.yaml create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_calibrator_sim.yaml delete mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_replay.yaml delete mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_sim.yaml create mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_calibrator.py delete mode 100644 ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_tuner.py diff --git a/README.md b/README.md index f665957..b7de97d 100644 --- a/README.md +++ b/README.md @@ -96,6 +96,6 @@ Below is a list of all tutorials available in this repository: ## Ros ### [ROS 2 Jazzy TurtleBot3 Diff-Drive Tuning with Pixi](ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md) -- 📝 Headless Gazebo simulation, MCAP recording, and live diff-drive tuning with Foxglove +- 📝 Headless Gazebo simulation, MCAP recording, and live wheel radius/separation modifier tuning with Foxglove Join the Foxglove [Discord](https://discord.gg/UEuytgVkks) and follow [our blog](https://foxglove.dev/blog) for more ideas on how to integrate Foxglove into your robotics development workflows. \ No newline at end of file diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md index 5deb657..b9e04a6 100644 --- a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md @@ -1,6 +1,6 @@ --- title: "ROS 2 Jazzy TurtleBot3 Diff-Drive Tuning with Pixi" -short_description: "Headless Gazebo simulation, MCAP recording, and live diff-drive tuning with Foxglove" +short_description: "Headless Gazebo simulation, MCAP recording, and live wheel radius/separation modifier tuning with Foxglove" --- # ROS 2 Jazzy + Pixi: TurtleBot3 diff-drive tuning workflow @@ -10,15 +10,15 @@ This tutorial creates an end-to-end, **pixi-only** workflow for: 1. Launching a **headless Gazebo** simulation with TurtleBot3 (Burger) on ROS 2 Jazzy 2. Recording an MCAP bag from the simulation 3. Replaying that bag with [`foxglove_mcap_player`](https://github.com/botsandus/foxglove_mcap_player) -4. Tuning diff-drive behavior live from Foxglove by updating ROS parameters +4. Tuning diff-drive wheel calibration live from Foxglove by updating ROS parameters ## What this tutorial contains - `pixi.toml`: ROS 2 Jazzy environment and repeatable tasks - `launch/headless_turtlebot3_gazebo.launch.py`: Gazebo server-only launch (no GUI) -- `scripts/diff_drive_tuner.py`: runtime diff-drive filter and tuner node -- `config/diff_drive_tuner_sim.yaml`: tuning params for simulation (`/cmd_vel_raw` -> `/cmd_vel`) -- `config/diff_drive_tuner_replay.yaml`: tuning params for replay (`/cmd_vel` -> `/cmd_vel_tuned`) +- `scripts/diff_drive_calibrator.py`: runtime diff-drive wheel modifier calibrator node +- `config/diff_drive_calibrator_sim.yaml`: calibration params for simulation (`/cmd_vel_raw` -> `/cmd_vel`) +- `config/diff_drive_calibrator_replay.yaml`: calibration params for replay (`/cmd_vel` -> `/cmd_vel_tuned`) - `scripts/setup_foxglove_mcap_player.sh`: clones/builds `foxglove_mcap_player` - `scripts/run_mcap_player.sh`: runs player against a chosen MCAP file @@ -44,16 +44,25 @@ pixi run sim This launches Gazebo in server mode (`-s`) with no GUI client. -## 3) Start the diff-drive tuner for simulation +## 3) Start the diff-drive wheel calibration node Terminal B: ```bash -pixi run tune-sim +pixi run calibrate-sim ``` -The tuner subscribes to `/cmd_vel_raw` and publishes tuned velocity commands to `/cmd_vel`. -Tuneable parameters are namespaced under `/diff_drive_tuner`. +The calibrator subscribes to `/cmd_vel_raw` and publishes compensated commands to `/cmd_vel`. +Tuneable parameters are namespaced under `/diff_drive_calibrator`. + +The main live tuning parameters are: + +- `wheel_radius_multiplier` +- `wheel_separation_multiplier` + +These map to the diff-drive calibration concept directly: +- if wheel radius is effectively too small/large, tune `wheel_radius_multiplier` +- if turning radius is off, tune `wheel_separation_multiplier` ## 4) Drive the robot and record MCAP @@ -117,31 +126,27 @@ You can set `PLAYER_HOST` and `PLAYER_PORT` if needed: PLAYER_HOST=0.0.0.0 PLAYER_PORT=8766 MCAP_FILE=... pixi run mcap-play ``` -## 7) Tune diff-drive during replay in Foxglove +## 7) Tune diff-drive wheel modifiers during replay in Foxglove Terminal F: ```bash -pixi run tune-replay +pixi run calibrate-replay ``` -Replay mode subscribes to `/cmd_vel` and publishes tuned commands to `/cmd_vel_tuned`. -This keeps original and tuned command streams separate for comparison. +Replay mode subscribes to `/cmd_vel` and publishes compensated commands to `/cmd_vel_tuned`. +This keeps original and calibrated command streams separate for comparison. In Foxglove, add panels for: - `/cmd_vel` (original) -- `/cmd_vel_tuned` (after tuning) +- `/cmd_vel_tuned` (after calibration) - `/odom` and `/tf` -Adjust ROS parameters on `/diff_drive_tuner` live: +Adjust ROS parameters on `/diff_drive_calibrator` live: -- `linear_scale` -- `angular_scale` -- `max_linear_speed` -- `max_angular_speed` -- `linear_accel_limit` -- `angular_accel_limit` +- `wheel_radius_multiplier` +- `wheel_separation_multiplier` - `cmd_timeout` ## Notes @@ -149,3 +154,4 @@ Adjust ROS parameters on `/diff_drive_tuner` live: - Default robot model is `burger` via pixi activation env (`TURTLEBOT3_MODEL`). - If your bag contains a different file name than `tb3_0.mcap`, point `MCAP_FILE` to the actual path. - `foxglove_mcap_player` is not currently packaged in RoboStack Jazzy, so this tutorial builds it from source in `external/`. +- TurtleBot3 Jazzy in Gazebo uses `gz::sim::systems::DiffDrive` with static SDF wheel params. The calibrator node keeps tuning live by exposing ROS parameters and pre-compensating `cmd_vel` in real time. diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_calibrator_replay.yaml b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_calibrator_replay.yaml new file mode 100644 index 0000000..92dc65f --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_calibrator_replay.yaml @@ -0,0 +1,9 @@ +diff_drive_calibrator: + ros__parameters: + use_sim_time: true + input_topic: "/cmd_vel" + output_topic: "/cmd_vel_tuned" + wheel_radius_multiplier: 1.0 + wheel_separation_multiplier: 1.0 + cmd_timeout: 0.5 + publish_rate_hz: 30.0 diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_calibrator_sim.yaml b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_calibrator_sim.yaml new file mode 100644 index 0000000..10b560d --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_calibrator_sim.yaml @@ -0,0 +1,9 @@ +diff_drive_calibrator: + ros__parameters: + use_sim_time: true + input_topic: "/cmd_vel_raw" + output_topic: "/cmd_vel" + wheel_radius_multiplier: 1.0 + wheel_separation_multiplier: 1.0 + cmd_timeout: 0.5 + publish_rate_hz: 30.0 diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_replay.yaml b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_replay.yaml deleted file mode 100644 index 6ee4fe1..0000000 --- a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_replay.yaml +++ /dev/null @@ -1,15 +0,0 @@ -diff_drive_tuner: - ros__parameters: - use_sim_time: true - input_topic: "/cmd_vel" - output_topic: "/cmd_vel_tuned" - linear_scale: 1.0 - angular_scale: 1.0 - linear_bias: 0.0 - angular_bias: 0.0 - max_linear_speed: 0.22 - max_angular_speed: 2.84 - linear_accel_limit: 0.4 - angular_accel_limit: 1.2 - cmd_timeout: 0.5 - publish_rate_hz: 30.0 diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_sim.yaml b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_sim.yaml deleted file mode 100644 index bcc91b9..0000000 --- a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/config/diff_drive_tuner_sim.yaml +++ /dev/null @@ -1,15 +0,0 @@ -diff_drive_tuner: - ros__parameters: - use_sim_time: true - input_topic: "/cmd_vel_raw" - output_topic: "/cmd_vel" - publish_rate_hz: 30.0 - linear_scale: 1.0 - angular_scale: 1.0 - linear_bias: 0.0 - angular_bias: 0.0 - max_linear_speed: 0.22 - max_angular_speed: 2.84 - linear_accel_limit: 0.6 - angular_accel_limit: 3.5 - cmd_timeout: 0.5 diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml index 49d4d00..a33fe4d 100644 --- a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml @@ -26,8 +26,8 @@ ros-jazzy-turtlebot3-simulations = "*" [tasks] sim = "bash -lc 'ros2 launch \"$PIXI_PROJECT_ROOT/launch/headless_turtlebot3_gazebo.launch.py\"'" -tune-sim = "python ./scripts/diff_drive_tuner.py --ros-args --params-file ./config/diff_drive_tuner_sim.yaml" -tune-replay = "python ./scripts/diff_drive_tuner.py --ros-args --params-file ./config/diff_drive_tuner_replay.yaml" +calibrate-sim = "python ./scripts/diff_drive_calibrator.py --ros-args --params-file ./config/diff_drive_calibrator_sim.yaml" +calibrate-replay = "python ./scripts/diff_drive_calibrator.py --ros-args --params-file ./config/diff_drive_calibrator_replay.yaml" teleop = "ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/cmd_vel_raw" drive-circle = "ros2 topic pub --rate 10 /cmd_vel_raw geometry_msgs/msg/Twist '{linear: {x: 0.2}, angular: {z: 0.4}}'" bag-record = "bash -lc 'mkdir -p bags && ros2 bag record -s mcap -o bags/tb3_$(date +%Y%m%d_%H%M%S) /clock /cmd_vel_raw /cmd_vel /odom /tf /tf_static /joint_states /scan'" diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_calibrator.py b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_calibrator.py new file mode 100644 index 0000000..0514fa8 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_calibrator.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python3 + +from typing import Optional + +import rclpy +from geometry_msgs.msg import Twist +from rcl_interfaces.msg import SetParametersResult +from rclpy.node import Node +from rclpy.parameter import Parameter + + +class DiffDriveCalibrator(Node): + def __init__(self) -> None: + super().__init__("diff_drive_calibrator") + + self.declare_parameter("input_topic", "/cmd_vel_raw") + self.declare_parameter("output_topic", "/cmd_vel") + self.declare_parameter("wheel_radius_multiplier", 1.0) + self.declare_parameter("wheel_separation_multiplier", 1.0) + self.declare_parameter("cmd_timeout", 0.5) + self.declare_parameter("publish_rate_hz", 30.0) + + self._sub = None + self._pub = None + self._timer = None + + self._input_topic = "" + self._output_topic = "" + self._wheel_radius_multiplier = 1.0 + self._wheel_separation_multiplier = 1.0 + self._cmd_timeout = 0.5 + + self._last_msg_time: Optional[float] = None + self._last_desired_cmd = Twist() + + self._load_params_and_rewire_topics() + self.add_on_set_parameters_callback(self._on_set_parameters) + + self.get_logger().info( + "Diff-drive calibrator started. " + f"input={self._input_topic} output={self._output_topic}" + ) + + def _on_cmd_vel(self, msg: Twist) -> None: + self._last_msg_time = self._now_s() + self._last_desired_cmd = msg + + def _on_timer(self) -> None: + now = self._now_s() + + desired = Twist() + if self._last_msg_time is not None: + if self._cmd_timeout <= 0.0 or (now - self._last_msg_time) < self._cmd_timeout: + desired = self._last_desired_cmd + + corrected = Twist() + + # Command pre-compensation: + # v_actual ~= wheel_radius_multiplier * v_cmd + # w_actual ~= (wheel_radius_multiplier / wheel_separation_multiplier) * w_cmd + # so we solve for cmd that achieves desired (v, w). + corrected.linear.x = desired.linear.x / self._wheel_radius_multiplier + corrected.angular.z = ( + desired.angular.z + * self._wheel_separation_multiplier + / self._wheel_radius_multiplier + ) + + self._pub.publish(corrected) + + def _on_set_parameters(self, params: list[Parameter]) -> SetParametersResult: + for param in params: + if param.name in {"wheel_radius_multiplier", "wheel_separation_multiplier"}: + if float(param.value) <= 0.0: + return SetParametersResult( + successful=False, + reason=f"{param.name} must be > 0", + ) + if param.name == "publish_rate_hz" and float(param.value) <= 0.0: + return SetParametersResult( + successful=False, + reason="publish_rate_hz must be > 0", + ) + if param.name == "cmd_timeout" and float(param.value) < 0.0: + return SetParametersResult( + successful=False, + reason="cmd_timeout must be >= 0", + ) + + self._load_params_and_rewire_topics() + return SetParametersResult(successful=True) + + def _load_params_and_rewire_topics(self) -> None: + previous_input = self._input_topic + previous_output = self._output_topic + + self._input_topic = str(self.get_parameter("input_topic").value) + self._output_topic = str(self.get_parameter("output_topic").value) + self._wheel_radius_multiplier = float( + self.get_parameter("wheel_radius_multiplier").value + ) + self._wheel_separation_multiplier = float( + self.get_parameter("wheel_separation_multiplier").value + ) + self._cmd_timeout = float(self.get_parameter("cmd_timeout").value) + publish_rate = float(self.get_parameter("publish_rate_hz").value) + + if self._pub is None or previous_output != self._output_topic: + if self._pub is not None: + self.destroy_publisher(self._pub) + self._pub = self.create_publisher(Twist, self._output_topic, 10) + + if self._sub is None or previous_input != self._input_topic: + if self._sub is not None: + self.destroy_subscription(self._sub) + self._sub = self.create_subscription( + Twist, + self._input_topic, + self._on_cmd_vel, + 10, + ) + + if self._timer is not None: + self.destroy_timer(self._timer) + self._timer = self.create_timer(1.0 / publish_rate, self._on_timer) + + def _now_s(self) -> float: + return self.get_clock().now().nanoseconds / 1e9 + + +def main() -> None: + rclpy.init() + node = DiffDriveCalibrator() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_tuner.py b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_tuner.py deleted file mode 100644 index e6ef4d6..0000000 --- a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/scripts/diff_drive_tuner.py +++ /dev/null @@ -1,187 +0,0 @@ -#!/usr/bin/env python3 - -from typing import Optional - -import rclpy -from geometry_msgs.msg import Twist -from rcl_interfaces.msg import SetParametersResult -from rclpy.node import Node -from rclpy.parameter import Parameter - - -def clamp(value: float, minimum: float, maximum: float) -> float: - return max(minimum, min(maximum, value)) - - -def move_toward(current: float, target: float, max_delta: float) -> float: - if max_delta <= 0.0: - return target - delta = target - current - if abs(delta) <= max_delta: - return target - return current + max_delta if delta > 0.0 else current - max_delta - - -class DiffDriveTuner(Node): - def __init__(self) -> None: - super().__init__("diff_drive_tuner") - - self.declare_parameter("input_topic", "/cmd_vel_raw") - self.declare_parameter("output_topic", "/cmd_vel") - self.declare_parameter("linear_scale", 1.0) - self.declare_parameter("angular_scale", 1.0) - self.declare_parameter("linear_bias", 0.0) - self.declare_parameter("angular_bias", 0.0) - self.declare_parameter("max_linear_speed", 0.4) - self.declare_parameter("max_angular_speed", 1.8) - self.declare_parameter("linear_accel_limit", 0.8) - self.declare_parameter("angular_accel_limit", 2.5) - self.declare_parameter("cmd_timeout", 0.5) - self.declare_parameter("publish_rate_hz", 30.0) - - self._sub = None - self._pub = None - self._timer = None - - self._input_topic = "" - self._output_topic = "" - self._linear_scale = 1.0 - self._angular_scale = 1.0 - self._linear_bias = 0.0 - self._angular_bias = 0.0 - self._max_linear = 0.4 - self._max_angular = 1.8 - self._linear_accel_limit = 0.8 - self._angular_accel_limit = 2.5 - self._cmd_timeout = 0.5 - - self._last_msg_time: Optional[float] = None - self._last_publish_time: Optional[float] = None - self._last_output = Twist() - - self._load_params_and_rewire_topics() - self.add_on_set_parameters_callback(self._on_set_parameters) - - self.get_logger().info( - f"DiffDrive tuner started. input={self._input_topic} output={self._output_topic}" - ) - - def _on_cmd_vel(self, msg: Twist) -> None: - now = self._now_s() - self._last_msg_time = now - self._publish_smoothed(msg, now) - - def _on_timer(self) -> None: - now = self._now_s() - if self._cmd_timeout <= 0.0 or self._last_msg_time is None: - return - if (now - self._last_msg_time) < self._cmd_timeout: - return - zero = Twist() - self._publish_smoothed(zero, now) - - def _publish_smoothed(self, msg: Twist, now: float) -> None: - raw_linear = msg.linear.x * self._linear_scale + self._linear_bias - raw_angular = msg.angular.z * self._angular_scale + self._angular_bias - - target_linear = clamp(raw_linear, -self._max_linear, self._max_linear) - target_angular = clamp(raw_angular, -self._max_angular, self._max_angular) - - dt = 0.0 - if self._last_publish_time is not None: - dt = max(0.0, now - self._last_publish_time) - - if dt > 0.0: - target_linear = move_toward( - self._last_output.linear.x, - target_linear, - self._linear_accel_limit * dt, - ) - target_angular = move_toward( - self._last_output.angular.z, - target_angular, - self._angular_accel_limit * dt, - ) - - out = Twist() - out.linear.x = target_linear - out.angular.z = target_angular - self._pub.publish(out) - self._last_publish_time = now - self._last_output = out - - def _on_set_parameters(self, params: list[Parameter]) -> SetParametersResult: - for param in params: - if param.name in {"max_linear_speed", "max_angular_speed"} and param.value <= 0.0: - return SetParametersResult( - successful=False, - reason=f"{param.name} must be > 0", - ) - if param.name in {"linear_accel_limit", "angular_accel_limit"} and param.value < 0.0: - return SetParametersResult( - successful=False, - reason=f"{param.name} must be >= 0", - ) - if param.name == "publish_rate_hz" and param.value <= 0.0: - return SetParametersResult( - successful=False, - reason="publish_rate_hz must be > 0", - ) - if param.name == "cmd_timeout" and param.value < 0.0: - return SetParametersResult( - successful=False, - reason="cmd_timeout must be >= 0", - ) - - self._load_params_and_rewire_topics() - return SetParametersResult(successful=True) - - def _load_params_and_rewire_topics(self) -> None: - previous_input = self._input_topic - previous_output = self._output_topic - - self._input_topic = str(self.get_parameter("input_topic").value) - self._output_topic = str(self.get_parameter("output_topic").value) - self._linear_scale = float(self.get_parameter("linear_scale").value) - self._angular_scale = float(self.get_parameter("angular_scale").value) - self._linear_bias = float(self.get_parameter("linear_bias").value) - self._angular_bias = float(self.get_parameter("angular_bias").value) - self._max_linear = float(self.get_parameter("max_linear_speed").value) - self._max_angular = float(self.get_parameter("max_angular_speed").value) - self._linear_accel_limit = float(self.get_parameter("linear_accel_limit").value) - self._angular_accel_limit = float(self.get_parameter("angular_accel_limit").value) - self._cmd_timeout = float(self.get_parameter("cmd_timeout").value) - publish_rate = float(self.get_parameter("publish_rate_hz").value) - - if self._pub is None or previous_output != self._output_topic: - if self._pub is not None: - self.destroy_publisher(self._pub) - self._pub = self.create_publisher(Twist, self._output_topic, 10) - - if self._sub is None or previous_input != self._input_topic: - if self._sub is not None: - self.destroy_subscription(self._sub) - self._sub = self.create_subscription(Twist, self._input_topic, self._on_cmd_vel, 10) - - if self._timer is not None: - self.destroy_timer(self._timer) - self._timer = self.create_timer(1.0 / publish_rate, self._on_timer) - - def _now_s(self) -> float: - return self.get_clock().now().nanoseconds / 1e9 - - -def main() -> None: - rclpy.init() - node = DiffDriveTuner() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main()