diff --git a/README.md b/README.md index c587800..b7de97d 100644 --- a/README.md +++ b/README.md @@ -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. \ No newline at end of file diff --git a/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore new file mode 100644 index 0000000..2f30ea7 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/.gitignore @@ -0,0 +1,4 @@ +bags/ +external/ +**/__pycache__/ +*.pyc 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..b9e04a6 --- /dev/null +++ b/ros/ros2_jazzy_turtlebot3_diffdrive_pixi/README.md @@ -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_/` 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 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. 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/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..a33fe4d --- /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\"'" +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"] } 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/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}"