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/SpaceInformation.h" 00042 #include "ompl/base/ProblemDefinition.h" 00043 #include "ompl/geometric/PathGeometric.h" 00044 #include "ompl/geometric/PathSimplifier.h" 00045 #include "ompl/util/Console.h" 00046 #include "ompl/util/Exception.h" 00047 00048 namespace ompl 00049 { 00050 00051 namespace geometric 00052 { 00053 00055 ClassForward(SimpleSetup); 00057 00063 class SimpleSetup 00064 { 00065 public: 00066 00068 explicit 00069 SimpleSetup(const base::StateSpacePtr &space); 00070 00071 virtual ~SimpleSetup(void) 00072 { 00073 } 00074 00076 const base::SpaceInformationPtr& getSpaceInformation(void) const 00077 { 00078 return si_; 00079 } 00080 00082 const base::ProblemDefinitionPtr& getProblemDefinition(void) const 00083 { 00084 return pdef_; 00085 } 00086 00088 const base::StateSpacePtr& getStateSpace(void) const 00089 { 00090 return si_->getStateSpace(); 00091 } 00092 00094 const base::StateValidityCheckerPtr& getStateValidityChecker(void) const 00095 { 00096 return si_->getStateValidityChecker(); 00097 } 00098 00100 const base::GoalPtr& getGoal(void) const 00101 { 00102 return pdef_->getGoal(); 00103 } 00104 00106 const base::PlannerPtr& getPlanner(void) const 00107 { 00108 return planner_; 00109 } 00110 00112 const base::PlannerAllocator& getPlannerAllocator(void) const 00113 { 00114 return pa_; 00115 } 00116 00118 const PathSimplifierPtr& getPathSimplifier(void) const 00119 { 00120 return psk_; 00121 } 00122 00124 PathSimplifierPtr& getPathSimplifier(void) 00125 { 00126 return psk_; 00127 } 00128 00130 bool haveExactSolutionPath(void) const; 00131 00133 bool haveSolutionPath(void) const 00134 { 00135 return getGoal()->getSolutionPath(); 00136 } 00137 00139 PathGeometric& getSolutionPath(void) const; 00140 00142 base::PlannerData getPlannerData(void) const; 00143 00145 void setStateValidityChecker(const base::StateValidityCheckerPtr &svc) 00146 { 00147 si_->setStateValidityChecker(svc); 00148 } 00149 00151 void setStateValidityChecker(const base::StateValidityCheckerFn &svc) 00152 { 00153 si_->setStateValidityChecker(svc); 00154 } 00155 00157 void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, 00158 const double threshold = std::numeric_limits<double>::epsilon()) 00159 { 00160 pdef_->setStartAndGoalStates(start, goal, threshold); 00161 } 00162 00165 void addStartState(const base::ScopedState<> &state) 00166 { 00167 pdef_->addStartState(state); 00168 } 00169 00171 void clearStartStates(void) 00172 { 00173 pdef_->clearStartStates(); 00174 } 00175 00177 void setStartState(const base::ScopedState<> &state) 00178 { 00179 clearStartStates(); 00180 addStartState(state); 00181 } 00182 00184 void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon()) 00185 { 00186 pdef_->setGoalState(goal, threshold); 00187 } 00188 00191 void setGoal(const base::GoalPtr &goal) 00192 { 00193 pdef_->setGoal(goal); 00194 } 00195 00200 void setPlanner(const base::PlannerPtr &planner) 00201 { 00202 if (planner && planner->getSpaceInformation().get() != si_.get()) 00203 throw Exception("Planner instance does not match space information"); 00204 planner_ = planner; 00205 configured_ = false; 00206 } 00207 00211 void setPlannerAllocator(const base::PlannerAllocator &pa) 00212 { 00213 pa_ = pa; 00214 planner_.reset(); 00215 configured_ = false; 00216 } 00217 00219 virtual bool solve(double time = 1.0); 00220 00222 virtual bool solve(const base::PlannerTerminationCondition &ptc); 00223 00225 bool invalidLastRequest(void) const 00226 { 00227 return invalid_request_; 00228 } 00229 00231 double getLastPlanComputationTime(void) const 00232 { 00233 return planTime_; 00234 } 00235 00237 double getLastSimplificationTime(void) const 00238 { 00239 return simplifyTime_; 00240 } 00241 00244 void simplifySolution(double duration = 0.0); 00245 00247 void simplifySolution(const base::PlannerTerminationCondition &ptc); 00248 00252 virtual void clear(void); 00253 00255 virtual void print(std::ostream &out = std::cout) const; 00256 00260 virtual void setup(void); 00261 00263 base::ParamSet& params(void) 00264 { 00265 return params_; 00266 } 00267 00269 const base::ParamSet& params(void) const 00270 { 00271 return params_; 00272 } 00273 00274 protected: 00275 00277 base::SpaceInformationPtr si_; 00278 00280 base::ProblemDefinitionPtr pdef_; 00281 00283 base::PlannerPtr planner_; 00284 00286 base::PlannerAllocator pa_; 00287 00289 PathSimplifierPtr psk_; 00290 00292 bool configured_; 00293 00295 double planTime_; 00296 00298 double simplifyTime_; 00299 00301 bool invalid_request_; 00302 00304 base::ParamSet params_; 00305 00307 msg::Interface msg_; 00308 00309 }; 00310 00312 base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal); 00313 } 00314 00315 } 00316 #endif