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...
STL namespace.
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
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...
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:65
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
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:60
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48