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();
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...
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.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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.
const T * as() const
Cast this instance to a desired type.
This namespace contains code that is specific to planning under geometric constraints.