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 
46 namespace ob = ompl::base;
47 namespace og = ompl::geometric;
48 namespace ot = ompl::tools;
49 namespace po = boost::program_options;
50 
51 bool 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 
62 int 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 
Definition of a scoped state.
Definition: ScopedState.h:56
Includes various tools such as self config, benchmarking, etc.
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
The lower and upper bounds for an Rn space.
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47