From 449c6d52485b894d557c3a9079cbb7c084fa4629 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 6 Dec 2025 15:27:46 +0000 Subject: [PATCH] fix(nav2): update to Jazzy plugin syntax and simplify Docker build - Fix Nav2 plugin names (/ -> ::) and add enable_stamped_cmd_vel - Shallow clone ros2_medkit from GitHub, local COPY for demo - Move gateway to /diagnostics namespace for Area demo - Add send-nav-goal.sh helper and update README --- demos/turtlebot3_integration/Dockerfile | 9 +- demos/turtlebot3_integration/README.md | 23 +++- .../config/medkit_params.yaml | 29 +++-- .../config/nav2_params.yaml | 106 +++++++--------- .../turtlebot3_integration/docker-compose.yml | 2 - .../launch/demo.launch.py | 116 ++++++++++-------- demos/turtlebot3_integration/run-demo.sh | 55 +++++++-- demos/turtlebot3_integration/send-nav-goal.sh | 61 +++++++++ 8 files changed, 264 insertions(+), 137 deletions(-) create mode 100755 demos/turtlebot3_integration/send-nav-goal.sh diff --git a/demos/turtlebot3_integration/Dockerfile b/demos/turtlebot3_integration/Dockerfile index e0cfdae..5f12de1 100644 --- a/demos/turtlebot3_integration/Dockerfile +++ b/demos/turtlebot3_integration/Dockerfile @@ -40,12 +40,13 @@ RUN apt-get update && apt-get install -y \ curl \ && rm -rf /var/lib/apt/lists/* -# Create workspace and clone ros2_medkit -# TODO: Replace with proper ROS 2 package dependency once ros2_medkit is released +# Clone ros2_medkit from GitHub WORKDIR ${COLCON_WS}/src -RUN git clone https://github.com/selfpatch/ros2_medkit.git +RUN git clone --depth 1 https://github.com/selfpatch/ros2_medkit.git && \ + mv ros2_medkit/src/ros2_medkit_gateway . && \ + rm -rf ros2_medkit -# Copy demo package +# Copy demo package from local context (this repo) COPY package.xml CMakeLists.txt ${COLCON_WS}/src/turtlebot3_medkit_demo/ COPY config/ ${COLCON_WS}/src/turtlebot3_medkit_demo/config/ COPY launch/ ${COLCON_WS}/src/turtlebot3_medkit_demo/launch/ diff --git a/demos/turtlebot3_integration/README.md b/demos/turtlebot3_integration/README.md index e41734f..671c0e0 100644 --- a/demos/turtlebot3_integration/README.md +++ b/demos/turtlebot3_integration/README.md @@ -148,7 +148,18 @@ curl -X POST http://localhost:8080/api/v1/topics/publish ... ## What You'll See -When TurtleBot3 simulation starts with Nav2, ros2_medkit will discover nodes such as: +When TurtleBot3 simulation starts with Nav2, ros2_medkit will discover nodes organized into **areas** based on their ROS 2 namespaces: + +### Areas (Namespaces) + +| Area | Namespace | Description | +|------|-----------|-------------| +| `root` | `/` | TurtleBot3, Nav2, and Gazebo nodes | +| `diagnostics` | `/diagnostics` | ros2_medkit gateway | + +### Components + +**Root** (`/`) - Main robot system: - `turtlebot3_node` - Main robot interface - `robot_state_publisher` - TF tree publisher @@ -157,9 +168,15 @@ When TurtleBot3 simulation starts with Nav2, ros2_medkit will discover nodes suc - `bt_navigator` - Behavior Tree Navigator - `controller_server` - Path following controller - `planner_server` - Global path planner -- Various sensor and lifecycle nodes +- `velocity_smoother` - Velocity command smoother +- Various lifecycle and manager nodes + +**Diagnostics** (`/diagnostics`): + +- `ros2_medkit_gateway` - REST API gateway -These appear as **components** in the ros2_medkit REST API, organized into **areas** based on their ROS 2 namespaces. +This demonstrates ros2_medkit's ability to discover ROS 2 nodes and organize them into areas. +For a more hierarchical demo with multiple areas, see the [ros2_medkit demo nodes](https://github.com/selfpatch/ros2_medkit/tree/main/src/ros2_medkit_gateway#demo-nodes) which use namespaces like `/powertrain`, `/chassis`, and `/body`. ## Architecture diff --git a/demos/turtlebot3_integration/config/medkit_params.yaml b/demos/turtlebot3_integration/config/medkit_params.yaml index bdc7c01..0b640a3 100644 --- a/demos/turtlebot3_integration/config/medkit_params.yaml +++ b/demos/turtlebot3_integration/config/medkit_params.yaml @@ -1,11 +1,20 @@ # ros2_medkit gateway configuration for TurtleBot3 demo -ros2_medkit_gateway: - ros__parameters: - server.host: "0.0.0.0" - server.port: 8080 - refresh_interval_ms: 2000 - cors.allowed_origins: ["*"] - cors.allowed_methods: ["GET", "PUT", "POST", "OPTIONS"] - cors.allowed_headers: ["Content-Type", "Accept"] - cors.allow_credentials: false - cors.max_age_seconds: 86400 +# Node runs under /diagnostics namespace, so we need to match that here +diagnostics: + ros2_medkit_gateway: + ros__parameters: + server: + # Bind to all interfaces for Docker networking + host: "0.0.0.0" + port: 8080 + + refresh_interval_ms: 2000 + + cors: + allowed_origins: ["*"] + allowed_methods: ["GET", "PUT", "POST", "OPTIONS"] + allowed_headers: ["Content-Type", "Accept"] + allow_credentials: false + max_age_seconds: 86400 + + max_parallel_topic_samples: 10 diff --git a/demos/turtlebot3_integration/config/nav2_params.yaml b/demos/turtlebot3_integration/config/nav2_params.yaml index 6ff7bf0..aa8753d 100644 --- a/demos/turtlebot3_integration/config/nav2_params.yaml +++ b/demos/turtlebot3_integration/config/nav2_params.yaml @@ -1,6 +1,22 @@ # Nav2 parameters for TurtleBot3 + ros2_medkit demo # Based on default nav2_bringup parameters for TurtleBot3 burger +# Lifecycle manager configuration - exclude docking_server which we don't use +lifecycle_manager_navigation: + ros__parameters: + use_sim_time: True + autostart: True + node_names: + - controller_server + - smoother_server + - planner_server + - behavior_server + - bt_navigator + - waypoint_follower + - velocity_smoother + - collision_monitor + # Note: docking_server and route_server removed - not configured for this demo + amcl: ros__parameters: use_sim_time: True @@ -60,65 +76,15 @@ bt_navigator: action_server_result_timeout: 900.0 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: - plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" navigate_through_poses: - plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_are_error_codes_active_condition_bt_node - - nav2_would_a_controller_recovery_help_condition_bt_node - - nav2_would_a_planner_recovery_help_condition_bt_node - - nav2_would_a_smoother_recovery_help_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - - nav2_is_battery_charging_condition_bt_node + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # Note: plugin_lib_names is no longer needed in Jazzy - plugins are auto-loaded controller_server: ros__parameters: use_sim_time: True + enable_stamped_cmd_vel: True controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 @@ -281,6 +247,7 @@ smoother_server: behavior_server: ros__parameters: + enable_stamped_cmd_vel: True local_costmap_topic: local_costmap/costmap_raw global_costmap_topic: global_costmap/costmap_raw local_footprint_topic: local_costmap/published_footprint @@ -288,15 +255,15 @@ behavior_server: cycle_frequency: 10.0 behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: - plugin: "nav2_behaviors/Spin" + plugin: "nav2_behaviors::Spin" backup: - plugin: "nav2_behaviors/BackUp" + plugin: "nav2_behaviors::BackUp" drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" + plugin: "nav2_behaviors::DriveOnHeading" wait: - plugin: "nav2_behaviors/Wait" + plugin: "nav2_behaviors::Wait" assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" + plugin: "nav2_behaviors::AssistedTeleop" local_frame: odom global_frame: map robot_base_frame: base_link @@ -322,6 +289,7 @@ waypoint_follower: velocity_smoother: ros__parameters: use_sim_time: True + enable_stamped_cmd_vel: True smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" @@ -337,6 +305,7 @@ velocity_smoother: collision_monitor: ros__parameters: use_sim_time: True + enable_stamped_cmd_vel: True base_frame_id: "base_link" odom_frame_id: "odom" cmd_vel_in_topic: "cmd_vel_smoothed" @@ -363,3 +332,22 @@ collision_monitor: min_height: 0.15 max_height: 2.0 enabled: True + +# Docking server - minimal config to satisfy lifecycle manager +# We don't actually use docking in this demo +docking_server: + ros__parameters: + use_sim_time: True + enable_stamped_cmd_vel: True + dock_plugins: ["simple_charging_dock"] + simple_charging_dock: + plugin: "opennav_docking::SimpleChargingDock" + use_external_detection_pose: false + docking_threshold: 0.02 + staging_x_offset: -0.5 + staging_yaw_offset: 0.0 + +# Route server - minimal config +route_server: + ros__parameters: + use_sim_time: True diff --git a/demos/turtlebot3_integration/docker-compose.yml b/demos/turtlebot3_integration/docker-compose.yml index cebc44a..92aae76 100644 --- a/demos/turtlebot3_integration/docker-compose.yml +++ b/demos/turtlebot3_integration/docker-compose.yml @@ -58,8 +58,6 @@ services: ros2 launch turtlebot3_medkit_demo demo.launch.py" sovd-web-ui: - # TODO: Replace with Docker Hub image once sovd_web_ui is published - # For now, we clone and build from GitHub build: context: https://github.com/selfpatch/sovd_web_ui.git dockerfile: Dockerfile diff --git a/demos/turtlebot3_integration/launch/demo.launch.py b/demos/turtlebot3_integration/launch/demo.launch.py index 01c80a9..12ff013 100644 --- a/demos/turtlebot3_integration/launch/demo.launch.py +++ b/demos/turtlebot3_integration/launch/demo.launch.py @@ -1,10 +1,20 @@ -"""Launch TurtleBot3 simulation with Nav2 and ros2_medkit gateway for discovery demo.""" +"""Launch TurtleBot3 simulation with Nav2 and ros2_medkit gateway for discovery demo. + +This launch file demonstrates ros2_medkit's hierarchical discovery by: + - Running TurtleBot3 + Nav2 (in root namespace) + - Adding ros2_medkit gateway under /diagnostics namespace + - Showing how nodes are organized into Areas based on namespaces +""" import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + SetEnvironmentVariable, +) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -12,59 +22,63 @@ def generate_launch_description(): # Get package directories - turtlebot3_gazebo_dir = get_package_share_directory('turtlebot3_gazebo') - nav2_bringup_dir = get_package_share_directory('nav2_bringup') - demo_pkg_dir = get_package_share_directory('turtlebot3_medkit_demo') + turtlebot3_gazebo_dir = get_package_share_directory("turtlebot3_gazebo") + nav2_bringup_dir = get_package_share_directory("nav2_bringup") + demo_pkg_dir = get_package_share_directory("turtlebot3_medkit_demo") # Path to config files from installed package - medkit_params_file = os.path.join(demo_pkg_dir, 'config', 'medkit_params.yaml') - nav2_params_file = os.path.join(demo_pkg_dir, 'config', 'nav2_params.yaml') - map_file = os.path.join(demo_pkg_dir, 'config', 'turtlebot3_world.yaml') + medkit_params_file = os.path.join(demo_pkg_dir, "config", "medkit_params.yaml") + nav2_params_file = os.path.join(demo_pkg_dir, "config", "nav2_params.yaml") + map_file = os.path.join(demo_pkg_dir, "config", "turtlebot3_world.yaml") # Launch configuration variables - use_sim_time = LaunchConfiguration('use_sim_time', default='True') - - return LaunchDescription([ - # Declare launch arguments - DeclareLaunchArgument( - 'use_sim_time', - default_value='True', - description='Use simulation (Gazebo) clock if true' - ), + use_sim_time = LaunchConfiguration("use_sim_time", default="True") - # Set TurtleBot3 model (can be overridden by environment variable) - SetEnvironmentVariable( - name='TURTLEBOT3_MODEL', - value=os.environ.get('TURTLEBOT3_MODEL', 'burger') - ), - - # Launch TurtleBot3 Gazebo simulation (turtlebot3_world) - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(turtlebot3_gazebo_dir, 'launch', 'turtlebot3_world.launch.py') + return LaunchDescription( + [ + # Declare launch arguments + DeclareLaunchArgument( + "use_sim_time", + default_value="True", + description="Use simulation (Gazebo) clock if true", ), - launch_arguments={'use_sim_time': use_sim_time}.items() - ), - - # Launch Nav2 navigation stack - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py') + # Set TurtleBot3 model (can be overridden by environment variable) + SetEnvironmentVariable( + name="TURTLEBOT3_MODEL", + value=os.environ.get("TURTLEBOT3_MODEL", "burger"), ), - launch_arguments={ - 'map': map_file, - 'params_file': nav2_params_file, - 'use_sim_time': use_sim_time, - 'autostart': 'True', - }.items() - ), - - # Launch ros2_medkit gateway - Node( - package='ros2_medkit_gateway', - executable='gateway_node', - name='ros2_medkit_gateway', - output='screen', - parameters=[medkit_params_file, {'use_sim_time': use_sim_time}], - ), - ]) + # Launch TurtleBot3 Gazebo simulation (turtlebot3_world) + # Runs in root namespace to publish standard topics (/scan, /odom, /cmd_vel) + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + turtlebot3_gazebo_dir, "launch", "turtlebot3_world.launch.py" + ) + ), + launch_arguments={"use_sim_time": use_sim_time}.items(), + ), + # Launch Nav2 navigation stack + # Runs in root namespace to subscribe to robot topics + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, "launch", "bringup_launch.py") + ), + launch_arguments={ + "map": map_file, + "params_file": nav2_params_file, + "use_sim_time": use_sim_time, + "autostart": "True", + }.items(), + ), + # Launch ros2_medkit gateway under /diagnostics namespace + # This demonstrates namespace-based Area organization in discovery + Node( + package="ros2_medkit_gateway", + executable="gateway_node", + name="ros2_medkit_gateway", + namespace="diagnostics", + output="screen", + parameters=[medkit_params_file, {"use_sim_time": use_sim_time}], + ), + ] + ) diff --git a/demos/turtlebot3_integration/run-demo.sh b/demos/turtlebot3_integration/run-demo.sh index a9e188f..64d04bc 100755 --- a/demos/turtlebot3_integration/run-demo.sh +++ b/demos/turtlebot3_integration/run-demo.sh @@ -37,13 +37,50 @@ cleanup() { } trap cleanup EXIT -# Select compose profile -if [[ "${1:-}" == "--nvidia" ]]; then - echo "Using NVIDIA GPU acceleration" - COMPOSE_ARGS="--profile nvidia" -else +# Parse arguments +COMPOSE_ARGS="" +BUILD_ARGS="" + +usage() { + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " --nvidia Use NVIDIA GPU acceleration" + echo " --no-cache Build Docker images without cache" + echo " -h, --help Show this help message" + echo "" + echo "Examples:" + echo " $0 # CPU-only mode" + echo " $0 --nvidia # With GPU acceleration" + echo " $0 --no-cache # Rebuild without cache" + echo " $0 --nvidia --no-cache # Both options" +} + +while [[ $# -gt 0 ]]; do + case "$1" in + --nvidia) + echo "Using NVIDIA GPU acceleration" + COMPOSE_ARGS="--profile nvidia" + ;; + --no-cache) + echo "Building without cache" + BUILD_ARGS="--no-cache" + ;; + -h|--help) + usage + exit 0 + ;; + *) + echo "Unknown option: $1" + usage + exit 1 + ;; + esac + shift +done + +if [[ -z "$COMPOSE_ARGS" ]]; then echo "Using CPU-only mode (use --nvidia flag for GPU acceleration)" - COMPOSE_ARGS="" fi # Build and run @@ -60,8 +97,10 @@ echo "" if docker compose version &> /dev/null; then # shellcheck disable=SC2086 - docker compose ${COMPOSE_ARGS} up --build + docker compose ${COMPOSE_ARGS} build ${BUILD_ARGS} && \ + docker compose ${COMPOSE_ARGS} up else # shellcheck disable=SC2086 - docker-compose ${COMPOSE_ARGS} up --build + docker-compose ${COMPOSE_ARGS} build ${BUILD_ARGS} && \ + docker-compose ${COMPOSE_ARGS} up fi diff --git a/demos/turtlebot3_integration/send-nav-goal.sh b/demos/turtlebot3_integration/send-nav-goal.sh new file mode 100755 index 0000000..cb0dcfa --- /dev/null +++ b/demos/turtlebot3_integration/send-nav-goal.sh @@ -0,0 +1,61 @@ +#!/bin/bash +# Send a navigation goal to the TurtleBot3 robot +# Usage: ./send-nav-goal.sh [x] [y] [yaw] +# x - target x position (default: 2.0) +# y - target y position (default: 0.5) +# yaw - target orientation in radians (default: 0.0) + +set -e + +CONTAINER_NAME="turtlebot3_medkit_demo" + +# Check for required dependencies +if ! command -v bc &> /dev/null; then + echo "❌ Error: 'bc' command not found. Please install bc (e.g., 'apt-get install bc')" + exit 1 +fi + +# Default goal position +X=${1:-2.0} +Y=${2:-0.5} +YAW=${3:-0.0} + +# Validate that inputs are numeric (prevents command injection) +validate_numeric() { + local value="$1" + local name="$2" + if ! [[ "$value" =~ ^-?[0-9]*\.?[0-9]+$ ]]; then + echo "❌ Error: '$name' must be a numeric value, got: '$value'" + exit 1 + fi +} + +validate_numeric "$X" "x" +validate_numeric "$Y" "y" +validate_numeric "$YAW" "yaw" + +# Calculate quaternion from yaw (rotation around z-axis) +# Full quaternion: x=0, y=0, z=sin(yaw/2), w=cos(yaw/2) +W=$(echo "c($YAW/2)" | bc -l) +Z=$(echo "s($YAW/2)" | bc -l) + +echo "🤖 Sending navigation goal to TurtleBot3" +echo " Target: x=$X, y=$Y, yaw=$YAW rad" +echo " Quaternion: x=0, y=0, z=$Z, w=$W" +echo "" + +# Check if container is running +if ! docker ps --format '{{.Names}}' | grep -q "^${CONTAINER_NAME}$"; then + echo "❌ Container '$CONTAINER_NAME' is not running!" + echo " Start with: ./run-demo.sh" + exit 1 +fi + +# Send the navigation goal +# Using validated numeric values - safe to interpolate after validation +docker exec -it "$CONTAINER_NAME" bash -c " +source /opt/ros/jazzy/setup.bash && \ +source /root/demo_ws/install/setup.bash && \ +ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose \ + \"{pose: {header: {frame_id: 'map'}, pose: {position: {x: $X, y: $Y, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: $Z, w: $W}}}}\" +"