CForestCircleGridBenchmark.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2014, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Javier V. Gomez, Mark Moll */
36
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>
45
46namespace ob = ompl::base;
47namespace og = ompl::geometric;
48namespace ot = ompl::tools;
49namespace po = boost::program_options;
50
51bool isStateValid(double radiusSquared, const ob::State *state)
52{
53 const auto *s = state->as<ob::SE2StateSpace::StateType>();
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;
60}
61
62int main(int argc, char **argv)
63{
64 int distance, gridLimit, runCount;
65 double obstacleRadius, turningRadius, runtimeLimit;
66
67 auto space(std::make_shared<ob::SE2StateSpace>());
68
69 po::options_description desc("Options");
70
71 desc.add_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")
82 ;
83
84 po::variables_map vm;
85 po::store(po::parse_command_line(argc, argv, desc), vm);
86 po::notify(vm);
87
88 if (vm.count("help") != 0u)
89 {
90 std::cout << desc << "\n";
91 return 1;
92 }
93
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);
100
101 // set the bounds for the R^2 part of SE(2)
102 ob::RealVectorBounds bounds(2);
103 bounds.setLow(-.5 * gridLimit);
104 bounds.setHigh(.5 * gridLimit);
105 space->setBounds(bounds);
106
107 // define a simple setup class
108 og::SimpleSetup ss(space);
109
110 // set state validity checking for this space
111 double radiusSquared = obstacleRadius * obstacleRadius;
112 ss.setStateValidityChecker(
113 [radiusSquared](const ob::State *state)
114 {
115 return isStateValid(radiusSquared, state);
116 });
117
118 // define start & goal states
119 ob::ScopedState<ob::SE2StateSpace> start(space), goal(space);
120 start->setXY(0., 0.5);
121 start->setYaw(0.);
122 goal->setXY(0., (double)distance + .5);
123 goal->setYaw(0);
124 ss.setStartAndGoalStates(start, goal);
125
126 // setting collision checking resolution to 0.05 (absolute)
127 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.05 / gridLimit);
128 ss.getProblemDefinition()->setOptimizationObjective(
129 std::make_shared<ompl::base::PathLengthOptimizationObjective>(ss.getSpaceInformation()));
130
131 // by default, use the Benchmark class
132 double memoryLimit = 4096;
133 ot::Benchmark::Request request(runtimeLimit, memoryLimit, runCount);
134 ot::Benchmark b(ss, "CircleGrid");
135
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");
140
141 exit(0);
142}
143
144
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition: ScopedState.h:57
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48
Includes various tools such as self config, benchmarking, etc.