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 {
54  const auto *s = state->as<ob::SE2StateSpace::StateType>();
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(const 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  const ob::SpaceInformation *si = ss.getSpaceInformation().get();
83  auto isStateValid = easy ? isStateValidEasy : isStateValidHard;
84  ss.setStateValidityChecker([isStateValid, si](const ob::State *state)
85  {
86  return isStateValid(si, state);
87  });
88 
89  // set the start and goal states
90  if (easy)
91  {
92  start[0] = start[1] = 1.; start[2] = 0.;
93  goal[0] = goal[1] = 17; goal[2] = -.99*boost::math::constants::pi<double>();
94  }
95  else
96  {
97  start[0] = start[1] = .5; start[2] = .5*boost::math::constants::pi<double>();;
98  goal[0] = 5.5; goal[1] = .5; goal[2] = .5*boost::math::constants::pi<double>();
99  }
100  ss.setStartAndGoalStates(start, goal);
101 
102  // this call is optional, but we put it in to get more output information
103  ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
104  ss.setup();
105  ss.print();
106 
107  // attempt to solve the problem within 30 seconds of planning time
108  ob::PlannerStatus solved = ss.solve(30.0);
109 
110  if (solved)
111  {
112  std::vector<double> reals;
113 
114  std::cout << "Found solution:" << std::endl;
115  ss.simplifySolution();
116  og::PathGeometric path = ss.getSolutionPath();
117  path.interpolate(1000);
118  path.printAsMatrix(std::cout);
119  }
120  else
121  std::cout << "No solution found" << std::endl;
122 }
123 
124 void printTrajectory(const ob::StateSpacePtr& space, const std::vector<double>& pt)
125 {
126  if (pt.size()!=3) throw ompl::Exception("3 arguments required for trajectory option");
127  const unsigned int num_pts = 50;
128  ob::ScopedState<> from(space), to(space), s(space);
129  std::vector<double> reals;
130 
131  from[0] = from[1] = from[2] = 0.;
132 
133  to[0] = pt[0];
134  to[1] = pt[1];
135  to[2] = pt[2];
136 
137  std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n";
138  for (unsigned int i=0; i<=num_pts; ++i)
139  {
140  space->interpolate(from(), to(), (double)i/num_pts, s());
141  reals = s.reals();
142  std::cout << "path " << reals[0] << ' ' << reals[1] << ' ' << reals[2] << ' ' << std::endl;
143  }
144 }
145 
146 void printDistanceGrid(const ob::StateSpacePtr& space)
147 {
148  // print the distance for (x,y,theta) for all points in a 3D grid in SE(2)
149  // over [-5,5) x [-5, 5) x [-pi,pi).
150  //
151  // The output should be redirected to a file, say, distance.txt. This
152  // can then be read and plotted in Matlab like so:
153  // x = reshape(load('distance.txt'),200,200,200);
154  // for i=1:200,
155  // contourf(squeeze(x(i,:,:)),30);
156  // axis equal; axis tight; colorbar; pause;
157  // end;
158  const unsigned int num_pts = 200;
159  ob::ScopedState<> from(space), to(space);
160  from[0] = from[1] = from[2] = 0.;
161 
162  for (unsigned int i=0; i<num_pts; ++i)
163  for (unsigned int j=0; j<num_pts; ++j)
164  for (unsigned int k=0; k<num_pts; ++k)
165  {
166  to[0] = 5. * (2. * (double)i/num_pts - 1.);
167  to[1] = 5. * (2. * (double)j/num_pts - 1.);
168  to[2] = boost::math::constants::pi<double>() * (2. * (double)k/num_pts - 1.);
169  std::cout << space->distance(from(), to()) << '\n';
170  }
171 
172 }
173 
174 int main(int argc, char* argv[])
175 {
176  try
177  {
178  po::options_description desc("Options");
179  desc.add_options()
180  ("help", "show help message")
181  ("dubins", "use Dubins state space")
182  ("dubinssym", "use symmetrized Dubins state space")
183  ("reedsshepp", "use Reeds-Shepp state space (default)")
184  ("easyplan", "solve easy planning problem and print path")
185  ("hardplan", "solve hard planning problem and print path")
186  ("trajectory", po::value<std::vector<double > >()->multitoken(),
187  "print trajectory from (0,0,0) to a user-specified x, y, and theta")
188  ("distance", "print distance grid")
189  ;
190 
191  po::variables_map vm;
192  po::store(po::parse_command_line(argc, argv, desc,
193  po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
194  po::notify(vm);
195 
196  if ((vm.count("help") != 0u) || argc==1)
197  {
198  std::cout << desc << "\n";
199  return 1;
200  }
201 
202  ob::StateSpacePtr space(std::make_shared<ob::ReedsSheppStateSpace>());
203 
204  if (vm.count("dubins") != 0u)
205  space = std::make_shared<ob::DubinsStateSpace>();
206  if (vm.count("dubinssym") != 0u)
207  space = std::make_shared<ob::DubinsStateSpace>(1., true);
208  if (vm.count("easyplan") != 0u)
209  plan(space, true);
210  if (vm.count("hardplan") != 0u)
211  plan(space, false);
212  if (vm.count("trajectory") != 0u)
213  printTrajectory(space, vm["trajectory"].as<std::vector<double> >());
214  if (vm.count("distance") != 0u)
215  printDistanceGrid(space);
216  }
217  catch(std::exception& e) {
218  std::cerr << "error: " << e.what() << "\n";
219  return 1;
220  }
221  catch(...) {
222  std::cerr << "Exception of unknown type!\n";
223  }
224 
225  return 0;
226 }
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...
Definition of a scoped state.
Definition: ScopedState.h:56
A shared pointer wrapper for ompl::base::StateSpace.
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
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:49
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
The exception type for ompl.
Definition: Exception.h:46
The lower and upper bounds for an Rn space.
Definition of a geometric path.
Definition: PathGeometric.h:60
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47