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> 45 namespace po = boost::program_options;
55 double x=s->getX(), y=s->getY();
56 return si->satisfiesBounds(s) && (x<5 || x>13 || (y>8.5 && y<9.5));
61 return si->satisfiesBounds(state);
83 auto isStateValid = easy ? isStateValidEasy : isStateValidHard;
84 ss.setStateValidityChecker([isStateValid, si](
const ob::State *state)
86 return isStateValid(si, state);
92 start[0] = start[1] = 1.; start[2] = 0.;
93 goal[0] = goal[1] = 17; goal[2] = -.99*boost::math::constants::pi<double>();
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>();
100 ss.setStartAndGoalStates(start, goal);
103 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
112 std::vector<double> reals;
114 std::cout <<
"Found solution:" << std::endl;
115 ss.simplifySolution();
117 path.interpolate(1000);
118 path.printAsMatrix(std::cout);
121 std::cout <<
"No solution found" << std::endl;
124 void printTrajectory(
const ob::StateSpacePtr& space,
const std::vector<double>& pt)
126 if (pt.size()!=3)
throw ompl::Exception(
"3 arguments required for trajectory option");
127 const unsigned int num_pts = 50;
129 std::vector<double> reals;
131 from[0] = from[1] = from[2] = 0.;
137 std::cout <<
"distance: " << space->distance(from(), to()) <<
"\npath:\n";
138 for (
unsigned int i=0; i<=num_pts; ++i)
140 space->interpolate(from(), to(), (
double)i/num_pts, s());
142 std::cout <<
"path " << reals[0] <<
' ' << reals[1] <<
' ' << reals[2] <<
' ' << std::endl;
158 const unsigned int num_pts = 200;
160 from[0] = from[1] = from[2] = 0.;
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)
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';
174 int main(
int argc,
char* argv[])
178 po::options_description desc(
"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")
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);
196 if ((vm.count(
"help") != 0u) || argc==1)
198 std::cout << desc <<
"\n";
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)
210 if (vm.count(
"hardplan") != 0u)
212 if (vm.count(
"trajectory") != 0u)
213 printTrajectory(space, vm[
"trajectory"].as<std::vector<double> >());
214 if (vm.count(
"distance") != 0u)
215 printDistanceGrid(space);
217 catch(std::exception& e) {
218 std::cerr <<
"error: " << e.what() <<
"\n";
222 std::cerr <<
"Exception of unknown type!\n";
Definition of a scoped state.
A shared pointer wrapper for ompl::base::StateSpace.
Create the set of classes typically needed to solve a geometric problem.
const T * as() const
Cast this instance to a desired type.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
A state space representing SE(2)
A class to store the exit status of Planner::solve()
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The exception type for ompl.
The lower and upper bounds for an Rn space.
Definition of a geometric path.
This namespace contains code that is specific to planning under geometric constraints.