diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 321f702..1154d24 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -22,11 +22,15 @@ jobs: python3-colcon-common-extensions \ build-essential cmake \ ros-humble-nav-msgs \ + ros-humble-sensor-msgs \ + ros-humble-visualization-msgs \ ros-humble-tf2-ros \ ros-humble-tf2-geometry-msgs \ ros-humble-launch-testing \ ros-humble-launch-testing-ament-cmake \ - ros-humble-launch-testing-ros + ros-humble-launch-testing-ros \ + python3-pip + pip3 install --no-cache-dir setuptools shell: bash - name: Build @@ -40,6 +44,6 @@ jobs: run: | source /opt/ros/humble/setup.bash source $GITHUB_WORKSPACE/install/setup.bash - colcon test --packages-select smooth_nav_core --return-code-on-test-failure + colcon test --return-code-on-test-failure colcon test-result --verbose shell: bash diff --git a/CHANGELOG.md b/CHANGELOG.md index cef4cce..d0b716c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,34 @@ All notable changes to this project will be documented in this file. +## [2.0.0] — 2025-06-XX + +### Added +- **Safety watchdog node** (`safety_watchdog_node.py`) — `/cmd_vel_raw` → `/cmd_vel` interposer with velocity clamping, acceleration limiting, command timeout, and laser proximity stop +- **Pipeline orchestrator** (`waypoint_client_node.py`) — end-to-end Smooth → Generate → Execute with retry, abort, and RViz markers +- **Curvature-based speed limiting** in trajectory generator ($v \leq \sqrt{a_{lat,max}/|\kappa|}$) +- **Adaptive look-ahead distance** in trajectory tracker ($L_d = L_{d,base} + k \cdot |v|$) +- **Goal deceleration** — smooth deceleration within configurable radius of final waypoint +- **Dynamic parameter reconfiguration** — all C++ nodes support `rqt_reconfigure` live tuning +- **Rich RViz visualization** — curvature markers, velocity profile, trajectory path, command arrows, waypoint spheres, safety status text overlay +- `/pipeline_status` topic with coordinated state machine feedback +- Signed cross-track error via cross product for PID correction + +### Changed +- Upgraded `path_smoother_node` — publishes curvature color-mapped markers, dynamic reconfig +- Upgraded `trajectory_generator_node` — curvature-limited profiles, publishes velocity profile markers +- Upgraded `trajectory_tracker_node` — adaptive look-ahead, goal deceleration, publishes velocity command arrows +- Unit tests expanded from 30 → **65 GTest cases** (all passing) +- Integration tests fixed: msg field mismatches (`smooth_path` → `smoothed_path`, `points` → `waypoints`, `total_time` → `duration`) +- `demo.launch.py` launches full pipeline including safety watchdog +- Updated documentation: algorithms, design decisions, README + +### Fixed +- Trajectory generator trapezoidal profile handles zero-length segments +- Pure Pursuit handles edge case when closest point is the last waypoint + +--- + ## [1.0.0] — 2025-01-XX ### Added diff --git a/README.md b/README.md index c93526a..fdc763a 100644 --- a/README.md +++ b/README.md @@ -1,15 +1,15 @@ # smooth_nav — TurtleBot3 Path Smoothing & Trajectory Control -Professional 8-package ROS 2 Humble architecture for **2D path smoothing**, **trajectory generation**, and **trajectory tracking** on TurtleBot3 Burger in Gazebo Classic simulation. +Professional 8-package ROS 2 Humble architecture for **2D path smoothing**, **trajectory generation**, and **trajectory tracking** on TurtleBot3 Burger in Gazebo Classic simulation — with **safety watchdog**, **dynamic reconfiguration**, and **rich RViz visualization**. ## Assignment Overview | Component | Points | Algorithm | |-----------|--------|-----------| | Path Smoothing | 25 | Cubic Spline (Thomas algorithm) + B-Spline Gradient Descent | -| Trajectory Generation | 25 | Trapezoidal Velocity Profile (arc-length parameterized) | -| Trajectory Tracking | 25 | Pure Pursuit + PID Heading Correction (Action Server) | -| Code Quality & Testing | 15 | 30+ GTest cases, Strategy/Factory patterns, zero-ROS core | +| Trajectory Generation | 25 | Trapezoidal Velocity Profile (curvature-limited, arc-length parameterized) | +| Trajectory Tracking | 25 | Adaptive Pure Pursuit + PID (goal deceleration, action server) | +| Code Quality & Testing | 15 | 65 GTest cases, Strategy/Factory patterns, zero-ROS core | | Documentation & Demo | 10 | Algorithms doc, design decisions, extension guides | --- @@ -42,9 +42,28 @@ Professional 8-package ROS 2 Humble architecture for **2D path smoothing**, **tr ### Pipeline Flow ``` -Waypoints → [SmoothPath Service] → SmoothedPath → [GenerateTrajectory Service] → Trajectory - ↓ - /cmd_vel ← [ExecuteTrajectory Action] ← /odom + Trajectory + ┌─────────────────────────────────┐ + │ waypoint_client_node │ ← Pipeline orchestrator (Python) + │ reads YAML → calls services │ + └──────┬──────────┬───────────┬───┘ + │ │ │ + SmoothPath GenerateTraj ExecuteTraj + Service Service Action + │ │ │ + ▼ ▼ ▼ + path_smoother traj_gen traj_tracker + _node _node _node + │ │ │ + └────┬─────┘ │ + ▼ ▼ + smooth_nav_core /cmd_vel_raw + (Pure C++17) │ + ▼ + safety_watchdog + _node (Python) + │ + ▼ + /cmd_vel → Robot ``` --- @@ -73,7 +92,15 @@ source install/setup.bash ### 3. Launch full demo ```bash # Start VcXsrv first (Windows) with "Disable access control" checked + +# Option A: Full sim + auto-running pipeline +ros2 launch smooth_nav_bringup demo.launch.py + +# Option B: Sim only (call services manually) ros2 launch smooth_nav_bringup smooth_nav.launch.py + +# Option C: Choose a waypoint set +ros2 launch smooth_nav_bringup demo.launch.py waypoint_set:=figure_eight ``` ### 4. Run unit tests @@ -149,23 +176,53 @@ RoboticsAssignment/ | **Factory** | `SmootherFactory::create(type)` | Instantiate strategies by name | | **Interface Segregation** | Separate abstract interfaces | Each node depends only on what it uses | | **Config Over Code** | YAML params loaded at launch | Tune without recompiling | +| **Pipeline Orchestrator** | `waypoint_client_node` | Sequences Smooth → Generate → Execute | +| **Safety Interposer** | `safety_watchdog_node` | Transparent `/cmd_vel_raw` → `/cmd_vel` filtering | +| **Dynamic Reconfiguration** | All C++ nodes | `add_on_set_parameters_callback` for live tuning | --- ## Key Parameters +All parameters are **dynamically reconfigurable** via `ros2 param set` or `rqt_reconfigure` — no restart required. + +### Path Smoother | Parameter | Default | Description | |-----------|---------|-------------| | `smoother_type` | `cubic_spline` | `cubic_spline` or `bspline` | | `num_smooth_points` | 200 | Interpolation density | +| `bspline_weight_data` | 0.1 | B-spline data fidelity weight | +| `bspline_weight_smooth` | 0.3 | B-spline smoothness weight | + +### Trajectory Generator +| Parameter | Default | Description | +|-----------|---------|-------------| | `generator_type` | `trapezoidal` | `trapezoidal` or `constant` | -| `max_velocity` | 0.22 m/s | TurtleBot3 Burger max | +| `max_velocity` | 0.18 m/s | Target cruise speed | | `max_acceleration` | 0.5 m/s² | Trapezoidal ramp rate | -| `look_ahead_distance` | 0.3 m | Pure Pursuit L_d | -| `goal_tolerance` | 0.05 m | Completion threshold | -| `kp / ki / kd` | 2.0 / 0.0 / 0.1 | Heading PID gains | +| `max_lateral_acceleration` | 0.5 m/s² | Curvature-based speed limiting | +| `time_step` | 0.05 s | Trajectory sampling period | + +### Trajectory Tracker +| Parameter | Default | Description | +|-----------|---------|-------------| +| `look_ahead_distance` | 0.3 m | Pure Pursuit base $L_d$ | +| `adaptive_look_ahead_gain` | 0.5 | $L_d = L_{d,base} + k \cdot |v|$ | +| `goal_deceleration_radius` | 0.3 m | Linear deceleration zone before goal | +| `goal_tolerance` | 0.08 m | Completion threshold | +| `pid_kp / ki / kd` | 1.0 / 0.0 / 0.1 | Cross-track PID gains | | `control_rate` | 20.0 Hz | Controller loop frequency | +### Safety Watchdog +| Parameter | Default | Description | +|-----------|---------|-------------| +| `max_linear_velocity` | 0.22 m/s | Hardware velocity limit | +| `max_angular_velocity` | 2.84 rad/s | Hardware angular limit | +| `max_linear_acceleration` | 0.5 m/s² | Jerk-free acceleration limit | +| `cmd_vel_timeout` | 0.5 s | Emergency stop if no command | +| `use_laser_safety` | false | Enable proximity obstacle stop | +| `obstacle_stop_distance` | 0.20 m | LaserScan stop threshold | + --- ## ROS 2 Interfaces @@ -186,17 +243,26 @@ RoboticsAssignment/ |-------|------|------| | `/smoothed_path` | `nav_msgs/Path` | path_smoother_node | | `/original_waypoints` | `visualization_msgs/MarkerArray` | path_smoother_node | -| `/cmd_vel` | `geometry_msgs/Twist` | trajectory_tracker_node | +| `/curvature_markers` | `visualization_msgs/MarkerArray` | path_smoother_node | +| `/trajectory_path` | `nav_msgs/Path` | trajectory_generator_node | +| `/velocity_profile` | `visualization_msgs/MarkerArray` | trajectory_generator_node | +| `/cmd_vel_raw` | `geometry_msgs/Twist` | trajectory_tracker_node | | `/actual_path` | `nav_msgs/Path` | trajectory_tracker_node | | `/tracking_error` | `std_msgs/Float64` | trajectory_tracker_node | | `/controller_diagnostics` | `smooth_nav_msgs/ControllerDiagnostics` | trajectory_tracker_node | +| `/velocity_command_markers` | `visualization_msgs/MarkerArray` | trajectory_tracker_node | +| `/cmd_vel` | `geometry_msgs/Twist` | safety_watchdog_node | +| `/safety_status` | `std_msgs/String` | safety_watchdog_node | +| `/pipeline_status` | `std_msgs/String` | waypoint_client_node | +| `/waypoint_markers` | `visualization_msgs/MarkerArray` | waypoint_client_node | +| `/trajectory_markers` | `visualization_msgs/MarkerArray` | waypoint_client_node | --- ## Testing -- **30+ unit tests** in `smooth_nav_core` (cubic spline, velocity profiles, pure pursuit, PID, geometry) -- **Integration tests** in `smooth_nav_tests` (service calls, action execution) +- **65 unit tests** in `smooth_nav_core` (cubic spline, B-spline, trapezoidal velocity, constant velocity, pure pursuit, PID, geometry utils) +- **Integration tests** in `smooth_nav_tests` (smoother service, generator service, tracker action) - **System launch test** (verifies all nodes start and advertise interfaces) ```bash diff --git a/docs/algorithms.md b/docs/algorithms.md index 704b1af..8ae6eab 100644 --- a/docs/algorithms.md +++ b/docs/algorithms.md @@ -61,7 +61,27 @@ Where $s_{accel} = s_{decel} = \frac{v_{max}^2}{2 a_{max}}$. **Implementation:** `TrapezoidalVelocityGenerator`. -### 2.2 Constant Velocity Profile +### 2.3 Curvature-Based Speed Limiting + +**Purpose:** Prevent the robot from taking sharp turns at high speed, which causes wheel slip and tracking error. + +**Constraint:** At any point with curvature $\kappa$, the lateral (centripetal) acceleration must not exceed a limit: + +$$a_{lat} = v^2 \cdot |\kappa| \leq a_{lat,max}$$ + +Solving for the curvature-limited speed: + +$$v_{curv} = \sqrt{\frac{a_{lat,max}}{|\kappa|}}$$ + +The actual velocity at each point is the **minimum** of the trapezoidal profile speed and the curvature limit: + +$$v(s) = \min\left(v_{trap}(s),\ \sqrt{\frac{a_{lat,max}}{|\kappa(s)|}}\right)$$ + +**Parameter:** `max_lateral_acceleration` (default: 0.5 m/s²). + +**Implementation:** Applied inside both `TrapezoidalVelocityGenerator` and `ConstantVelocityGenerator` via `setMaxLateralAcceleration()`. + +### 2.4 Constant Velocity Profile **Purpose:** Simple profile at fixed speed — useful for testing and comparison. @@ -77,26 +97,76 @@ Where $s_{accel} = s_{decel} = \frac{v_{max}^2}{2 a_{max}}$. **Algorithm:** 1. Find the nearest trajectory point to the robot's current position -2. Find the look-ahead point at distance $L_d$ ahead on the trajectory -3. Transform look-ahead point to robot's local frame -4. Compute curvature: $\kappa = \frac{2 y_{local}}{L_d^2}$ -5. Angular velocity: $\omega = v \cdot \kappa$ +2. Compute adaptive look-ahead distance: $L_d = L_{d,base} + k \cdot |v|$ +3. Find the look-ahead point at distance $L_d$ ahead on the trajectory +4. Transform look-ahead point to robot's local frame +5. Compute curvature: $\kappa = \frac{2 y_{local}}{L_d^2}$ +6. Angular velocity: $\omega = v \cdot \kappa$ + +### 3.2 Adaptive Look-Ahead + +**Purpose:** At low speed the robot needs tight tracking (small $L_d$); at high speed a larger $L_d$ prevents oscillation. + +$$L_d = L_{d,base} + k_{ld} \cdot |v|$$ + +**Parameters:** `look_ahead_distance` ($L_{d,base}$, default 0.3 m), `adaptive_look_ahead_gain` ($k_{ld}$, default 0.5). Set $k_{ld} = 0$ for classic fixed look-ahead. + +### 3.3 Goal Deceleration + +**Purpose:** Prevent overshoot at the final waypoint by linearly reducing speed inside a deceleration radius. + +$$v_{cmd} = v_{ref} \cdot \min\left(1,\ \frac{d_{goal}}{r_{decel}}\right)$$ -### 3.2 PID Controller (heading correction) +where $d_{goal}$ is the distance to the last trajectory point and $r_{decel}$ is the deceleration radius (default 0.3 m). + +**Implementation:** Inside `PurePursuitController::computeControl()`. + +### 3.4 PID Controller (cross-track correction) Layered on top of Pure Pursuit for fine heading error correction: $$u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de}{dt}$$ -**Implementation:** `PurePursuitController` + `PIDController` (standalone). +### 3.4 PID Controller (cross-track correction) + +Layered on top of Pure Pursuit for fine cross-track error correction: + +$$\omega_{correction} = K_p \cdot e_{cte} + K_i \int_0^t e_{cte}(\tau) d\tau + K_d \frac{de_{cte}}{dt}$$ + +The signed cross-track error is computed via the cross product of the path tangent and the robot-to-path vector, providing directionality (left vs. right of path). + +**Implementation:** `PurePursuitController` + `PIDController` (standalone, composable). + +--- + +## 4. Safety Layer + +### 4.1 Velocity & Acceleration Limiting + +The `safety_watchdog_node` sits between the controller (`/cmd_vel_raw`) and the motor driver (`/cmd_vel`). It enforces: + +$$|v_{cmd}| \leq v_{max},\quad |\omega_{cmd}| \leq \omega_{max}$$ + +$$\frac{|v_{cmd} - v_{prev}|}{\Delta t} \leq a_{max}$$ + +### 4.2 Command Timeout (Watchdog) + +If no `/cmd_vel_raw` message is received for `cmd_vel_timeout` seconds (default 0.5), the watchdog publishes a zero-velocity command — an emergency stop. + +### 4.3 Laser Proximity Stop (optional) + +When `use_laser_safety` is enabled, the watchdog subscribes to `/scan` and halts the robot if any range reading falls below `obstacle_stop_distance` (default 0.20 m) in the forward arc (±45°). --- -## 4. Design Patterns +## 5. Design Patterns | Pattern | Where | Why | |---------|-------|-----| | **Strategy** | `IPathSmoother`, `ITrajectoryGenerator`, `IController` | Swap algorithms at runtime | | **Factory** | `SmootherFactory::create()` | Instantiate strategies by name/config | +| **Pipeline Orchestrator** | `waypoint_client_node` | Sequences Smooth → Generate → Execute end-to-end | +| **Safety Interposer** | `safety_watchdog_node` | Transparent filtering between controller and actuators | | **Interface Segregation** | Separate interfaces per concern | Each node depends only on what it uses | | **Config Over Code** | YAML parameters loaded at launch | Tune without recompiling | +| **Dynamic Reconfiguration** | All C++ nodes | `add_on_set_parameters_callback` for live tuning | diff --git a/docs/design_decisions.md b/docs/design_decisions.md index 3f29940..bfc3edc 100644 --- a/docs/design_decisions.md +++ b/docs/design_decisions.md @@ -79,3 +79,60 @@ This document explains the key architectural and design decisions made in this p - No "works on my machine" issues - CI uses the same Docker image - TurtleBot3 + Gazebo dependencies are managed in the Dockerfile + +--- + +## 8. Safety Watchdog as Separate Node + +**Decision:** A dedicated `safety_watchdog_node` sits between the controller (`/cmd_vel_raw`) and the motor driver (`/cmd_vel`), rather than embedding safety logic inside the controller. + +**Rationale:** +- **Single Responsibility** — the controller focuses on trajectory tracking; the watchdog focuses on hardware protection +- **Re-usable** — the same watchdog works with any controller (Pure Pursuit, nav2, teleop) +- **Fail-safe** — if the controller node crashes, the watchdog's timeout triggers an emergency stop +- **Transparent** — services and action server don't need to know about safety limits + +--- + +## 9. Pipeline Orchestrator Node + +**Decision:** A Python `waypoint_client_node` sequences the entire Smooth → Generate → Execute pipeline, rather than requiring the user to call each service manually. + +**Rationale:** +- **End-to-end demo** — `ros2 launch smooth_nav_bringup demo.launch.py` runs everything hands-free +- **Retry with exponential backoff** — handles service startup delays gracefully +- **Abort mechanism** — publish to `/abort_mission` to stop at any point +- **Visualization** — publishes waypoint and trajectory markers for RViz + +--- + +## 10. Dynamic Parameter Reconfiguration + +**Decision:** All C++ nodes use `add_on_set_parameters_callback` for live tuning, with input validation. + +**Rationale:** +- Tune PID gains, look-ahead distance, velocity limits via `rqt_reconfigure` while the robot is running +- No restart = faster iteration in simulation and on real hardware +- Validation callbacks reject invalid values (e.g., negative velocities) before they cause crashes + +--- + +## 11. Curvature-Based Speed Limiting + +**Decision:** The trajectory generator enforces $v \leq \sqrt{a_{lat,max} / |\kappa|}$ at every path point. + +**Rationale:** +- Without this, the robot attempts sharp turns at full speed → wheel slip, tracking divergence +- `max_lateral_acceleration` is exposed as a ROS parameter so it can be tuned per environment +- This matches how industrial AGVs and autonomous cars handle curvature-limited corridors + +--- + +## 12. Adaptive Look-Ahead Distance + +**Decision:** $L_d = L_{d,base} + k \cdot |v|$ instead of a fixed look-ahead. + +**Rationale:** +- At low speed: small $L_d$ → tight tracking through curves +- At high speed: large $L_d$ → smooth, stable pursuit without oscillation +- Classic Pure Pursuit uses fixed $L_d$ and requires careful per-path tuning; adaptive scales automatically diff --git a/docs/real_robot_extension.md b/docs/real_robot_extension.md index 9852958..e326f52 100644 --- a/docs/real_robot_extension.md +++ b/docs/real_robot_extension.md @@ -41,23 +41,37 @@ ros2 launch smooth_nav_controller controller.launch.py ### 4. Tuning -Key parameters to adjust for real hardware: +Key parameters to adjust for real hardware (all live-tunable via `rqt_reconfigure`): | Parameter | Sim Value | Real Starting Value | Notes | |-----------|-----------|-------------------|-------| | `max_linear_velocity` | 0.22 | 0.15 | Start conservative | | `max_angular_velocity` | 2.84 | 1.5 | Reduce for stability | | `look_ahead_distance` | 0.3 | 0.4 | Increase for sensor lag | +| `adaptive_look_ahead_gain` | 0.5 | 0.3 | Lower for real-world lag | +| `max_lateral_acceleration` | 1.0 | 0.5 | Tighter for wheel-slip prevention | +| `goal_deceleration_radius` | 0.5 | 0.8 | Larger for real inertia | | `kp_heading` | 2.0 | 1.0 | Lower gains initially | | `goal_tolerance` | 0.05 | 0.10 | Larger for real noise | | `control_rate` | 20.0 | 10.0 | Match sensor rate | -### 5. Safety Additions +### 5. Safety Layer (Already Implemented) -For a real robot, add: -- **Emergency stop** node subscribing to `/scan` for obstacle proximity -- **Velocity governor** that clamps `/cmd_vel` based on battery and IMU data -- **Watchdog timer** that stops the robot if no control command in 500 ms +The following safety features are **already part of smooth_nav** via `safety_watchdog_node`: + +| Feature | Status | Details | +|---------|--------|---------| +| **Emergency stop** (laser proximity) | ✅ Implemented | Subscribes to `/scan`, stops when obstacle within `safety_distance` (default 0.3 m) | +| **Velocity governor** | ✅ Implemented | Clamps `/cmd_vel_raw` to `max_linear_velocity` / `max_angular_velocity` | +| **Command timeout watchdog** | ✅ Implemented | Publishes zero-velocity if no command received within `cmd_vel_timeout` (default 0.5 s) | +| **Acceleration limiter** | ✅ Implemented | Limits linear acceleration to `max_linear_acceleration` (default 0.5 m/s²) | + +For real deployment, tune the safety parameters: +```bash +ros2 param set /safety_watchdog safety_distance 0.5 # wider margin +ros2 param set /safety_watchdog max_linear_velocity 0.15 # conservative +ros2 param set /safety_watchdog cmd_vel_timeout 0.3 # faster timeout +``` ---