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  // We can't use regular simplify because it also tries to use spline interpolation,
113  // which doesn't work for Dubins curves.
114  //ss.simplifySolution();
115  og::PathGeometric path = ss.getSolutionPath();
116  og::PathSimplifierPtr ps = ss.getPathSimplifier();
117  ps->reduceVertices(path);
118  ps->collapseCloseVertices(path);
119  path.interpolate(1000);
120  for (unsigned int i=0; i < path.getStateCount(); ++i)
121  {
122  reals = ob::ScopedState<>(space, path.getState(i)).reals();
123  std::cout << "path " << reals[0] <<' '<< reals[1] << ' ' << reals[2] << std::endl;
124  }
125  }
126  else
127  std::cout << "No solution found" << std::endl;
128 }
129 
130 void printTrajectory(ob::StateSpacePtr space, const std::vector<double>& pt)
131 {
132  if (pt.size()!=3) throw ompl::Exception("3 arguments required for trajectory option");
133  const unsigned int num_pts = 50;
134  ob::ScopedState<> from(space), to(space), s(space);
135  std::vector<double> reals;
136 
137  from[0] = from[1] = from[2] = 0.;
138 
139  to[0] = pt[0];
140  to[1] = pt[1];
141  to[2] = pt[2];
142 
143  std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n";
144  for (unsigned int i=0; i<=num_pts; ++i)
145  {
146  space->interpolate(from(), to(), (double)i/num_pts, s());
147  reals = s.reals();
148  std::cout << "path " << reals[0] << ' ' << reals[1] << ' ' << reals[2] << ' ' << std::endl;
149  }
150 }
151 
152 void printDistanceGrid(ob::StateSpacePtr space)
153 {
154  // print the distance for (x,y,theta) for all points in a 3D grid in SE(2)
155  // over [-5,5) x [-5, 5) x [-pi,pi).
156  //
157  // The output should be redirected to a file, say, distance.txt. This
158  // can then be read and plotted in Matlab like so:
159  // x = reshape(load('distance.txt'),200,200,200);
160  // for i=1:200,
161  // contourf(squeeze(x(i,:,:)),30);
162  // axis equal; axis tight; colorbar; pause;
163  // end;
164  const unsigned int num_pts = 200;
165  ob::ScopedState<> from(space), to(space);
166  from[0] = from[1] = from[2] = 0.;
167 
168  for (unsigned int i=0; i<num_pts; ++i)
169  for (unsigned int j=0; j<num_pts; ++j)
170  for (unsigned int k=0; k<num_pts; ++k)
171  {
172  to[0] = 5. * (2. * (double)i/num_pts - 1.);
173  to[1] = 5. * (2. * (double)j/num_pts - 1.);
174  to[2] = boost::math::constants::pi<double>() * (2. * (double)k/num_pts - 1.);
175  std::cout << space->distance(from(), to()) << '\n';
176  }
177 
178 }
179 
180 int main(int argc, char* argv[])
181 {
182  try
183  {
184  po::options_description desc("Options");
185  desc.add_options()
186  ("help", "show help message")
187  ("dubins", "use Dubins state space")
188  ("dubinssym", "use symmetrized Dubins state space")
189  ("reedsshepp", "use Reeds-Shepp state space (default)")
190  ("easyplan", "solve easy planning problem and print path")
191  ("hardplan", "solve hard planning problem and print path")
192  ("trajectory", po::value<std::vector<double > >()->multitoken(),
193  "print trajectory from (0,0,0) to a user-specified x, y, and theta")
194  ("distance", "print distance grid")
195  ;
196 
197  po::variables_map vm;
198  po::store(po::parse_command_line(argc, argv, desc,
199  po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
200  po::notify(vm);
201 
202  if (vm.count("help") || argc==1)
203  {
204  std::cout << desc << "\n";
205  return 1;
206  }
207 
209 
210  if (vm.count("dubins"))
212  if (vm.count("dubinssym"))
213  space = ob::StateSpacePtr(new ob::DubinsStateSpace(1., true));
214  if (vm.count("easyplan"))
215  plan(space, true);
216  if (vm.count("hardplan"))
217  plan(space, false);
218  if (vm.count("trajectory"))
219  printTrajectory(space, vm["trajectory"].as<std::vector<double> >());
220  if (vm.count("distance"))
221  printDistanceGrid(space);
222  }
223  catch(std::exception& e) {
224  std::cerr << "error: " << e.what() << "\n";
225  return 1;
226  }
227  catch(...) {
228  std::cerr << "Exception of unknown type!\n";
229  }
230 
231  return 0;
232 }