Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -94,4 +94,8 @@ Below is a list of all tutorials available in this repository:
- 📝 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 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.
4 changes: 4 additions & 0 deletions ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
bags/
external/
**/__pycache__/
*.pyc
157 changes: 157 additions & 0 deletions ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,157 @@
---
title: "ROS 2 Jazzy TurtleBot3 Diff-Drive Tuning with Pixi"
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

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

## 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 wheel calibration node

Terminal B:

```bash
pixi run calibrate-sim
```

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

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_<timestamp>/` 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://<machine-ip>: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 wheel modifiers during replay in Foxglove

Terminal F:

```bash
pixi run calibrate-replay
```

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 calibration)
- `/odom` and `/tf`

Adjust ROS parameters on `/diff_drive_calibrator` live:

- `wheel_radius_multiplier`
- `wheel_separation_multiplier`
- `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/`.
- 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.
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -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,
]
)
36 changes: 36 additions & 0 deletions ros/ros2_jazzy_turtlebot3_diffdrive_pixi/pixi.toml
Original file line number Diff line number Diff line change
@@ -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\"'"
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'"
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"] }
Loading
Loading