#include <roadmapPlanner.h>
Public Member Functions | |
RoadmapPlanner (TpbvpSolver *solver, bool reverse=true, size_t knn=5) | |
Constructor. | |
Path * | getPath (const PathPoint &wp0, const PathPoint &wpf, Shape *body, const std::vector< Point > &obstacles) |
Returns a path that goes from one waypoint to another. | |
void | addWaypoint (PathPoint wp) |
Adds a waypoint. | |
void | addWaypoints (std::vector< PathPoint > wps) |
Adds a set of waypoints. | |
void | clearWaypoints () |
Clears all waypoints. | |
void | removeWaypoint (PathPoint wp) |
Removes a waypoint. | |
void | removeWaypoints (std::vector< PathPoint > wps) |
Removes a set of waypoints. |
RoadmapPlanner::RoadmapPlanner | ( | TpbvpSolver * | solver, | |
bool | reverse = true , |
|||
size_t | knn = 5 | |||
) |
Constructor.
solver | two point boundary value solver | |
reverse | set to true if the vehicle can move in reverse | |
knn | max number of neighbors each waypoint has |
Path * RoadmapPlanner::getPath | ( | const PathPoint & | wp0, | |
const PathPoint & | wpf, | |||
Shape * | body, | |||
const std::vector< Point > & | obstacles | |||
) | [virtual] |
Returns a path that goes from one waypoint to another.
wp0 | initial waypoint | |
wpf | final waypoint | |
body | vehicle shape | |
obstacles | point cloud of obstacles |
Implements navlib::PathPlanner.
References navlib::PathPoint::getDistance(), navlib::TpbvpSolver::getPath(), navlib::PathChain::insert(), and navlib::VpTree< T >::search().