Skip to content

DETAILS.md

Latest commit

 

History

History
286 lines (228 loc) · 13.2 KB

DETAILS.md

File metadata and controls

286 lines (228 loc) · 13.2 KB
Raw

TurtleBot3 CV Navigation: Technical Details, Planners & Calibration

This document contains detailed system architecture diagrams, layout specifications, configuration procedures, parameters reference, and troubleshooting steps.

Table of Contents

  1. System Architecture
  2. Workspace Layout
  3. Algorithm Deep-Dive
  4. SSH Setup For Robot Bringup
  5. Workspace & Camera Calibration
  6. One-Time Background Capture
  7. Planning & Control Parameters Reference
  8. Safety Behavior
  9. Troubleshooting Guide
  10. What Not To Launch

1. System Architecture

The control loop runs entirely on the operator host workstation, issuing direct /cmd_vel instructions over the local network to namespaced TurtleBots.

graph TD
    subgraph Sensing & Input
        Cam[Overhead Camera] -->|Raw Frames| GUI[GUI / EKF Localization Node]
        Odom[Robot Odometry] -->|Yaw Deltas| GUI
        Operator[Operator Clicks] -->|Initial Pose & Goals| GUI
    end

    subgraph Controller Dispatch
        GUI -->|Robot Poses & Goals| CBS[CBS / Prioritized Planner]
    end

    subgraph Safety & Execution
        CBS -->|Planned Paths| Follower[Path Follower & ORCA Filter]
        Follower -->|Velocity Commands| SafEnv[Safety Envelope Limits]
        SafEnv -->|/tb_N/cmd_vel| TB[TurtleBot3 Swarm]
    end
    
    style SafEnv fill:#e74c3c,stroke:#c0392b,stroke-width:2px,color:#fff
Loading

2. Workspace Layout

Package Purpose
src/cv_localization Camera detection, EKF sensor fusion, Click GUI, and calibration.
src/multi_robot_swarm_planner Direct controller, CBS/prioritized offline schedule planners, path follower, and local ORCA velocity filter.
src/multi_robot_navigation_ROS2 SSH remote execution utility for namespaced robot bringup on the lab swarm.
src/turtlebot3 TurtleBot3 driver, standard descriptions, and SDK dependencies.

3. Algorithm Deep-Dive

Theoretical Foundations

The path planning and collision avoidance methods implemented in this workspace follow the concepts from these publications:

  • Conflict-Based Search (CBS): Sharon et al., "Conflict-Based Search For Optimal Multi-Agent Path Finding"
    👉 Read CBS Paper
  • Safe Interval Path Planning (SIPP): Phillips and Likhachev, "SIPP: Safe Interval Path Planning for Dynamic Environments"
    👉 Read SIPP Paper
  • Optimal Reciprocal Collision Avoidance (ORCA): van den Berg et al., "Reciprocal n-body Collision Avoidance"
    👉 Read ORCA Project Page
  • RVO2/ORCA Reference Implementation
    👉 RVO2 Page

Repository Implementation Details

  • Offline Planner: Uses a Conflict-Based Search (CBS) approach computed over temporal $A^*$ state spaces (as opposed to a full SIPP representation).
  • Local Velocity Filter: An ORCA-inspired local collision avoidance filter. It adjusts commanded velocities when robots approach each other or workspace boundaries.
  • Priority Yielding: The local filter supports priority-aware yielding. Robots with higher priority break symmetric deadlocks. Priority is dynamic and updates based on how long a robot has been waiting or how far behind its schedule it is.
  • Crossing Gate: An optional crossing gate token is available under mppi.crossing_* parameters for physical bottleneck environments, though it is disabled by default.

4. SSH Setup For Robot Bringup

The workspace relies on remote execution of bringup scripts on the three TurtleBot3 Burgers. Passwordless SSH keys must be set up beforehand.

1. Key Generation

On the operator host PC:

ssh-keygen -t ed25519 -f ~/.ssh/turtlebot_lab_ed25519 -C turtlebot-lab

2. Copy Key to Robots

ssh-copy-id -i ~/.ssh/turtlebot_lab_ed25519.pub turtlebot@192.168.1.20
ssh-copy-id -i ~/.ssh/turtlebot_lab_ed25519.pub ubuntu@192.168.1.15
ssh-copy-id -i ~/.ssh/turtlebot_lab_ed25519.pub ubuntu@192.168.1.16

3. Verify Connection

Ensure you can log in without password prompts:

ssh -i ~/.ssh/turtlebot_lab_ed25519 turtlebot@192.168.1.20 hostname
ssh -i ~/.ssh/turtlebot_lab_ed25519 ubuntu@192.168.1.15 hostname
ssh -i ~/.ssh/turtlebot_lab_ed25519 ubuntu@192.168.1.16 hostname

Note

The remote robots expect the local workspace setup file at ~/turtlebot3_ws/install/setup.bash and will source /opt/ros/humble/setup.bash on login.

5. Workspace & Camera Calibration

Whenever the overhead camera is physically adjusted, zoomed, or has its resolution changed, you must calibrate the camera-to-world homography.

Workspace Frame Dimensions

  • Width x Height: $3.048\text{ m} \times 3.048\text{ m}$ (10 ft $\times$ 10 ft)
  • Coordinate Origin $(0,0)$: Centered in the workspace
  • Axes: +x points right in the camera view; +y points up.
  • Outer Boundaries: $x, y \in [-1.524, +1.524]\text{ m}$
  • Safe Target Envelope: Outer boundary minus wall_margin_m (default: $0.20\text{ m}$, keeping robots inside $x, y \in [-1.324, +1.324]\text{ m}$).

Homography Calibration Run

  1. Edit src/cv_localization/config/config.yaml to set camera.device (a persistent /dev/v4l/by-id/... device path is highly recommended).
  2. Execute the calibration tool: 
    cd ~/turtlebot_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
python3 src/cv_localization/cv_localization/calibrate_workspace.py \
  --config src/cv_localization/config/config.yaml \
  --output src/cv_localization/config/calibration.yaml \
  --width-m 3.048 \
  --height-m 3.048
  3. In the window, click the four workspace corners in this exact sequence:
    1. Top-Left
    2. Top-Right
    3. Bottom-Right
    4. Bottom-Left
  4. Press Enter to approve the calibration. The GUI will render a rectified birds-eye view and write the homography matrix to src/cv_localization/config/calibration.yaml.

6. One-Time Background Capture

The localization pipeline uses static background subtraction to track the robot blobs. A clean background reference image must be captured when the workspace is empty under stable lighting.

Save the reference image as:

src/cv_localization/config/background.jpg

You can capture this image using the helper script below:

cd ~/turtlebot_ws
python3 - <<'PY'
from pathlib import Path
import time
import cv2
import yaml

config_path = Path("src/cv_localization/config/config.yaml")
output_path = Path("src/cv_localization/config/background.jpg")
cfg = yaml.safe_load(config_path.read_text())
cam = cfg.get("camera", {})
cap = cv2.VideoCapture(cam.get("device", 0))
if not cap.isOpened():
    raise SystemExit(f"Could not open camera {cam.get('device', 0)}")
fourcc = cam.get("fourcc")
if fourcc:
    cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*fourcc))
cap.set(cv2.CAP_PROP_FRAME_WIDTH, cam.get("width", 1280))
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, cam.get("height", 960))
cap.set(cv2.CAP_PROP_FPS, cam.get("fps", 30))
for _ in range(30):
    cap.read()
    time.sleep(0.03)
ok, frame = cap.read()
cap.release()
if not ok:
    raise SystemExit("Camera read failed")
output_path.parent.mkdir(parents=True, exist_ok=True)
cv2.imwrite(str(output_path), frame)
print(f"Saved {output_path}")
PY

Important

If background.jpg is not present, the launches will fail immediately.

7. Planning & Control Parameters Reference

Configuration settings are stored in:

src/cv_localization/config/config.yaml

Configuration Parameters Table

Parameter Default Value Description
mppi.control_mode scheduled Default controller mode (scheduled or legacy mppi).
mppi.planner_algorithm cbs Choice of planner (cbs or prioritized).
mppi.goal_radius_m 0.12 Distance threshold where a robot is considered to have reached its goal.
mppi.max_v_mps 0.2 Maximum forward linear velocity (m/s).
mppi.allow_reverse true Allows planning and tracking reverse motions.
mppi.max_reverse_v_mps 0.15 Maximum reverse linear velocity (m/s).
mppi.max_w_radps 1.0 Maximum angular velocity (rad/s).
mppi.wall_margin_m 0.20 Minimum allowed distance from boundaries (m).
mppi.min_live_spacing_m 0.35 Spacing threshold under which all motion is halted.
mppi.safety_distance_m 0.25 Spacing limit before trigger safety overrides.
mppi.max_dv_step 0.05 Linear acceleration slew limit (change per control tick).
mppi.max_dw_step 0.5 Angular acceleration slew limit (change per control tick).
mppi.planning_clearance_m 0.35 Obstacle clearance distance used during A* path planning.
mppi.offline_grid_resolution_m 0.05 Spatial resolution of the offline grid search.
mppi.offline_time_step_s 0.5 Time discretization step size for the temporal plan.
mppi.cbs_max_nodes 1000 Search node expansion limit for CBS.
mppi.path_heading_gain 1.5 Proportional gain for path tracking yaw error.
mppi.reverse_heading_threshold_rad 2.2 Angle error threshold above which reverse velocity is triggered.
mppi.orca_filter_enabled true Enables/Disables runtime ORCA-style avoidance.
mppi.orca_time_horizon_s 3.0 Lookahead time window for dynamic avoidance.
mppi.orca_priority_enabled true Enables priority-based yielding inside ORCA.
mppi.orca_priority_wait_gain 0.1 Scale factor to increase priority based on waiting duration.
mppi.orca_priority_schedule_lag_gain 0.2 Scale factor to increase priority based on path deviation/delay.
mppi.boundary_slowdown_margin_m 0.15 Distance from wall where linear velocity scaling begins.

8. Safety Behavior

Both Scheduled and legacy MPPI controllers pipe commands through a defensive hardware safety layer. A stop command (zero velocity) is dispatched instantly to all robots if any of these events occur:

  • Manual Stops: Keypress Space, Esc, q in the GUI, or /fleet_mppi/stop service call.
  • Localization Stale: No camera coordinate updates received for more than $0.5$ seconds.
  • Boundary Violation: Robot centers enter the hard workspace boundary envelope.
  • Stale Odometry: Odometry feedback from any robot stops.
  • Proximity Violation: Distance between any two robots drops below min_live_spacing_m.
  • Goal Completion: All robots arrive within the goal threshold.

9. Troubleshooting Guide

❌ GUI fails immediately with "missing background" error

  • Resolution: Capture a reference background frame using the Python script in Section 4 and save it to src/cv_localization/config/background.jpg.

❌ Service ready errors: "Cannot plan: service not ready"

  • Resolution: The core controller node (mppi_direct_controller) failed to launch or crashed. Check the console output for Python tracebacks, library conflicts, or missing dependencies.

❌ Detections are unstable or identities swap

  • Resolution:
    • Ensure lighting is consistent. If room lighting changes, recapture background.jpg.
    • Adjust detection filters in src/cv_localization/config/config.yaml:
      • Increase/decrease detection.background_diff_threshold to isolate robots.
      • Tune detection.min_blob_area and detection.max_blob_area.
    • If tracking labels swap during crossings, increase tracking.position_history_size or tune the Kalman tracking parameters.

❌ Robots do not rotate or exhibit sluggish tracking

  • Resolution:
    • Ensure mppi.max_w_radps is set sufficiently high (e.g., 1.0 or 1.5).
    • Increase mppi.path_heading_gain.
    • Validate that the initial clicked heading in the GUI matched the physical robot orientation closely.

❌ Robots deadlock in bottlenecks

  • Resolution:
    • Verify mppi.orca_priority_enabled is set to true.
    • Adjust mppi.orca_priority_wait_gain so waiting robots gain precedence quickly.
    • Adjust mppi.orca_priority_strength for more aggressive yielding.

❌ Remote ssh bringup commands fail

  • Resolution:
    • Verify network connectivity: ping -c 3 192.168.1.20.
    • Verify passwordless login: ssh -i ~/.ssh/turtlebot_lab_ed25519 ubuntu@192.168.1.15 hostname.
    • Ensure ROS_DOMAIN_ID=30 is set consistently on both the PC and the robots.

10. What Not To Launch

To avoid topic conflicts, lifecycle errors, and hardware damage, do not launch any of the following nodes or scripts:

  • multi_nav2_launch.py (legacy Nav2 configuration)
  • lab_three_robot_nav.launch.py (legacy localization/navigation)
  • amcl, map_server, slam_toolbox, or nav2_lifecycle_manager