This repository contains a LPA* algorithm implementation. It also contains some abstractions and classes to inherit from if you want to customize you dynamic system behaviour. Check out python documentation and examples. Below, you have the basic explanation of the algorithm and description of the project.
This project contains lpastar_pf package, which you can build and install with pip3. It is not available on the PyPi for the moment. To install init run make install. You can also just build it with make build or init it for development by running make init.
It also contains ros package with path-finder node definition and interfaces and clients to communicate with sensor and robot nodes. You must just implement the servers.
Finally, it contains simulator package with complete path-finder simulation in dynamic environment.
You can check out examples if you want.
LPA* is an algorithm which allows us to find the shortest path from the start vertex to the goal vertex. It proceeds exactly as A*, but is adapted to the dynamic changes of the map in condition that the map is known at every instant of time.
The implementation of this algorithm is based on the Sven Koeing, Maxim Likhachev D* Lite paper. Here is a pseudocode and intuitive explanation of the algorithm:
LPA* uses 3 functions g, rhs and h.
- g(s) represents the minimal distance from
to s.
- rhs(s) is a one-step lookup based on g values.
, where c(s', s) is a transition cost from s' to s. rhs has more information and influences the expansion of the vertices.
- h(s, s') is a heuristics. It is used by LPA* to expand only on vertices "which make sense" before expand on the other vertices if the obstacles are present on the way.
Main function initializes all variables that matter for path-finding. It assigns all g-values and rhs-values to infinity and then puts . Then, while our agent (robot for example) does not reach the goal we compute the shortest path with current map configuration. We rescan the full map then and we update each vertex for which the edge cost has been changed.
UpdateVertex function recalculates the rhs-value of the vertex using the following formula: . Then it removes the vertex from priority queue and reinserts it if g-value of the vertex is not equal to its rhs-value (there is possible optimization).
This project allows you to implement your own path-finding agent (robot + sensor) by extending GRobot and ASensor classes. It also provides possibility to use it as a ROS node(in developement).
Here is the UML-diagram of the project that can help you in better understanding of the architecture:
