Skip to content

docs: comprehensive documentation update for v2.0 ROS2 integration #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
hihry merged 1 commit into from
Feb 26, 2026
Merged

docs: comprehensive documentation update for v2.0 ROS2 integration #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
8 changes: 6 additions & 2 deletions .github/workflows/ci.yml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
28 changes: 28 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
94 changes: 80 additions & 14 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 |

---
Expand Down Expand Up @@ -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
```

---
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down
86 changes: 78 additions & 8 deletions docs/algorithms.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand All @@ -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 |
57 changes: 57 additions & 0 deletions docs/design_decisions.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Loading
Loading