Planner Structure
In this repository we propose a framework for sampling based receding horizon planning. For this purpose we provide a common scaffolding that structures all planners. A planner consists of 4 essential ingredients. The planner structure is depicted and further described below.
High level modular structure of a planner.
Optional low level structure of the core components, white operations can be delegated to corresponding modules.
This is the master node containing the 3 other classes as members. In a receding horizon fashion, a tree of possible trajectory segments is continuously expanded and expected costs and gains are computed. Once the current trajectory segment has finished executing, the next best adjacent segment is published and the trajectory tree updated. Furthermore, the main planner contains a backtracker, which tells the planner what to do in case it gets stuck somewhere.
The trajectory generator is responsible for expanding the trajectory tree. To guarantee constraint satisfaction, the proposed new trajectories need to fulfill all system (such as maximum thrusts, rates, ...) and environment (such as collision, ...) constraints. All constraints therefore need to be incorporated in the trajectory generator. Trajectory generators need to implement the following virtual functions:
- selectSegment Expansion policy where to expand the current tree.
- expandSegment Add adjacent trajectory segments to the target segment.
- updateSegments Whether and how to update existing segments when a new trajectory is executed.
The trajectory evaluator computes expected gains, costs and final values for different trajectory segments. Trajectory generators need to implement the following virtual functions:
- computeGain Compute the expected gain from executing a trajectory segment.
- computeCost Compute the expected cost from executing a trajectory segment.
- computeValue Assign the final value for a trajectory segment, usually f(gain, cost, ...).
- selectNextBest Policy for executing the next segment.
- updateSegments Whether and how to update existing segments when a new trajectory is executed.
- visualizeTrajectoryValue Optional. How to display the expected gain in RVIZ.
The main planner includes a voxblox server, which is updated from a separate voxblox node and contains all information about the environment. The server is made available to the trajectory generator and evaluator for constraint satisfaction, gain computation and more.
The underlying data structure is a tree of trajectory segments. All child segments are owned by their immediate parent, the root segment is owned by the planner node.