All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
SimpleSetup.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, 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: Ioan Sucan */
00036 
00037 #include "ompl/geometric/SimpleSetup.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/geometric/planners/rrt/RRTConnect.h"
00040 #include "ompl/geometric/planners/rrt/RRT.h"
00041 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
00042 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
00043 
00044 ompl::base::PlannerPtr ompl::geometric::getDefaultPlanner(const base::GoalPtr &goal)
00045 {
00046     base::PlannerPtr planner;
00047     if (!goal)
00048         throw Exception("Unable to allocate default planner for unspecified goal definition");
00049 
00050     // if we can sample the goal region, use a bi-directional planner
00051     if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
00052     {
00053         // if we have a default projection
00054         if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
00055             planner = base::PlannerPtr(new LBKPIECE1(goal->getSpaceInformation()));
00056         else
00057             planner = base::PlannerPtr(new RRTConnect(goal->getSpaceInformation()));
00058     }
00059     // other use a single-tree planner
00060     else
00061     {
00062         // if we have a default projection
00063         if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
00064             planner = base::PlannerPtr(new KPIECE1(goal->getSpaceInformation()));
00065         else
00066             planner = base::PlannerPtr(new RRT(goal->getSpaceInformation()));
00067     }
00068 
00069     if (!planner)
00070         throw Exception("Unable to allocate default planner");
00071 
00072     return planner;
00073 }
00074 
00075 void ompl::geometric::SimpleSetup::setup(void)
00076 {
00077     if (!configured_)
00078     {
00079         if (!si_)
00080             throw Exception("No space information defined");
00081 
00082         if (!si_->isSetup())
00083             si_->setup();
00084         if (!planner_)
00085         {
00086             if (pa_)
00087                 planner_ = pa_(si_);
00088             else
00089             {
00090                 msg_.inform("No planner specified. Using default.");
00091                 planner_ = getDefaultPlanner(getGoal());
00092             }
00093         }
00094         planner_->setProblemDefinition(pdef_);
00095         if (!planner_->isSetup())
00096             planner_->setup();
00097         configured_ = true;
00098     }
00099 }
00100 
00101 void ompl::geometric::SimpleSetup::clear(void)
00102 {
00103     if (planner_)
00104         planner_->clear();
00105     if (pdef_ && pdef_->getGoal())
00106         pdef_->getGoal()->clearSolutionPath();
00107 }
00108 
00109 bool ompl::geometric::SimpleSetup::solve(double time)
00110 {
00111     setup();
00112     time::point start = time::now();
00113     bool result = planner_->solve(time);
00114     planTime_ = time::seconds(time::now() - start);
00115     if (result)
00116         msg_.inform("Solution found in %f seconds", planTime_);
00117     else
00118         msg_.inform("No solution found after %f seconds", planTime_);
00119     return result;
00120 }
00121 
00122 void ompl::geometric::SimpleSetup::simplifySolution(void)
00123 {
00124     if (pdef_ && pdef_->getGoal())
00125     {
00126         const base::PathPtr &p = pdef_->getGoal()->getSolutionPath();
00127         if (p)
00128         {
00129             time::point start = time::now();
00130             psk_->simplifyMax(static_cast<PathGeometric&>(*p));
00131             simplifyTime_ = time::seconds(time::now() - start);
00132             msg_.inform("Path simplification took %f seconds", simplifyTime_);
00133         }
00134         else
00135             msg_.warn("No solution to simplify");
00136     }
00137     else
00138         msg_.warn("No solution to simplify");
00139 }
00140 
00141 ompl::geometric::PathGeometric& ompl::geometric::SimpleSetup::getSolutionPath(void) const
00142 {
00143     if (pdef_ && pdef_->getGoal())
00144     {
00145         const base::PathPtr &p = pdef_->getGoal()->getSolutionPath();
00146         if (p)
00147             return static_cast<PathGeometric&>(*p);
00148     }
00149     throw Exception("No solution path");
00150 }
00151 
00152 ompl::base::PlannerData ompl::geometric::SimpleSetup::getPlannerData(void) const
00153 {
00154     base::PlannerData pd;
00155     if (planner_)
00156         planner_->getPlannerData(pd);
00157     return pd;
00158 }
00159 
00160 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
00161 {
00162     if (si_)
00163     {
00164         si_->printSettings(out);
00165         si_->printProperties(out);
00166     }
00167     if (pdef_)
00168         pdef_->print(out);
00169 }