PlannerData.cpp
57 const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
65 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
108 std::cout << "Found solution with " << slnPath.getStateCount() << " states and length " << slnPath.length() << std::endl;
167 boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), graph);
196 std::cout << "Found stored solution with " << path.getStateCount() << " states and length " << path.length() << std::endl;
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
Definition: PlannerDataStorage.h:79
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
A boost shared pointer wrapper for ompl::base::StateSpace.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Definition: PathGeometric.h:237
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
virtual void load(const char *filename, PlannerData &pd)
Load the PlannerData structure from the given stream. The StateSpace that was used to store the data ...
Definition: PlannerDataStorage.cpp:91
Cost costToGo(const State *state, const Goal *goal) const
Uses a cost-to-go heuristic to calculate an admissible estimate of the optimal cost from a given stat...
Definition: OptimizationObjective.cpp:125
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:65
The planner found an exact solution.
Definition: PlannerStatus.h:66
virtual double length() const
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
Definition: PathGeometric.cpp:99
A boost shared pointer wrapper for ompl::base::SpaceInformation.
The base class for space information. This contains all the information about the space planning is d...
Definition: SpaceInformation.h:86
An optimization objective which corresponds to optimizing path length.
Definition: PathLengthOptimizationObjective.h:47
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
Abstract definition of optimization objectives.
Definition: OptimizationObjective.h:66
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:48
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.
Definition: PlannerDataStorage.cpp:50