Interactive Path Planning

A useful, and dynamic path planner that works is a rarity. Yet, side projects, industry, and everything in between can benefit from a platform that can perform path planning efficiently. These ROS2 packages plan to implement this by reading SLAM map files, and publishing interactive markers in RVIZ to dynamically plan paths from the start node to the goal node. We utilize the a-star algorithm, as it is one of the most common for finding shortest paths.

Most map files are in the .pgm and .yaml pair formats. The current iteration of these packages read the map from a PNG file instead. A PNG is lightweight, easily convertible, and most importantly easy to read. There is no metadata to access, you can simply use OpenCV to loop through each pixel. We read the pixels from the PNG map file and publish them on an occupancy_grid topic, which is then displayed in RVIZ on the /map fixed frame. This sets the stage for planning paths, where the white space is free, and the dark spaces are occupied.

The A algorithm* is a popular pathfinding and graph traversal algorithm that efficiently finds the shortest path between two points by combining the benefits of Dijkstra's algorithm (guaranteed shortest path) and greedy best-first search (heuristic-based efficiency). It is ideal for path planning because it uses a heuristic function to estimate the cost to the goal, making it faster and more efficient than algorithms that explore all possible paths blindly. The algorithm works by maintaining a priority queue of nodes to explore, prioritizing those with the lowest total cost (known cost from the start + estimated cost to the goal), and iteratively expanding the most promising paths until the goal is reached.

The start and goal nodes are published as interactive marker messages in ROS2 allowing the user to move them. This interactive element updates the path automatically, where the user can visualize the updates to the path in real time. Although the environment is static, this could be a great tool for others to use in factory, warehouse, or other semi controlled environments.