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;
112 ss.simplifySolution();
118 std::cout <<
"No solution found" << std::endl;
123 if (pt.size()!=3)
throw ompl::Exception(
"3 arguments required for trajectory option");
124 const unsigned int num_pts = 50;
126 std::vector<double> reals;
128 from[0] = from[1] = from[2] = 0.;
134 std::cout <<
"distance: " << space->distance(from(), to()) <<
"\npath:\n";
135 for (
unsigned int i=0; i<=num_pts; ++i)
137 space->interpolate(from(), to(), (
double)i/num_pts, s());
139 std::cout <<
"path " << reals[0] <<
' ' << reals[1] <<
' ' << reals[2] <<
' ' << std::endl;
155 const unsigned int num_pts = 200;
157 from[0] = from[1] = from[2] = 0.;
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)
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';
171 int main(
int argc,
char* argv[])
175 po::options_description desc(
"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")
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);
193 if (vm.count(
"help") || argc==1)
195 std::cout << desc <<
"\n";
201 if (vm.count(
"dubins"))
203 if (vm.count(
"dubinssym"))
205 if (vm.count(
"easyplan"))
207 if (vm.count(
"hardplan"))
209 if (vm.count(
"trajectory"))
210 printTrajectory(space, vm[
"trajectory"].as<std::vector<double> >());
211 if (vm.count(
"distance"))
212 printDistanceGrid(space);
214 catch(std::exception& e) {
215 std::cerr <<
"error: " << e.what() <<
"\n";
219 std::cerr <<
"Exception of unknown type!\n";
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
An SE(2) state space where distance is measured by the length of Dubins curves.
CompoundState StateType
Define the type of state allocated by this state space.
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...
const T * as(void) const
Cast this instance to a desired type.
Create the set of classes typically needed to solve a geometric problem.
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)
A class to store the exit status of Planner::solve()
Definition of an abstract state.
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
The exception type for ompl.
The lower and upper bounds for an Rn space.
Definition of a geometric path.