Octomap planner
This package comprises the algorithms for planning of paths in 3D environments represented as an octomap. The path planning is followed by generation of a trajectory along the planned path, which is then sent as a request for trajectory tracking to the MRS trajectory tracker. The node considers the current state of the UAV during the trajectory generation process and correctly appends the trajectory following the new path to the current prediction horizon without violating dynamic constraints. The Octomap Planner implements also periodic replanning of the path, checks that the current trajectory is not in collision with the most recent map of the environment and prevents potential collisions by cutting off the coliding segment of the trajectory.
After the planning request to user-specified goal is obtained, the Octomap Planner proceeds as follows:
- gets an initial condition for planning from the current MPC prediction (x seconds in the future),
- starts the planning algorithm to get a path from initial condition to the user-specified goal,
- cuts the generated path to the required length to prevent generating unnecesarry long trajectory,
- sends the trajectory generation request to MRS UAV trajectory generator,
- publishes the obtained trajectory reference to MRS trajectory tracker,
- repeats the above described steps until the user-specified goal is reached while preventing potential collisions with the environment.
Configuration and use
ROS interface
Input: The user-specified goal can be passed to Octomap Planner as a service of type mrs_msgs/Vec4 or mrs_msgs/ReferenceStampedSrv on topics:
/uav*/octomap_planner/goto
/uav*/octomap_planner/reference
The planning can be stopped by calling service
/uav*/octomap_planner/stop
which stops the planning process and sets the UAV to hovering state.
Output: after each planning phase the node calls /uav*/control_manager/trajectory_reference service provided by the ControlManager.
Planner types
The Octomap Planner enables to switch between MRS A planner* implementation and MRS SubT planner using the service of type std/String.
/uav*/octomap_planner/set_planner subt/mrs
Both implementations in default setting provide the optimal path given the current map represented by an octomap. The MRS SubT Planner provides functionality to push the path further from the obstacles above the specified safety distance and lowers the computational demands in the majority of path planning instances. Further, it can be parametrized to allow generation of paths up to x-times longer than optimal path, which significantly speeds up the planning process. The paths produced by MRS A Planner* are generally more straight, which makes the planner favoured for fast flight in open environments with low obstacle density.
Planning for sensors with limited horizontal field of view
The Octomap planner allows to sample the heading reference in a direction of flight and also with a constant heading_offset from a direction of flight. The use of this feature is recommended for UAVs that build occupancy maps using sensors with a limited horizontal field of view (e.g., depth cameras) and either fly in potentially dynamic environments or apply localization methods that experience drift.
trajectory_generator:
turn_in_flight_direction: true
heading_offset: 0.0 # [rad]