All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
GeometricCarPlanning.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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: Mark Moll */
36 
37 #include <ompl/base/spaces/DubinsStateSpace.h>
38 #include <ompl/base/spaces/ReedsSheppStateSpace.h>
39 #include <ompl/base/ScopedState.h>
40 #include <ompl/geometric/SimpleSetup.h>
41 #include <boost/program_options.hpp>
42 
43 namespace ob = ompl::base;
44 namespace og = ompl::geometric;
45 namespace po = boost::program_options;
46 
47 // The easy problem is the standard narrow passage problem: two big open
48 // spaces connected by a narrow passage. The hard problem is essentially
49 // one long narrow passage with the robot facing towards the long walls
50 // in both the start and goal configurations.
51 
52 bool isStateValidEasy(const ob::SpaceInformation *si, const ob::State *state)
53 {
55  double x=s->getX(), y=s->getY();
56  return si->satisfiesBounds(s) && (x<5 || x>13 || (y>8.5 && y<9.5));
57 }
58 
59 bool isStateValidHard(const ob::SpaceInformation *si, const ob::State *state)
60 {
61  return si->satisfiesBounds(state);
62 }
63 
64 void plan(ob::StateSpacePtr space, bool easy)
65 {
66  ob::ScopedState<> start(space), goal(space);
67  ob::RealVectorBounds bounds(2);
68  bounds.setLow(0);
69  if (easy)
70  bounds.setHigh(18);
71  else
72  {
73  bounds.high[0] = 6;
74  bounds.high[1] = .6;
75  }
76  space->as<ob::SE2StateSpace>()->setBounds(bounds);
77 
78  // define a simple setup class
79  og::SimpleSetup ss(space);
80 
81  // set state validity checking for this space
82  ob::SpaceInformationPtr si(ss.getSpaceInformation());
83  ss.setStateValidityChecker(boost::bind(
84  easy ? &isStateValidEasy : &isStateValidHard, si.get(), _1));
85 
86  // set the start and goal states
87  if (easy)
88  {
89  start[0] = start[1] = 1.; start[2] = 0.;
90  goal[0] = goal[1] = 17; goal[2] = -.99*boost::math::constants::pi<double>();
91  }
92  else
93  {
94  start[0] = start[1] = .5; start[2] = .5*boost::math::constants::pi<double>();;
95  goal[0] = 5.5; goal[1] = .5; goal[2] = .5*boost::math::constants::pi<double>();
96  }
97  ss.setStartAndGoalStates(start, goal);
98 
99  // this call is optional, but we put it in to get more output information
100  ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
101  ss.setup();
102  ss.print();
103 
104  // attempt to solve the problem within 30 seconds of planning time
105  ob::PlannerStatus solved = ss.solve(30.0);
106 
107  if (solved)
108  {
109  std::vector<double> reals;
110 
111  std::cout << "Found solution:" << std::endl;
112  ss.simplifySolution();
113  og::PathGeometric path = ss.getSolutionPath();
114  path.interpolate(1000);
115  path.printAsMatrix(std::cout);
116  }
117  else
118  std::cout << "No solution found" << std::endl;
119 }
120 
121 void printTrajectory(ob::StateSpacePtr space, const std::vector<double>& pt)
122 {
123  if (pt.size()!=3) throw ompl::Exception("3 arguments required for trajectory option");
124  const unsigned int num_pts = 50;
125  ob::ScopedState<> from(space), to(space), s(space);
126  std::vector<double> reals;
127 
128  from[0] = from[1] = from[2] = 0.;
129 
130  to[0] = pt[0];
131  to[1] = pt[1];
132  to[2] = pt[2];
133 
134  std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n";
135  for (unsigned int i=0; i<=num_pts; ++i)
136  {
137  space->interpolate(from(), to(), (double)i/num_pts, s());
138  reals = s.reals();
139  std::cout << "path " << reals[0] << ' ' << reals[1] << ' ' << reals[2] << ' ' << std::endl;
140  }
141 }
142 
143 void printDistanceGrid(ob::StateSpacePtr space)
144 {
145  // print the distance for (x,y,theta) for all points in a 3D grid in SE(2)
146  // over [-5,5) x [-5, 5) x [-pi,pi).
147  //
148  // The output should be redirected to a file, say, distance.txt. This
149  // can then be read and plotted in Matlab like so:
150  // x = reshape(load('distance.txt'),200,200,200);
151  // for i=1:200,
152  // contourf(squeeze(x(i,:,:)),30);
153  // axis equal; axis tight; colorbar; pause;
154  // end;
155  const unsigned int num_pts = 200;
156  ob::ScopedState<> from(space), to(space);
157  from[0] = from[1] = from[2] = 0.;
158 
159  for (unsigned int i=0; i<num_pts; ++i)
160  for (unsigned int j=0; j<num_pts; ++j)
161  for (unsigned int k=0; k<num_pts; ++k)
162  {
163  to[0] = 5. * (2. * (double)i/num_pts - 1.);
164  to[1] = 5. * (2. * (double)j/num_pts - 1.);
165  to[2] = boost::math::constants::pi<double>() * (2. * (double)k/num_pts - 1.);
166  std::cout << space->distance(from(), to()) << '\n';
167  }
168 
169 }
170 
171 int main(int argc, char* argv[])
172 {
173  try
174  {
175  po::options_description desc("Options");
176  desc.add_options()
177  ("help", "show help message")
178  ("dubins", "use Dubins state space")
179  ("dubinssym", "use symmetrized Dubins state space")
180  ("reedsshepp", "use Reeds-Shepp state space (default)")
181  ("easyplan", "solve easy planning problem and print path")
182  ("hardplan", "solve hard planning problem and print path")
183  ("trajectory", po::value<std::vector<double > >()->multitoken(),
184  "print trajectory from (0,0,0) to a user-specified x, y, and theta")
185  ("distance", "print distance grid")
186  ;
187 
188  po::variables_map vm;
189  po::store(po::parse_command_line(argc, argv, desc,
190  po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
191  po::notify(vm);
192 
193  if (vm.count("help") || argc==1)
194  {
195  std::cout << desc << "\n";
196  return 1;
197  }
198 
200 
201  if (vm.count("dubins"))
203  if (vm.count("dubinssym"))
204  space = ob::StateSpacePtr(new ob::DubinsStateSpace(1., true));
205  if (vm.count("easyplan"))
206  plan(space, true);
207  if (vm.count("hardplan"))
208  plan(space, false);
209  if (vm.count("trajectory"))
210  printTrajectory(space, vm["trajectory"].as<std::vector<double> >());
211  if (vm.count("distance"))
212  printDistanceGrid(space);
213  }
214  catch(std::exception& e) {
215  std::cerr << "error: " << e.what() << "\n";
216  return 1;
217  }
218  catch(...) {
219  std::cerr << "Exception of unknown type!\n";
220  }
221 
222  return 0;
223 }
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
An SE(2) state space where distance is measured by the length of Dubins curves.
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:543
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path...
const T * as(void) const
Cast this instance to a desired type.
Definition: State.h:74
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:64
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states. States are inserted uniformly (more states on longer segments). Changes are performed only if a path has less than count states.
A state space representing SE(2)
Definition: SE2StateSpace.h:50
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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 of an abstract state.
Definition: State.h:50
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
The exception type for ompl.
Definition: Exception.h:47
The lower and upper bounds for an Rn space.
Definition of a geometric path.
Definition: PathGeometric.h:55