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/geometric/SimpleSetup.h>
43 #include <boost/graph/astar_search.hpp>
46 namespace ob = ompl::base;
47 namespace og = ompl::geometric;
64 return (
void*)rot != (
void*)pos;
67 void planWithSimpleSetup(
void)
83 ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
94 ss.setStartAndGoalStates(start, goal);
105 std::cout << std::endl;
106 std::cout <<
"Found solution with " << slnPath.
getStateCount() <<
" states and length " << slnPath.
length() << std::endl;
110 std::cout <<
"Writing PlannerData to file './myPlannerData'" << std::endl;
112 ss.getPlannerData(data);
115 dataStorage.
store(data,
"myPlannerData");
118 std::cout <<
"No solution found" << std::endl;
123 const boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type& plannerDataVertices)
125 return space->distance(plannerDataVertices[v1]->getState(), goal);
128 void readPlannerData(
void)
130 std::cout << std::endl;
131 std::cout <<
"Reading PlannerData from './myPlannerData'" << std::endl;
141 dataStorage.
load(
"myPlannerData", data);
144 if (data.numStartVertices() > 0 && data.numGoalVertices() > 0)
148 data.computeEdgeWeights();
151 ob::PlannerData::Graph::Type& graph = data.toBoostGraph();
156 boost::vector_property_map<ob::PlannerData::Graph::Vertex> prev(data.numVertices());
159 boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type vertices =
get(vertex_type_t(), graph);
162 ob::PlannerData::Graph::Vertex goal = boost::vertex(data.getGoalIndex(0), graph);
163 ob::PlannerData::Graph::Vertex start = boost::vertex(data.getStartIndex(0), graph);
164 boost::astar_search(graph, start,
165 boost::bind(&distanceHeuristic, _1, vertices[goal]->getState(), space, vertices),
166 boost::predecessor_map(prev));
170 for (ob::PlannerData::Graph::Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
171 path.append(vertices[pos]->getState());
172 path.append(vertices[start]->getState());
177 std::cout <<
"Found stored solution with " << path.getStateCount() <<
" states and length " << path.length() << std::endl;
181 int main(
int,
char **)
184 planWithSimpleSetup();