demos/PlannerProgressProperties.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, 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: Luis G. Torres */ 00036 00037 #include <ompl/geometric/SimpleSetup.h> 00038 #include <ompl/tools/benchmark/Benchmark.h> 00039 #include <ompl/base/spaces/RealVectorStateSpace.h> 00040 #include <ompl/geometric/planners/rrt/RRTstar.h> 00041 00042 namespace ob = ompl::base; 00043 namespace og = ompl::geometric; 00044 00046 // 00047 // In this demo we define a simple 2D planning problem to get from 00048 // (0,0) to (1,1) without obstacles. We're interested in collecting 00049 // data over the execution of a planner using the Benchmark class. 00050 // 00051 // This program will output two files: a .console file, and a .log 00052 // file. You can generate plots in a file called "plot.pdf" from this 00053 // experiment using the ompl/scripts/ompl_benchmark_statistics.py 00054 // script and the .log file like so: 00055 // 00056 // python path/to/ompl/scripts/ompl_benchmark_statistics.py <your_log_filename>.log -p plot.pdf 00057 // 00058 // Toward the bottom of the generated plot.pdf file, you'll see plots 00059 // for how various properties change, on average, during planning 00060 // runs. 00061 int main(int argc, char** argv) 00062 { 00063 ob::StateSpacePtr space(new ob::RealVectorStateSpace(2)); 00064 space->as<ob::RealVectorStateSpace>()->setBounds(0, 1); 00065 og::SimpleSetup ss(space); 00066 00067 // Set our robot's starting state to be the bottom-left corner of 00068 // the environment, or (0,0). 00069 ob::ScopedState<> start(space); 00070 start->as<ob::RealVectorStateSpace::StateType>()->values[0] = 0.0; 00071 start->as<ob::RealVectorStateSpace::StateType>()->values[1] = 0.0; 00072 00073 // Set our robot's goal state to be the top-right corner of the 00074 // environment, or (1,1). 00075 ob::ScopedState<> goal(space); 00076 goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0; 00077 goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0; 00078 00079 // Goal tolerance is 0.05 00080 ss.setStartAndGoalStates(start, goal, 0.05); 00081 00082 // Create a new Benchmark object and set the RRTstar algorithm as 00083 // the planner. We're using RRTstar because currently, only the 00084 // RRTstar algorithm reports interesting planner progress 00085 // properties. 00086 ompl::tools::Benchmark b(ss, "my experiment"); 00087 og::RRTstar* rrt = new og::RRTstar(ss.getSpaceInformation()); 00088 rrt->setName("rrtstar"); 00089 00090 // We disable goal biasing so that the straight-line path doesn't 00091 // get found immediately. We want to demonstrate the steady 00092 // downward trend in path cost that characterizes RRTstar. 00093 rrt->setGoalBias(0.0); 00094 00095 b.addPlanner(ob::PlannerPtr(rrt)); 00096 00097 // Here we specify options for this benchmark. Maximum time spent 00098 // per planner execution is 2.0 seconds, maximum memory is 100MB, 00099 // number of planning runs is 50, planner progress properties will 00100 // be queried every 0.01 seconds, and a progress bar will be 00101 // displayed to show how the benchmarking is going. 00102 ompl::tools::Benchmark::Request req; 00103 req.maxTime = 2.0; 00104 req.maxMem = 100; 00105 req.runCount = 50; 00106 req.timeBetweenUpdates = 0.01; 00107 req.displayProgress = true; 00108 b.benchmark(req); 00109 00110 b.saveResultsToFile(); 00111 00112 return 0; 00113 00114 }