00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ompl/base/PlannerData.h>
00038 #include <ompl/base/PlannerDataStorage.h>
00039 #include <ompl/base/PlannerDataGraph.h>
00040 #include <ompl/base/spaces/SE3StateSpace.h>
00041 #include <ompl/geometric/SimpleSetup.h>
00042
00043 #include <boost/graph/astar_search.hpp>
00044 #include <iostream>
00045
00046 namespace ob = ompl::base;
00047 namespace og = ompl::geometric;
00048
00049 bool isStateValid(const ob::State *state)
00050 {
00051
00052 const ob::SE3StateSpace::StateType *se3state = state->as<ob::SE3StateSpace::StateType>();
00053
00054
00055 const ob::RealVectorStateSpace::StateType *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
00056
00057
00058 const ob::SO3StateSpace::StateType *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
00059
00060
00061
00062
00063
00064 return (void*)rot != (void*)pos;
00065 }
00066
00067 void planWithSimpleSetup(void)
00068 {
00069
00070 ob::StateSpacePtr space(new ob::SE3StateSpace());
00071
00072
00073 ob::RealVectorBounds bounds(3);
00074 bounds.setLow(-10);
00075 bounds.setHigh(10);
00076
00077 space->as<ob::SE3StateSpace>()->setBounds(bounds);
00078
00079
00080 og::SimpleSetup ss(space);
00081
00082
00083 ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
00084
00085
00086 ob::ScopedState<> start(space);
00087 start.random();
00088
00089
00090 ob::ScopedState<> goal(space);
00091 goal.random();
00092
00093
00094 ss.setStartAndGoalStates(start, goal);
00095
00096
00097 ss.setup();
00098 ss.print();
00099
00100
00101 if (ss.solve(5.0) == ob::PlannerStatus::EXACT_SOLUTION)
00102 {
00103 og::PathGeometric slnPath = ss.getSolutionPath();
00104
00105 std::cout << std::endl;
00106 std::cout << "Found solution with " << slnPath.getStateCount() << " states and length " << slnPath.length() << std::endl;
00107
00108
00109
00110 std::cout << "Writing PlannerData to file './myPlannerData'" << std::endl;
00111 ob::PlannerData data(ss.getSpaceInformation());
00112 ss.getPlannerData(data);
00113
00114 ob::PlannerDataStorage dataStorage;
00115 dataStorage.store(data, "myPlannerData");
00116 }
00117 else
00118 std::cout << "No solution found" << std::endl;
00119 }
00120
00121
00122 double distanceHeuristic(ob::PlannerData::Graph::Vertex v1, const ob::State* goal, const ob::StateSpacePtr& space,
00123 const boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type& plannerDataVertices)
00124 {
00125 return space->distance(plannerDataVertices[v1]->getState(), goal);
00126 }
00127
00128 void readPlannerData(void)
00129 {
00130 std::cout << std::endl;
00131 std::cout << "Reading PlannerData from './myPlannerData'" << std::endl;
00132
00133
00134 ob::StateSpacePtr space(new ob::SE3StateSpace());
00135 ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
00136
00137 ob::PlannerDataStorage dataStorage;
00138 ob::PlannerData data(si);
00139
00140
00141 dataStorage.load("myPlannerData", data);
00142
00143
00144 if (data.numStartVertices() > 0 && data.numGoalVertices() > 0)
00145 {
00146
00147
00148 data.computeEdgeWeights();
00149
00150
00151 ob::PlannerData::Graph::Type& graph = data.toBoostGraph();
00152
00153
00154
00155
00156 boost::vector_property_map<ob::PlannerData::Graph::Vertex> prev(data.numVertices());
00157
00158
00159 boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), graph);
00160
00161
00162 ob::PlannerData::Graph::Vertex goal = boost::vertex(data.getGoalIndex(0), graph);
00163 ob::PlannerData::Graph::Vertex start = boost::vertex(data.getStartIndex(0), graph);
00164 boost::astar_search(graph, start,
00165 boost::bind(&distanceHeuristic, _1, vertices[goal]->getState(), space, vertices),
00166 boost::predecessor_map(prev));
00167
00168
00169 og::PathGeometric path(si);
00170 for (ob::PlannerData::Graph::Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
00171 path.append(vertices[pos]->getState());
00172 path.append(vertices[start]->getState());
00173 path.reverse();
00174
00175
00176
00177 std::cout << "Found stored solution with " << path.getStateCount() << " states and length " << path.length() << std::endl;
00178 }
00179 }
00180
00181 int main(int, char **)
00182 {
00183
00184 planWithSimpleSetup();
00185
00186
00187 readPlannerData();
00188
00189 return 0;
00190 }