Cost-Aware A* Path Planner for ROS 2

A custom global path planning algorithm for the Nav2 navigation stack that uses cost-aware A* search to generate safe, efficient paths while avoiding obstacles.

Project Background

This project is a continuation of Exercise 5 - Path Planning from RXR Course from UEF, Finland.

Original Repository (Exercise 5): [Link to Exercise 5 repository]

A heartfelt thank you <3! It was simply sublime to learn from you during this process!

What's New in This Project:

• Replaced straight-line planner with cost-aware A* algorithm
• Added costmap integration for obstacle-aware pathfinding
• Implemented 8-directional movement with diagonal support
• Added extensive parameter tuning for low-lying obstacle detection

Acknowledgments

• Based on the Exercise 5 boilerplate from Henki Robotics
• Original straight-line planner template provided by course materials
• Extended and enhanced for this mini-project assignment

Overview

This project implements an intelligent path planner for the Andino robot that:

• Uses A* search algorithm with costmap-aware traversal costs
• Integrates with ROS 2 Nav2 navigation stack
• Navigates around obstacles (including the notoriously tricky low lying planks)
• Generates near-optimal paths prioritizing safety

Features

• Cost-Aware Pathfinding: Incorporates costmap values into path cost calculation
• 8-Directional Movement: Supports diagonal movement for more natural paths
• Safety Guarantees: Infinite cost for lethal obstacles ensures collision-free paths
• ROS 2 Integration: Compatible with Nav2 global planner interface

Installation

# Navigate to your ROS 2 workspace
cd ~/ros2_ws/src

# Clone the repository
git clone https://github.com/altCourier/ros2-pathplanner-project.git
cd ~/ros2_ws

# Build with colcon
colcon build
source install/setup.bash

Usage

1. Launch the Andino Robot in Gazebo with Navigation

# Terminal 1: Launch Andino in Gazebo with Nav2
ros2 launch andino_gz andino_gz.launch.py nav2:=True

2. Run the Custom Path Planner Node

# Terminal 2: Start the cost-aware A* planner
ros2 run path_planner_example path_planner_node --ros-args -p use_sim_time:=True

3. Set Navigation Goals

You can now send navigation goals through:

• RViz: Click "2D Goal Pose" button and click on the map
• Command line:

  ros2 topic pub /goal_pose geometry_msgs/PoseStamped "{header: {frame_id: 'map'}, pose: {position: {x: 2.0, y: 1.0, z: 0.0}, orientation: {w: 1.0}}}"

• Nav2 Simple Commander API (Python script)

4. Verify the Planner is Running

# Check if the service is available
ros2 service list | grep create_plan

# Monitor planner output
ros2 run rqt_console rqt_console

---

Quick Start (All-in-One)

For convenience, you can run all commands in separate terminals:

# Terminal 1: Simulation + Navigation
ros2 launch andino_gz andino_gz.launch.py nav2:=True

# Terminal 2: Custom Planner (wait for Gazebo to load first)
ros2 run path_planner_example path_planner_node --ros-args -p use_sim_time:=True

# Terminal 3: Monitor logs (optional)
ros2 run rqt_console rqt_console

Then use RViz to set navigation goals and watch the robot navigate using your cost-aware A* planner!

Algorithm Details

Complexity

• Time Complexity: O(n log n) where n is the number of grid cells
• Space Complexity: O(n)

Known Limitations

• No path smoothing (angular trajectories)
• Static planning only (no dynamic replanning)
• May reject very narrow passages (< 0.4m)

Dependencies

• ROS 2 Humble
• Nav2 navigation stack
• Python 3.8+

License

MIT License

Author

altCourier RXR Course from UEF, Finland December 2025