This project implements a comprehensive autonomous navigation system for the TurtleBot4 platform in ROS2 Humble, extending from a basic path smoothing assignment to a full-featured navigation suite. The system combines advanced path planning algorithms including cubic spline interpolation, A* obstacle avoidance, and nonlinear trajectory tracking to enable autonomous robot navigation in both static and dynamic environments.
The project demonstrates the complete pipeline from waypoint specification (either hardcoded or interactively selected in RViz) through path smoothing, obstacle-aware trajectory generation, and precise robot control using Pure Pursuit algorithms. Built for TurtleBot4 simulation in Gazebo, the system provides a robust foundation for autonomous mobile robot applications.
- Introduction
- Demo Overview
- Features
- Setup & Installation
- Running the System
- File Structure
- System Architecture
- Algorithms & Design Choices
- Extending to a Real Robot
- Future Improvements
Fig: TurtleBot4 Robot Platform
Fig: Maze World Environment for Navigation Testing
📺 Complete System Demonstration Video
This demonstration showcases the core path smoothing pipeline using predefined waypoints:
Waypoints: [(-2.75, -0.82), (-3.75, -0.1), (-3.80, 0.88), (-1.81, 0.88), (-1.71, -0.04)]
The system generates smooth cubic spline trajectories from these discrete points, creating 200 interpolated waypoints for continuous robot motion. The smoothing algorithm uses SciPy's parametric spline functions (splprep/splev) with automatic fallback to distance-based cubic splines for robustness.
Output: High-resolution path visualization showing original waypoints vs. smoothed trajectory.
Untitled.video.-.Made.with.Clipchamp.7.mp4
Hardcoded Waypoints Demo
|
Fig: Hardcoded Waypoint Path Visualization |
Fig: Hardcoded Waypoint Performance Graph |
This demo enables real-time navigation goal specification through RViz interface:
Workflow: User clicks points in RViz → goals queued in FIFO order → automatic path smoothing and execution
The WaypointManager node subscribes to /clicked_point messages, building a navigation queue that's processed sequentially. Each goal triggers path smoothing from the robot's current position, enabling intuitive exploration and navigation.
Output: Dynamic trajectory generation and real-time robot movement following clicked waypoints.
Untitled.video.-.Made.with.Clipchamp.8.mp4
Interactive RViz Waypoints Demo
|
Fig: Interactive RViz Waypoint Path |
Fig: Interactive Navigation Performance Graph |
Theis demo integrates A* pathfinding for collision-free navigation:
Pipeline: A* obstacle-free path generation → spline smoothing refinement → trajectory tracking execution
The system uses the occupancy grid from /map to compute obstacle-free paths using A* search with safety inflation. The resulting grid-based path is then refined using the same smoothing pipeline for optimal robot motion.
Output: Collision-free navigation with visualization of A* paths vs. smoothed trajectories.
Untitled.video.-.Made.with.Clipchamp.9.mp4
Obstacle Avoidance Demo
|
Fig: Obstacle Avoidance Navigation Path |
Fig: Obstacle Avoidance Performance Graph |
visualization_output/
├── path_visualization_YYYYMMDD_HHMMSS.png # Combined path plots
└── tracking_error_YYYYMMDD_HHMMSS.png # Error analysis graphs
- Multi-method Path Smoothing: SciPy parametric splines with distance-based fallback for robust interpolation
- Interactive Waypoint Selection: RViz
/clicked_pointintegration for real-time navigation goal specification - Time-Parameterized Trajectory Generation: Constant velocity profiles with proper orientation computation
- Nonlinear Trajectory Tracking: Pure Pursuit controller with configurable lookahead distance
- A Grid-Based Obstacle Avoidance*: Complete occupancy grid pathfinding with safety inflation
- TF2 Transform Integration: Robust frame handling between
mapandbase_linkcoordinate systems
- Comprehensive Visualization: Real-time path plotting, trajectory comparison, and tracking error analysis
- State Machine Architecture: Robust goal queue management and execution monitoring
- Obstacle Proximity Costing: Exponential penalty functions for safer path planning
- Dynamic Goal Management: FIFO queue system for multiple waypoint navigation
- Error Recovery: Graceful failure handling and automatic replanning capabilities
- ROS2 Humble (Ubuntu 22.04 recommended)
- Gazebo Ignition for simulation
- Git for cloning the repository
- Clone the repository into your ROS2 workspace:
git clone https://github.com/AshishRamesh/10x_Assignment
cd ~/10x_Assignment- Run the automated setup script:
./dev/setup.shThe script will automatically:
- Install Python dependencies (NumPy, SciPy, Matplotlib)
- Install TurtleBot4 simulation packages
- Install additional ROS2 navigation packages
- Build the workspace with colcon
- Provide next steps for verification
If you prefer manual installation:
- Clone the repository:
git clone https://github.com/AshishRamesh/10x_Assignment
cd ~/10x_Assignment- Install system dependencies:
sudo apt update
sudo apt install python3-numpy python3-scipy python3-matplotlib
sudo apt install ignition-gazebo6 ros-humble-ros-ign-gazebo
sudo apt install ros-humble-turtlebot4-simulator ros-humble-irobot-create-nodes- Build the workspace:
cd ~/ros2_ws
colcon build --symlink-install- Source the workspace:
source install/setup.bashros2 pkg list | grep bot_ctrl
ros2 interface list | grep nav_msgsEnsure the following commands are run at the root of the workspace: Example: If your workspace is '10x_Assignment', run the commands from there after sourcing the workspace.
# Terminal 1: Start TurtleBot4 simulation with mapping
ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py rviz:=true world:=maze localization:=true map:="src/bot_ctrl/maps/maze_map.yaml"Choose one of the following launch configurations:
# Terminal 2: Full system with A* planning
ros2 launch bot_ctrl obstacle_avoidance_launch.py# Terminal 2: Click-to-navigate system
ros2 launch bot_ctrl clicked_waypoints_launch.py# Terminal 2: Static waypoint following
ros2 launch bot_ctrl hardcoded_waypoints_launch.py- Select "Publish Point" tool
- Click desired locations on the map
- Robot automatically navigates to clicked points in sequence
10x_Assignment/
├── src/bot_ctrl/ # ROS2 package source
│ ├── bot_ctrl/
│ │ ├── obstacle_avoid.py # A* pathfinding implementation
│ │ ├── smoother_obstacle.py # A* path smoothing
│ │ ├── smoother_clicked.py # Click-based waypoint manager
│ │ ├── smoother.py # Hardcoded waypoint smoother
│ │ ├── planner.py # Trajectory generator
│ │ ├── controller.py # Pure Pursuit tracker
│ │ └── visualizer.py # Path visualization tools
│ ├── launch/
│ │ ├── obstacle_avoidance_launch.py # Complete obstacle avoidance system
│ │ ├── clicked_waypoints_launch.py # Interactive RViz navigation
│ │ ├── hardcoded_waypoints_launch.py # Static waypoint navigation
│ │ └── visualization_launch.py # Visualization-only mode
│ └── maps/
│ ├── maze_map.pgm # Maze world occupancy grid data
│ └── maze_map.yaml # Map metadata and configuration
├── dev/ # Development tools
│ └── setup.sh # Automated installation script
├── assets/ # Documentation images
├── visualization_output/ # Generated analysis plots
├── build/ # Colcon build artifacts
├── install/ # Installed packages
└── log/ # Build logs
| Node | Purpose | Topics | Key Features |
|---|---|---|---|
obstacle_avoid |
A* pathfinding | /clicked_point → /a_star_path |
Grid-based planning, safety inflation |
smoother_obstacle |
Path refinement | /a_star_path → /smooth_path |
Spline smoothing of A* output |
waypoint_manager |
Interactive navigation | /clicked_point → /smooth_path |
Click-to-navigate, goal queueing |
planner |
Trajectory generation | /smooth_path → /trajectory |
Time parameterization, orientation |
controller |
Motion control | /trajectory + /odom → /cmd_vel |
Pure Pursuit, lookahead control |
visualizer |
System monitoring | Multiple → PNG files | Error analysis, performance metrics |
┌─────────────────┐ ┌──────────────────┐ ┌─────────────────┐
│ RViz User │ │ Hardcoded │ │ Occupancy │
│ /clicked_point │────│ Waypoints │ │ Grid │
└─────────────────┘ └──────────────────┘ │ /map │
│ │ └─────────────────┘
│ │ │
▼ ▼ ▼
┌─────────────────┐ ┌──────────────────┐ ┌─────────────────┐
│ waypoint_manager│ │ smoother │ │ obstacle_avoid │
│ (clicked) │ │ (hardcoded) │ │ (A* planner) │
└─────────────────┘ └──────────────────┘ └─────────────────┘
│ │ │
│ /smooth_path /a_star_path │
└──────────────┬──────────────────────┬─────────┘
│ │
▼ ▼
┌─────────────────┐ ┌──────────────────┐
│ planner │ │ smoother_obstacle│
│ (trajectory gen)│ │ (A* smoother) │
└─────────────────┘ └──────────────────┘
│ │
│ /smooth_path │
└──────────┬───────────┘
│
▼ /trajectory
┌─────────────────┐
│ controller │
│ (Pure Pursuit) │
└─────────────────┘
│
▼ /cmd_vel
┌─────────────────┐
│ TurtleBot4 │
│ Simulation │
└─────────────────┘
│
▼ /odom
┌─────────────────┐
│ visualizer │
│ (Path Analysis) │
└─────────────────┘
- Waypoint Input: Multiple sources feed navigation goals
- Path Planning: A* generates obstacle-free grid paths
- Path Smoothing: Cubic splines create continuous trajectories
- Trajectory Generation: Time parameterization with velocity profiles
- Trajectory Tracking: Pure Pursuit controller generates velocity commands
- State Feedback: Odometry provides position feedback for control and visualization
The navigation system implements a modular pipeline architecture with clear data flow and responsibilities:
Fig: System Architecture and Topic Flow Diagram
Algorithm: Cubic Spline Interpolation with Parametric Representation
The system implements a dual-method smoothing approach for robustness:
Primary Method - Parametric Splines:
- Uses
scipy.interpolate.splprepfor 2D parametric curve representation - Treats waypoints as control points for smooth curve generation
- Handles complex geometries including loops and sharp turns
- Generates 150 interpolated points for high-resolution paths
Fallback Method - Distance-Based Splines:
- Uses
scipy.interpolate.CubicSplinewith natural boundary conditions - Distance-parameterized sampling for uniform point distribution
- Provides robust interpolation when parametric method fails
Design Rationale: Cubic splines chosen over Bézier curves for exact waypoint interpolation and continuous second derivatives, ensuring smooth robot motion without oscillations.
Algorithm: Constant Velocity Time Parameterization
Implementation:
time_offset = cumulative_distance / linear_velocity # 0.5 m/s
yaw = atan2(y_next - y_current, x_next - x_current)Features:
- Constant linear velocity (0.5 m/s) for predictable motion
- Timestamp assignment based on path distance
- Orientation computation from successive waypoint vectors
- Starting trajectory from current robot pose (via
/odom)
Design Rationale: Constant velocity profiles chosen for simplicity and predictability. More sophisticated velocity planning (considering curvature limits) reserved for future improvements.
Algorithm: Pure Pursuit Controller
Mathematical Foundation:
Lookahead Point: L = 0.5m ahead on path
Heading Error: α = angle between robot heading and line to target
Curvature: κ = 2×sin(α) / L
Angular Velocity: ω = v × κ
Implementation Details:
- Lookahead Distance: 0.5m (tuned for TurtleBot4 dynamics)
- Control Frequency: 10Hz for responsive tracking
- Linear Velocity: 0.5m/s (increased from original 0.15m/s)
- Goal Tolerance: 0.3m for robust goal detection
Design Rationale: Pure Pursuit selected for its simplicity, stability, and proven performance with differential drive robots. The geometric nature provides intuitive tuning parameters.
Algorithm: A* Search with Safety Inflation and Proximity Costing
Grid Processing Pipeline:
- Obstacle Inflation: Expand obstacles by safety radius (0.5m)
- Distance Field Computation: BFS-based distance to nearest obstacle
- Proximity Costing: Exponential penalty near obstacles
- A Search*: 8-connected movement with heuristic guidance
Safety Features:
# Obstacle inflation for robot radius safety
inflation_radius = 0.5m # Robot radius + safety margin
# Exponential proximity cost near obstacles
if distance_to_obstacle < inflation_radius:
penalty = weight × (1 - normalized_distance)²Design Rationale: A* chosen for optimal pathfinding with safety guarantees. Grid-based planning in /map frame avoids odometry drift issues. Post-smoothing applied to A* output for optimal robot motion.
Architecture: TF2-Based Multi-Frame Coordination
Frame Strategy:
- Planning Frame:
/map- Global, drift-free planning - Control Frame:
/odom- Smooth, continuous control - Robot Frame:
/base_link- Local robot coordinates
Critical Design Decision: Planning in /map frame prevents accumulated odometry drift from affecting long-range navigation, while control in /odom frame ensures smooth trajectory tracking.
SLAM & Localization:
- Replace simulation mapping with real SLAM (e.g.,
slam_toolbox) - Implement robust localization using AMCL or equivalent
- Handle GPS-denied indoor navigation scenarios
Sensor Integration:
- LiDAR: Real-time obstacle detection and map updates
- Camera: Visual odometry and landmark-based localization
- IMU: Improved orientation estimation and dead reckoning
- Wheel Encoders: High-frequency odometry updates
Real-World Adaptations:
- Dynamic Obstacle Handling:
# Emergency stop based on laser scan
def laser_callback(self, scan_msg):
min_distance = min(scan_msg.ranges)
if min_distance < EMERGENCY_DISTANCE:
self.publish_stop_command()- Sensor Noise Filtering:
- Implement Kalman filtering for state estimation
- Use sensor fusion for robust localization
- Handle sensor failures gracefully
- Computational Constraints:
- Optimize A* grid resolution for real-time performance
- Implement hierarchical planning for large environments
- Use onboard computation-friendly controllers
- Safety Systems:
- Laser-based emergency stopping
- Cliff detection for mobile platforms
- Battery monitoring and return-to-dock behaviors
Performance Optimizations:
- Reduce path planning frequency (1-2Hz vs continuous)
- Implement path caching and incremental updates
- Use multi-threading for sensor processing and control
Dynamic Obstacle Avoidance:
- Implement D* Lite for real-time replanning
- Add velocity obstacles for moving obstacle prediction
- Integrate sensor-based reactive behaviors
Advanced Path Planning:
- Curvature-Constrained Planning: RRT* or hybrid A* for kinodynamic constraints
- Multi-Objective Optimization: Balance path length, safety, and energy efficiency
- Hierarchical Planning: Coarse global planning with fine local adjustments
Sophisticated Velocity Profiles:
- Curvature-Based Speed Control: Reduce velocity in sharp turns
- Acceleration Limits: Smooth acceleration/deceleration profiles
- Dynamic Window Approach: Real-time velocity optimization
Advanced Controllers:
- Model Predictive Control (MPC): Optimal trajectory tracking with constraints
- Adaptive Pure Pursuit: Dynamic lookahead distance based on velocity and curvature
- Learning-Based Control: Neural network controllers for complex environments
Full Nav2 Integration:
- Replace custom planner with Nav2 planner plugins
- Integrate behavior trees for complex navigation tasks
- Add recovery behaviors for navigation failures
Real-Time Capabilities:
- ROS2 Real-Time: Deterministic control loops for safety-critical applications
- Hardware Acceleration: GPU-based path planning for complex environments
- Distributed Computing: Multi-robot coordination and planning
Fault Tolerance:
- Sensor failure detection and graceful degradation
- Multiple planning algorithm fallbacks
- Network connectivity loss handling
Safety Systems:
- Certified safe control barriers for obstacle avoidance
- Formal verification of control algorithms
- Emergency stop and recovery protocols
This project demonstrates advanced autonomous navigation capabilities suitable for research and industrial applications. The modular architecture enables easy extension and customization for specific robotic platforms and mission requirements.