Simple Path Planner (ROS 2)

A robust, lightweight, and non-blocking global path planner for mobile robots implemented in C++ for ROS 2.

This project implements a custom grid-based A* (A-Star) algorithm with dynamic map inflation (Configuration Space) and post-process path smoothing. It is designed to operate asynchronously, ensuring the main ROS event loop remains responsive for high-frequency visualization and data handling.

Key Features

• Custom A Implementation:* Efficient grid search using a min-heap priority queue logic, independent of heavy external navigation stacks.
• Asynchronous Architecture: Utilizes std::async and std::future to offload computationally intensive pathfinding tasks to a worker thread. This prevents the Rviz visualization and ROS callbacks from freezing (non-blocking UI).
• Dynamic Map Inflation: Calculates the Configuration Space (C-Space) in real-time based on robot_radius and safety margins, effectively treating the robot as a point mass.
• Path Smoothing: Implements a Gradient Descent (Elastic Band) algorithm to relax jagged grid paths into smooth trajectories using a double-buffering technique for numerical stability.
• Thread-Safe Visualization: Features a dedicated, buffered visualization publisher with mutex synchronization (std::mutex) to ensure no data loss during high-speed algorithm expansion rendering.

System Architecture

The node operates using a multi-threaded approach to separate ROS callbacks from computational logic:

1. Main Thread (Event Loop): Handles ROS 2 callbacks (map updates, parameter changes), manages the visualization timer, and publishes the final path.
2. Worker Thread (std::async): Spawned when a new goal is received. It performs the heavy lifting: map inflation, heuristic calculation, and the A* search loop.
3. Synchronization: Access to shared resources (specifically the visualization buffer) is protected via std::mutex to prevent data races between the worker thread and the visualization timer.

Algorithm Pipeline

1. Input: Subscribes to Static Map (/map), Start Pose (/initialpose), and Goal Pose (/goal_pose).
2. Inflation: Map obstacles are expanded by robot_radius + boundary_safety_margin using Multi-Source BFS.
3. Search: A* algorithm finds the shortest optimal path on the inflated grid graph.
4. Smoothing: The raw path acts as an initial guess for the elastic band algorithm, which iteratively optimizes waypoints to reduce jaggedness.
5. Output: A smooth nav_msgs/Path is published to /path.

Installation & Build

Requirements:

• ROS 2 Jazzy
• C++17 compliant compiler

Optional Prerequisites (Map Server):

This node subscribes to the /map topic and does not publish a map itself. If you do not have a custom map provider (SLAM or a custom server), you will need the standard Navigation2 map server:

sudo apt install ros-$ROS_DISTRO-nav2-map-server

# 1. Create a workspace (if you haven't already)
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# 2. Clone the repository
git clone <YOUR_REPOSITORY_URL> simple_path_planner

# 3. Install dependencies
cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y

# 4. Build the package
colcon build --packages-select simple_path_planner --symlink-install

# 5. Source the workspace
source install/setup.bash

Usage

1. Prerequisite: Publish a Map

The planner requires an occupancy grid map to function. Since this node is a consumer, you must have a map server running. Example using standard Navigation2 map server:

ros2 run nav2_map_server map_server --ros-args -p yaml_filename:=/path/to/your/map.yaml
ros2 lifecycle set /map_server configure
ros2 lifecycle set /map_server activate

2. Run the Planner Node

Launch the path planner node:

ros2 run simple_path_planner path_planner

3. Configure Visualization (Rviz2)

To visualize the algorithm's expansion and the resulting path, open Rviz2 and configure the displays manually as follows.

Global Options:

• Fixed Frame: Set to map

1. Map (OccupancyGrid):

• Topic: /map
• QoS - Durability: Transient Local
• QoS - Reliability: Reliable
• Color Scheme: map

2. Path:

• Topic: /path
• Line Style: Billboards
• Line Width: 0.3
• Color: Green (R: 25, G: 255, B: 0)
• QoS: Reliable, Transient Local

3. Marker:

• Topic: /draw
• History Policy: Keep All or Keep Last
• QoS - Reliability: Reliable
• QoS - Durability: Transient Local

4. Execute Planning: Once Rviz2 is set up:

1. Select the 2D Pose Estimate tool in the top toolbar and click on a free space in the map to set the Start Point.
2. Select the 2D Goal Pose tool and click on another location to set the Target.
3. The algorithm will visualize the search process (Red Cloud) and draw the final smoothed path (Green Line) once calculation is complete.

Utilities: Random Map Generator

To facilitate robust testing and benchmarking, the package includes a dedicated Python script located in the maps/ directory. This tool generates procedural occupancy grid maps with customizable parameters.

Features:

• Full Parametrization: Allows configuration of map dimensions (width, height), resolution, and obstacle density/fill percentage.
• ROS 2 Compatibility: Outputs standard .pgm and .yaml files ready to be loaded by the Navigation2 Map Server.
• Randomization: Ensures unique environments for every run to test the planner's adaptability.

Usage: Navigate to the maps directory and run the script:

cd src/simple_path_planner/maps
# Run the generator (ensure you have necessary libs like numpy installed)
python3 generate_random.py

Configuration (Parameters)

The node supports dynamic parameter reconfiguration. You can change these values via launch files or at runtime using ros2 param set.

Parameter Name	Type	Default Value	Description
frame_id	string	"map"	The reference frame in which the path planning is performed.
robot_radius	double	0.15	The physical radius of the robot in meters. Used to calculate the Configuration Space (map inflation).
boundary_safety_margin	double	0.1	Additional safety buffer (in meters) added to the robot radius during map inflation to prevent collisions.
smoothing_factor	double	0.1	Controls the intensity of the path smoothing algorithm (Range: 0.001 - 1.0). Higher values mean stronger smoothing.
expansion_draw_buffer	int	170	Number of visited nodes packed into a single visualization message. Controls the rendering speed/smoothness of the expansion cloud.
marker_height	double	0.01	The Z-axis height of the visualization markers (useful to display markers slightly above the map layer).
marker_color_rgb	double[]	[1.0, 0.0, 0.0]	RGB color vector for the expansion markers. Values must be between 0.0 and 1.0.