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>
43 namespace ob = ompl::base;
44 namespace og = ompl::geometric;
45 namespace po = boost::program_options;
55 double x=s->getX(), y=s->getY();
84 easy ? &isStateValidEasy : &isStateValidHard, si.get(), _1));
89 start[0] = start[1] = 1.; start[2] = 0.;
90 goal[0] = goal[1] = 17; goal[2] = -.99*boost::math::constants::pi<double>();
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>();
97 ss.setStartAndGoalStates(start, goal);
100 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
109 std::vector<double> reals;
111 std::cout <<
"Found solution:" << std::endl;
117 ps->reduceVertices(path);
118 ps->collapseCloseVertices(path);
123 std::cout <<
"path " << reals[0] <<
' '<< reals[1] <<
' ' << reals[2] << std::endl;
127 std::cout <<
"No solution found" << std::endl;
132 if (pt.size()!=3)
throw ompl::Exception(
"3 arguments required for trajectory option");
133 const unsigned int num_pts = 50;
135 std::vector<double> reals;
137 from[0] = from[1] = from[2] = 0.;
143 std::cout <<
"distance: " << space->distance(from(), to()) <<
"\npath:\n";
144 for (
unsigned int i=0; i<=num_pts; ++i)
146 space->interpolate(from(), to(), (
double)i/num_pts, s());
148 std::cout <<
"path " << reals[0] <<
' ' << reals[1] <<
' ' << reals[2] <<
' ' << std::endl;
164 const unsigned int num_pts = 200;
166 from[0] = from[1] = from[2] = 0.;
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)
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';
180 int main(
int argc,
char* argv[])
184 po::options_description desc(
"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")
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);
202 if (vm.count(
"help") || argc==1)
204 std::cout << desc <<
"\n";
210 if (vm.count(
"dubins"))
212 if (vm.count(
"dubinssym"))
214 if (vm.count(
"easyplan"))
216 if (vm.count(
"hardplan"))
218 if (vm.count(
"trajectory"))
219 printTrajectory(space, vm[
"trajectory"].as<std::vector<double> >());
220 if (vm.count(
"distance"))
221 printDistanceGrid(space);
223 catch(std::exception& e) {
224 std::cerr <<
"error: " << e.what() <<
"\n";
228 std::cerr <<
"Exception of unknown type!\n";