CForestCircleGridBenchmark.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, Rice University
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Rice University nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Javier V. Gomez, Mark Moll */
 
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/cforest/CForest.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/tools/benchmark/Benchmark.h>
#include <boost/program_options.hpp>
 
namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace ot = ompl::tools;
namespace po = boost::program_options;
 
bool isStateValid(double radiusSquared, const ob::State *state)
{
    const auto *s = state->as<ob::SE2StateSpace::StateType>();
    double x = s->getX(), y = s->getY();
    x = std::abs(x - std::floor(x));
    y = std::abs(y - std::floor(y));
    x = std::min(x, 1. - x);
    y = std::min(y, 1. - y);
    return x * x + y * y > radiusSquared;
}
 
int main(int argc, char **argv)
{
    int distance, gridLimit, runCount;
    double obstacleRadius, turningRadius, runtimeLimit;
 
    auto space(std::make_shared<ob::SE2StateSpace>());
 
    po::options_description desc("Options");
 
    desc.add_options()("help", "show help message")("dubins", "use Dubins state space")(
        "dubinssym", "use symmetrized Dubins state space")("reedsshepp", "use Reeds-Shepp state space")(
        "distance", po::value<int>(&distance)->default_value(3), "integer grid distance between start and goal")(
        "obstacle-radius", po::value<double>(&obstacleRadius)->default_value(.25),
        "radius of obstacles")("turning-radius", po::value<double>(&turningRadius)->default_value(.5),
                               "turning radius of robot (ignored for default point robot)")(
        "grid-limit", po::value<int>(&gridLimit)->default_value(10), "size of the grid")(
        "runtime-limit", po::value<double>(&runtimeLimit)->default_value(2), "time limit for every test")(
        "run-count", po::value<int>(&runCount)->default_value(100), "number of times to run each planner");
 
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
 
    if (vm.count("help") != 0u)
    {
        std::cout << desc << "\n";
        return 1;
    }
 
    if (vm.count("dubins") != 0u)
        space = std::make_shared<ob::DubinsStateSpace>(turningRadius);
    if (vm.count("dubinssym") != 0u)
        space = std::make_shared<ob::DubinsStateSpace>(turningRadius, true);
    if (vm.count("reedsshepp") != 0u)
        space = std::make_shared<ob::ReedsSheppStateSpace>(turningRadius);
 
    // set the bounds for the R^2 part of SE(2)
    ob::RealVectorBounds bounds(2);
    bounds.setLow(-.5 * gridLimit);
    bounds.setHigh(.5 * gridLimit);
    space->setBounds(bounds);
 
    // define a simple setup class
    og::SimpleSetup ss(space);
 
    // set state validity checking for this space
    double radiusSquared = obstacleRadius * obstacleRadius;
    ss.setStateValidityChecker([radiusSquared](const ob::State *state) { return isStateValid(radiusSquared, state); });
 
    // define start & goal states
    ob::ScopedState<ob::SE2StateSpace> start(space), goal(space);
    start->setXY(0., 0.5);
    start->setYaw(0.);
    goal->setXY(0., (double)distance + .5);
    goal->setYaw(0);
    ss.setStartAndGoalStates(start, goal);
 
    // setting collision checking resolution to 0.05 (absolute)
    ss.getSpaceInformation()->setStateValidityCheckingResolution(0.05 / gridLimit);
    ss.getProblemDefinition()->setOptimizationObjective(
        std::make_shared<ompl::base::PathLengthOptimizationObjective>(ss.getSpaceInformation()));
 
    // by default, use the Benchmark class
    double memoryLimit = 4096;
    ot::Benchmark::Request request(runtimeLimit, memoryLimit, runCount);
    ot::Benchmark b(ss, "CircleGrid");
 
    b.addPlanner(std::make_shared<og::RRTstar>(ss.getSpaceInformation()));
    b.addPlanner(std::make_shared<og::CForest>(ss.getSpaceInformation()));
    b.benchmark(request);
    b.saveResultsToFile("circleGrid.log");
 
    exit(0);
}