37 #include <ompl/base/spaces/DubinsStateSpace.h> 38 #include <ompl/base/spaces/ReedsSheppStateSpace.h> 39 #include <ompl/geometric/planners/rrt/RRTstar.h> 40 #include <ompl/geometric/planners/cforest/CForest.h> 41 #include <ompl/base/objectives/PathLengthOptimizationObjective.h> 42 #include <ompl/geometric/SimpleSetup.h> 43 #include <ompl/tools/benchmark/Benchmark.h> 44 #include <boost/program_options.hpp> 49 namespace po = boost::program_options;
51 bool isStateValid(
double radiusSquared,
const ob::State *state)
54 double x=s->getX(), y=s->getY();
55 x = std::abs(x - std::floor(x));
56 y = std::abs(y - std::floor(y));
57 x = std::min(x, 1. - x);
58 y = std::min(y, 1. - y);
59 return x*x + y*y > radiusSquared;
62 int main(
int argc,
char **argv)
64 int distance, gridLimit, runCount;
65 double obstacleRadius, turningRadius, runtimeLimit;
67 auto space(std::make_shared<ob::SE2StateSpace>());
69 po::options_description desc(
"Options");
72 (
"help",
"show help message")
73 (
"dubins",
"use Dubins state space")
74 (
"dubinssym",
"use symmetrized Dubins state space")
75 (
"reedsshepp",
"use Reeds-Shepp state space")
76 (
"distance", po::value<int>(&distance)->default_value(3),
"integer grid distance between start and goal")
77 (
"obstacle-radius", po::value<double>(&obstacleRadius)->default_value(.25),
"radius of obstacles")
78 (
"turning-radius", po::value<double>(&turningRadius)->default_value(.5),
"turning radius of robot (ignored for default point robot)")
79 (
"grid-limit", po::value<int>(&gridLimit)->default_value(10),
"size of the grid")
80 (
"runtime-limit", po::value<double>(&runtimeLimit)->default_value(2),
"time limit for every test")
81 (
"run-count", po::value<int>(&runCount)->default_value(100),
"number of times to run each planner")
85 po::store(po::parse_command_line(argc, argv, desc), vm);
88 if (vm.count(
"help") != 0u)
90 std::cout << desc <<
"\n";
94 if (vm.count(
"dubins") != 0u)
95 space = std::make_shared<ob::DubinsStateSpace>(turningRadius);
96 if (vm.count(
"dubinssym") != 0u)
97 space = std::make_shared<ob::DubinsStateSpace>(turningRadius,
true);
98 if (vm.count(
"reedsshepp") != 0u)
99 space = std::make_shared<ob::ReedsSheppStateSpace>(turningRadius);
103 bounds.setLow(-.5 * gridLimit);
104 bounds.setHigh(.5 * gridLimit);
105 space->setBounds(bounds);
111 double radiusSquared = obstacleRadius * obstacleRadius;
112 ss.setStateValidityChecker(
115 return isStateValid(radiusSquared, state);
120 start->setXY(0., 0.5);
122 goal->setXY(0., (
double)distance + .5);
124 ss.setStartAndGoalStates(start, goal);
127 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.05 / gridLimit);
128 ss.getProblemDefinition()->setOptimizationObjective(
129 std::make_shared<ompl::base::PathLengthOptimizationObjective>(ss.getSpaceInformation()));
132 double memoryLimit = 4096;
133 ot::Benchmark::Request request(runtimeLimit, memoryLimit, runCount);
136 b.addPlanner(std::make_shared<og::RRTstar>(ss.getSpaceInformation()));
137 b.addPlanner(std::make_shared<og::CForest>(ss.getSpaceInformation()));
138 b.benchmark(request);
139 b.saveResultsToFile(
"circleGrid.log");
Definition of a scoped state.
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.
Definition of an abstract state.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The lower and upper bounds for an Rn space.
This namespace contains code that is specific to planning under geometric constraints.