All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PlannerData.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ryan Luna */
36 
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>
42 
43 #include <boost/graph/astar_search.hpp>
44 #include <iostream>
45 
46 namespace ob = ompl::base;
47 namespace og = ompl::geometric;
48 
49 bool isStateValid(const ob::State *state)
50 {
51  // cast the abstract state type to the type we expect
53 
54  // extract the first component of the state and cast it to what we expect
56 
57  // extract the second component of the state and cast it to what we expect
59 
60  // check validity of state defined by pos & rot
61 
62 
63  // return a value that is always true but uses the two variables we define, so we avoid compiler warnings
64  return (void*)rot != (void*)pos;
65 }
66 
67 void planWithSimpleSetup(void)
68 {
69  // construct the state space we are planning in
71 
72  // set the bounds for the R^3 part of SE(3)
73  ob::RealVectorBounds bounds(3);
74  bounds.setLow(-10);
75  bounds.setHigh(10);
76 
77  space->as<ob::SE3StateSpace>()->setBounds(bounds);
78 
79  // define a simple setup class
80  og::SimpleSetup ss(space);
81 
82  // set state validity checking for this space
83  ss.setStateValidityChecker(boost::bind(&isStateValid, _1));
84 
85  // create a random start state
86  ob::ScopedState<> start(space);
87  start.random();
88 
89  // create a random goal state
90  ob::ScopedState<> goal(space);
91  goal.random();
92 
93  // set the start and goal states
94  ss.setStartAndGoalStates(start, goal);
95 
96  // this call is optional, but we put it in to get more output information
97  ss.setup();
98  ss.print();
99 
100  // attempt to find an exact solution within five seconds
101  if (ss.solve(5.0) == ob::PlannerStatus::EXACT_SOLUTION)
102  {
103  og::PathGeometric slnPath = ss.getSolutionPath();
104 
105  std::cout << std::endl;
106  std::cout << "Found solution with " << slnPath.getStateCount() << " states and length " << slnPath.length() << std::endl;
107  // print the path to screen
108  //slnPath.print(std::cout);
109 
110  std::cout << "Writing PlannerData to file './myPlannerData'" << std::endl;
111  ob::PlannerData data(ss.getSpaceInformation());
112  ss.getPlannerData(data);
113 
114  ob::PlannerDataStorage dataStorage;
115  dataStorage.store(data, "myPlannerData");
116  }
117  else
118  std::cout << "No solution found" << std::endl;
119 }
120 
121 // Used for A* search. Computes the heuristic distance from vertex v1 to the goal
122 double distanceHeuristic(ob::PlannerData::Graph::Vertex v1, const ob::State* goal, const ob::StateSpacePtr& space,
123  const boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type& plannerDataVertices)
124 {
125  return space->distance(plannerDataVertices[v1]->getState(), goal);
126 }
127 
128 void readPlannerData(void)
129 {
130  std::cout << std::endl;
131  std::cout << "Reading PlannerData from './myPlannerData'" << std::endl;
132 
133  // Recreating the space information from the stored planner data instance
136 
137  ob::PlannerDataStorage dataStorage;
138  ob::PlannerData data(si);
139 
140  // Loading an instance of PlannerData from disk.
141  dataStorage.load("myPlannerData", data);
142 
143  // Re-extract the shortest path from the loaded planner data
144  if (data.numStartVertices() > 0 && data.numGoalVertices() > 0)
145  {
146  // Computing the weights of all edges based on the state space distance
147  // This is not done by default for efficiency
148  data.computeEdgeWeights();
149 
150  // Getting a handle to the raw Boost.Graph data
151  ob::PlannerData::Graph::Type& graph = data.toBoostGraph();
152 
153  // Now we can apply any Boost.Graph algorithm. How about A*!
154 
155  // create a predecessor map to store A* results in
156  boost::vector_property_map<ob::PlannerData::Graph::Vertex> prev(data.numVertices());
157 
158  // Retieve a property map with the PlannerDataVertex object pointers for quick lookup
159  boost::property_map<ob::PlannerData::Graph::Type, vertex_type_t>::type vertices = get(vertex_type_t(), graph);
160 
161  // Run A* search over our planner data
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));
167 
168  // Extracting the path
169  og::PathGeometric path(si);
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());
173  path.reverse();
174 
175  // print the path to screen
176  //path.print(std::cout);
177  std::cout << "Found stored solution with " << path.getStateCount() << " states and length " << path.length() << std::endl;
178  }
179 }
180 
181 int main(int, char **)
182 {
183  // Plan and save all of the planner data to disk
184  planWithSimpleSetup();
185 
186  // Read in the saved planner data and extract the solution path
187  readPlannerData();
188 
189  return 0;
190 }