demos/CForestCircleGridBenchmark.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2014, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Javier V. Gomez, Mark Moll */ 00036 00037 #include <ompl/base/spaces/DubinsStateSpace.h> 00038 #include <ompl/base/spaces/ReedsSheppStateSpace.h> 00039 #include <ompl/geometric/planners/rrt/RRTstar.h> 00040 #include <ompl/geometric/planners/cforest/CForest.h> 00041 #include <ompl/base/objectives/PathLengthOptimizationObjective.h> 00042 #include <ompl/geometric/SimpleSetup.h> 00043 #include <ompl/tools/benchmark/Benchmark.h> 00044 #include <boost/program_options.hpp> 00045 00046 namespace ob = ompl::base; 00047 namespace og = ompl::geometric; 00048 namespace ot = ompl::tools; 00049 namespace po = boost::program_options; 00050 00051 ompl::base::OptimizationObjectivePtr getPathLengthObjective(const ompl::base::SpaceInformationPtr& si) 00052 { 00053 return ompl::base::OptimizationObjectivePtr(new ompl::base::PathLengthOptimizationObjective(si)); 00054 } 00055 00056 bool isStateValid(double radiusSquared, const ob::State *state) 00057 { 00058 const ob::SE2StateSpace::StateType *s = state->as<ob::SE2StateSpace::StateType>(); 00059 double x=s->getX(), y=s->getY(); 00060 x = std::abs(x - std::floor(x)); 00061 y = std::abs(y - std::floor(y)); 00062 x = std::min(x, 1. - x); 00063 y = std::min(y, 1. - y); 00064 return x*x + y*y > radiusSquared; 00065 } 00066 00067 int main(int argc, char **argv) 00068 { 00069 int distance, gridLimit, runCount; 00070 double obstacleRadius, turningRadius, runtimeLimit; 00071 00072 ob::StateSpacePtr space(new ob::SE2StateSpace()); 00073 00074 po::options_description desc("Options"); 00075 00076 desc.add_options() 00077 ("help", "show help message") 00078 ("dubins", "use Dubins state space") 00079 ("dubinssym", "use symmetrized Dubins state space") 00080 ("reedsshepp", "use Reeds-Shepp state space") 00081 ("distance", po::value<int>(&distance)->default_value(3), "integer grid distance between start and goal") 00082 ("obstacle-radius", po::value<double>(&obstacleRadius)->default_value(.25), "radius of obstacles") 00083 ("turning-radius", po::value<double>(&turningRadius)->default_value(.5), "turning radius of robot (ignored for default point robot)") 00084 ("grid-limit", po::value<int>(&gridLimit)->default_value(10), "size of the grid") 00085 ("runtime-limit", po::value<double>(&runtimeLimit)->default_value(2), "time limit for every test") 00086 ("run-count", po::value<int>(&runCount)->default_value(100), "number of times to run each planner") 00087 ; 00088 00089 po::variables_map vm; 00090 po::store(po::parse_command_line(argc, argv, desc), vm); 00091 po::notify(vm); 00092 00093 if (vm.count("help")) 00094 { 00095 std::cout << desc << "\n"; 00096 return 1; 00097 } 00098 00099 if (vm.count("dubins")) 00100 space = ob::StateSpacePtr(new ob::DubinsStateSpace(turningRadius)); 00101 if (vm.count("dubinssym")) 00102 space = ob::StateSpacePtr(new ob::DubinsStateSpace(turningRadius, true)); 00103 if (vm.count("reedsshepp")) 00104 space = ob::StateSpacePtr(new ob::ReedsSheppStateSpace(turningRadius)); 00105 00106 // set the bounds for the R^2 part of SE(2) 00107 ob::RealVectorBounds bounds(2); 00108 bounds.setLow(-.5 * gridLimit); 00109 bounds.setHigh(.5 * gridLimit); 00110 space->as<ob::SE2StateSpace>()->setBounds(bounds); 00111 00112 // define a simple setup class 00113 og::SimpleSetup ss(space); 00114 00115 // set state validity checking for this space 00116 ss.setStateValidityChecker(boost::bind(&isStateValid, obstacleRadius*obstacleRadius, _1)); 00117 00118 // define start & goal states 00119 ob::ScopedState<ob::SE2StateSpace> start(space), goal(space); 00120 start->setXY(0., 0.5); 00121 start->setYaw(0.); 00122 goal->setXY(0., (double)distance + .5); 00123 goal->setYaw(0); 00124 ss.setStartAndGoalStates(start, goal); 00125 00126 // setting collision checking resolution to 0.05 (absolute) 00127 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.05 / gridLimit); 00128 ss.getProblemDefinition()->setOptimizationObjective(getPathLengthObjective(ss.getSpaceInformation())); 00129 00130 // by default, use the Benchmark class 00131 double memoryLimit = 4096; 00132 ot::Benchmark::Request request(runtimeLimit, memoryLimit, runCount); 00133 ot::Benchmark b(ss, "CircleGrid"); 00134 00135 b.addPlanner(ob::PlannerPtr(new og::RRTstar(ss.getSpaceInformation()))); 00136 b.addPlanner(ob::PlannerPtr(new og::CForest(ss.getSpaceInformation()))); 00137 b.benchmark(request); 00138 b.saveResultsToFile("circleGrid.log"); 00139 00140 exit(0); 00141 } 00142 00143