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