37 #include <ompl/base/PlannerData.h>
38 #include <ompl/base/PlannerDataStorage.h>
39 #include <ompl/base/PlannerDataGraph.h>
40 #include <ompl/base/spaces/SE3StateSpace.h>
41 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
42 #include <ompl/geometric/SimpleSetup.h>
43 #include <ompl/base/goals/GoalState.h>
45 #include <boost/graph/astar_search.hpp>
48 namespace ob = ompl::base;
49 namespace og = ompl::geometric;
66 return (
const void*)rot != (
const void*)pos;
69 void planWithSimpleSetup(
void)
85 ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
96 ss.setStartAndGoalStates(start, goal);
107 std::cout << std::endl;
108 std::cout <<
"Found solution with " << slnPath.
getStateCount() <<
" states and length " << slnPath.
length() << std::endl;
112 std::cout <<
"Writing PlannerData to file './myPlannerData'" << std::endl;
114 ss.getPlannerData(data);
117 dataStorage.
store(data,
"myPlannerData");
120 std::cout <<
"No solution found" << std::endl;
124 ob::Cost distanceHeuristic(ob::PlannerData::Graph::Vertex v1,
127 const boost::property_map<ob::PlannerData::Graph::Type,
128 vertex_type_t>::type& plannerDataVertices)
133 void readPlannerData(
void)
135 std::cout << std::endl;
136 std::cout <<
"Reading PlannerData from './myPlannerData'" << std::endl;
146 dataStorage.
load(
"myPlannerData", data);
149 if (data.numStartVertices() > 0 && data.numGoalVertices() > 0)
156 data.computeEdgeWeights(opt);
159 ob::PlannerData::Graph::Type& graph = data.toBoostGraph();
164 boost::vector_property_map<ob::PlannerData::Graph::Vertex> prev(data.numVertices());
167 boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), graph);
171 goal.setState(data.getGoalVertex(0).getState());
172 ob::PlannerData::Graph::Vertex start = boost::vertex(data.getStartIndex(0), graph);
173 boost::astar_search(graph, start,
174 boost::bind(&distanceHeuristic, _1, &goal, &opt, vertices),
175 boost::predecessor_map(prev).
177 isCostBetterThan, &opt, _1, _2)).
179 combineCosts, &opt, _1, _2)).
180 distance_inf(opt.infiniteCost()).
181 distance_zero(opt.identityCost()));
185 for (ob::PlannerData::Graph::Vertex pos = boost::vertex(data.getGoalIndex(0), graph);
189 path.append(vertices[pos]->getState());
191 path.append(vertices[start]->getState());
196 std::cout <<
"Found stored solution with " << path.getStateCount() <<
" states and length " << path.length() << std::endl;
200 int main(
int,
char **)
203 planWithSimpleSetup();
Object that handles loading/storing a PlannerData object to/from a binary stream. Serialization of ve...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
Definition of a goal state.
CompoundState StateType
Define the type of state allocated by this state space.
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
State StateType
Define the type of state allocated by this space.
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 ...
const T * as(void) const
Cast this instance to a desired type.
Create the set of classes typically needed to solve a geometric problem.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
The planner found an exact solution.
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...
An optimization objective which corresponds to optimizing path length.
Definition of an abstract state.
Abstract definition of optimization objectives.
The lower and upper bounds for an Rn space.
Definition of a geometric path.
A state space representing SE(3)
virtual double length(void) const
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
virtual void store(const PlannerData &pd, const char *filename)
Store (serialize) the PlannerData structure to the given filename.