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/control/SimpleSetup.h"
00038 #include "ompl/control/planners/rrt/RRT.h"
00039 #include "ompl/control/planners/kpiece/KPIECE1.h"
00040 
00041 ompl::base::PlannerPtr ompl::control::getDefaultPlanner(const base::GoalPtr &goal)
00042 {
00043     base::PlannerPtr planner;
00044     if (!goal)
00045         throw Exception("Unable to allocate default planner for unspecified goal definition");
00046 
00047     SpaceInformationPtr si = boost::static_pointer_cast<SpaceInformation, base::SpaceInformation>(goal->getSpaceInformation());
00048     if (si->getStateSpace()->hasDefaultProjection())
00049         planner = base::PlannerPtr(new KPIECE1(si));
00050     else
00051         planner = base::PlannerPtr(new RRT(si));
00052 
00053     return planner;
00054 }
00055 
00056 void ompl::control::SimpleSetup::setup(void)
00057 {
00058     if (!configured_)
00059     {
00060         if (!si_)
00061             throw Exception("No space information defined");
00062 
00063         if (!si_->isSetup())
00064             si_->setup();
00065         if (!planner_)
00066         {
00067             if (pa_)
00068                 planner_ = pa_(si_);
00069             else
00070             {
00071                 msg_.inform("No planner specified. Using default.");
00072                 planner_ = getDefaultPlanner(getGoal());
00073             }
00074         }
00075         planner_->setProblemDefinition(pdef_);
00076         if (!planner_->isSetup())
00077             planner_->setup();
00078         configured_ = true;
00079     }
00080 }
00081 
00082 void ompl::control::SimpleSetup::clear(void)
00083 {
00084     if (planner_)
00085         planner_->clear();
00086     if (pdef_ && pdef_->getGoal())
00087         pdef_->getGoal()->clearSolutionPath();
00088 }
00089 
00090 bool ompl::control::SimpleSetup::solve(double time)
00091 {
00092     setup();
00093     time::point start = time::now();
00094     bool result = planner_->solve(time);
00095     planTime_ = time::seconds(time::now() - start);
00096     if (result)
00097         msg_.inform("Solution found in %f seconds", planTime_);
00098     else
00099         msg_.inform("No solution found after %f seconds", planTime_);
00100     return result;
00101 }
00102 
00103 ompl::control::PathControl& ompl::control::SimpleSetup::getSolutionPath(void) const
00104 {
00105     if (pdef_ && pdef_->getGoal())
00106     {
00107         const base::PathPtr &p = pdef_->getGoal()->getSolutionPath();
00108         if (p)
00109             return static_cast<PathControl&>(*p);
00110     }
00111     throw Exception("No solution path");
00112 }
00113 
00114 ompl::control::PlannerData ompl::control::SimpleSetup::getPlannerData(void) const
00115 {
00116     control::PlannerData pd;
00117     if (planner_)
00118         planner_->getPlannerData(pd);
00119     return pd;
00120 }
00121 
00122 void ompl::control::SimpleSetup::print(std::ostream &out) const
00123 {
00124     if (si_)
00125     {
00126         si_->printSettings(out);
00127         si_->printProperties(out);
00128     }
00129     if (pdef_)
00130         pdef_->print(out);
00131 }