ompl/geometric/SimpleSetup.h
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 #ifndef OMPL_GEOMETRIC_SIMPLE_SETUP_
00038 #define OMPL_GEOMETRIC_SIMPLE_SETUP_
00039 
00040 #include "ompl/base/Planner.h"
00041 #include "ompl/base/PlannerData.h"
00042 #include "ompl/base/SpaceInformation.h"
00043 #include "ompl/base/ProblemDefinition.h"
00044 #include "ompl/geometric/PathGeometric.h"
00045 #include "ompl/geometric/PathSimplifier.h"
00046 #include "ompl/util/Console.h"
00047 #include "ompl/util/Exception.h"
00048 #include "ompl/util/Deprecation.h"
00049 
00050 namespace ompl
00051 {
00052 
00053     namespace geometric
00054     {
00055 
00057         OMPL_CLASS_FORWARD(SimpleSetup);
00059 
00065         class SimpleSetup
00066         {
00067         public:
00068 
00070             explicit
00071             SimpleSetup(const base::SpaceInformationPtr &si);
00072 
00074             explicit
00075             SimpleSetup(const base::StateSpacePtr &space);
00076 
00077             virtual ~SimpleSetup()
00078             {
00079             }
00080 
00082             const base::SpaceInformationPtr& getSpaceInformation() const
00083             {
00084                 return si_;
00085             }
00086 
00088             const base::ProblemDefinitionPtr& getProblemDefinition() const
00089             {
00090                 return pdef_;
00091             }
00092 
00094             const base::StateSpacePtr& getStateSpace() const
00095             {
00096                 return si_->getStateSpace();
00097             }
00098 
00100             const base::StateValidityCheckerPtr& getStateValidityChecker() const
00101             {
00102                 return si_->getStateValidityChecker();
00103             }
00104 
00106             const base::GoalPtr& getGoal() const
00107             {
00108                 return pdef_->getGoal();
00109             }
00110 
00112             const base::PlannerPtr& getPlanner() const
00113             {
00114                 return planner_;
00115             }
00116 
00118             const base::PlannerAllocator& getPlannerAllocator() const
00119             {
00120                 return pa_;
00121             }
00122 
00124             const PathSimplifierPtr& getPathSimplifier() const
00125             {
00126                 return psk_;
00127             }
00128 
00130             PathSimplifierPtr& getPathSimplifier()
00131             {
00132                 return psk_;
00133             }
00134 
00136             const base::OptimizationObjectivePtr& getOptimizationObjective() const
00137             {
00138                 return pdef_->getOptimizationObjective();
00139             }
00140 
00142             bool haveExactSolutionPath() const;
00143 
00145             bool haveSolutionPath() const
00146             {
00147                 return pdef_->getSolutionPath().get();
00148             }
00149 
00151             const std::string getSolutionPlannerName(void) const;
00152 
00154             PathGeometric& getSolutionPath() const;
00155 
00157             void getPlannerData(base::PlannerData &pd) const;
00158 
00160             void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
00161             {
00162                 si_->setStateValidityChecker(svc);
00163             }
00164 
00166             void setStateValidityChecker(const base::StateValidityCheckerFn &svc)
00167             {
00168                 si_->setStateValidityChecker(svc);
00169             }
00170 
00172             void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
00173             {
00174                 pdef_->setOptimizationObjective(optimizationObjective);
00175             }
00176 
00178             void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal,
00179                                        const double threshold = std::numeric_limits<double>::epsilon())
00180             {
00181                 pdef_->setStartAndGoalStates(start, goal, threshold);
00182 
00183                 // Clear any past solutions since they no longer correspond to our start and goal states
00184                 pdef_->clearSolutionPaths();
00185             }
00186 
00189             void addStartState(const base::ScopedState<> &state)
00190             {
00191                 pdef_->addStartState(state);
00192             }
00193 
00195             void clearStartStates()
00196             {
00197                 pdef_->clearStartStates();
00198             }
00199 
00201             void setStartState(const base::ScopedState<> &state)
00202             {
00203                 clearStartStates();
00204                 addStartState(state);
00205             }
00206 
00208             void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00209             {
00210                 pdef_->setGoalState(goal, threshold);
00211             }
00212 
00215             void setGoal(const base::GoalPtr &goal)
00216             {
00217                 pdef_->setGoal(goal);
00218             }
00219 
00224             void setPlanner(const base::PlannerPtr &planner)
00225             {
00226                 if (planner && planner->getSpaceInformation().get() != si_.get())
00227                     throw Exception("Planner instance does not match space information");
00228                 planner_ = planner;
00229                 configured_ = false;
00230             }
00231 
00235             void setPlannerAllocator(const base::PlannerAllocator &pa)
00236             {
00237                 pa_ = pa;
00238                 planner_.reset();
00239                 configured_ = false;
00240             }
00241 
00243             virtual base::PlannerStatus solve(double time = 1.0);
00244 
00246             virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00247 
00249             base::PlannerStatus getLastPlannerStatus() const
00250             {
00251                 return lastStatus_;
00252             }
00253 
00255             double getLastPlanComputationTime() const
00256             {
00257                 return planTime_;
00258             }
00259 
00261             double getLastSimplificationTime() const
00262             {
00263                 return simplifyTime_;
00264             }
00265 
00268             void simplifySolution(double duration = 0.0);
00269 
00271             void simplifySolution(const base::PlannerTerminationCondition &ptc);
00272 
00276             virtual void clear();
00277 
00279             virtual void print(std::ostream &out = std::cout) const;
00280 
00284             virtual void setup();
00285 
00286         protected:
00287 
00289             base::SpaceInformationPtr     si_;
00290 
00292             base::ProblemDefinitionPtr    pdef_;
00293 
00295             base::PlannerPtr              planner_;
00296 
00298             base::PlannerAllocator        pa_;
00299 
00301             PathSimplifierPtr             psk_;
00302 
00304             bool                          configured_;
00305 
00307             double                        planTime_;
00308 
00310             double                        simplifyTime_;
00311 
00313             base::PlannerStatus           lastStatus_;
00314         };
00315 
00318         OMPL_DEPRECATED base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal);
00319     }
00320 
00321 }
00322 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines