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 00049 namespace ompl 00050 { 00051 00052 namespace geometric 00053 { 00054 00056 ClassForward(SimpleSetup); 00058 00064 class SimpleSetup 00065 { 00066 public: 00067 00069 explicit 00070 SimpleSetup(const base::StateSpacePtr &space); 00071 00072 virtual ~SimpleSetup(void) 00073 { 00074 } 00075 00077 const base::SpaceInformationPtr& getSpaceInformation(void) const 00078 { 00079 return si_; 00080 } 00081 00083 const base::ProblemDefinitionPtr& getProblemDefinition(void) const 00084 { 00085 return pdef_; 00086 } 00087 00089 const base::StateSpacePtr& getStateSpace(void) const 00090 { 00091 return si_->getStateSpace(); 00092 } 00093 00095 const base::StateValidityCheckerPtr& getStateValidityChecker(void) const 00096 { 00097 return si_->getStateValidityChecker(); 00098 } 00099 00101 const base::GoalPtr& getGoal(void) const 00102 { 00103 return pdef_->getGoal(); 00104 } 00105 00107 const base::PlannerPtr& getPlanner(void) const 00108 { 00109 return planner_; 00110 } 00111 00113 const base::PlannerAllocator& getPlannerAllocator(void) const 00114 { 00115 return pa_; 00116 } 00117 00119 const PathSimplifierPtr& getPathSimplifier(void) const 00120 { 00121 return psk_; 00122 } 00123 00125 PathSimplifierPtr& getPathSimplifier(void) 00126 { 00127 return psk_; 00128 } 00129 00131 bool haveExactSolutionPath(void) const; 00132 00134 bool haveSolutionPath(void) const 00135 { 00136 return pdef_->getSolutionPath(); 00137 } 00138 00140 PathGeometric& getSolutionPath(void) const; 00141 00143 void getPlannerData(base::PlannerData &pd) const; 00144 00146 void setStateValidityChecker(const base::StateValidityCheckerPtr &svc) 00147 { 00148 si_->setStateValidityChecker(svc); 00149 } 00150 00152 void setStateValidityChecker(const base::StateValidityCheckerFn &svc) 00153 { 00154 si_->setStateValidityChecker(svc); 00155 } 00156 00158 void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, 00159 const double threshold = std::numeric_limits<double>::epsilon()) 00160 { 00161 pdef_->setStartAndGoalStates(start, goal, threshold); 00162 } 00163 00166 void addStartState(const base::ScopedState<> &state) 00167 { 00168 pdef_->addStartState(state); 00169 } 00170 00172 void clearStartStates(void) 00173 { 00174 pdef_->clearStartStates(); 00175 } 00176 00178 void setStartState(const base::ScopedState<> &state) 00179 { 00180 clearStartStates(); 00181 addStartState(state); 00182 } 00183 00185 void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon()) 00186 { 00187 pdef_->setGoalState(goal, threshold); 00188 } 00189 00192 void setGoal(const base::GoalPtr &goal) 00193 { 00194 pdef_->setGoal(goal); 00195 } 00196 00201 void setPlanner(const base::PlannerPtr &planner) 00202 { 00203 if (planner && planner->getSpaceInformation().get() != si_.get()) 00204 throw Exception("Planner instance does not match space information"); 00205 planner_ = planner; 00206 configured_ = false; 00207 } 00208 00212 void setPlannerAllocator(const base::PlannerAllocator &pa) 00213 { 00214 pa_ = pa; 00215 planner_.reset(); 00216 configured_ = false; 00217 } 00218 00220 virtual base::PlannerStatus solve(double time = 1.0); 00221 00223 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00224 00226 bool invalidLastRequest(void) const 00227 { 00228 return invalid_request_; 00229 } 00230 00232 double getLastPlanComputationTime(void) const 00233 { 00234 return planTime_; 00235 } 00236 00238 double getLastSimplificationTime(void) const 00239 { 00240 return simplifyTime_; 00241 } 00242 00245 void simplifySolution(double duration = 0.0); 00246 00248 void simplifySolution(const base::PlannerTerminationCondition &ptc); 00249 00253 virtual void clear(void); 00254 00256 virtual void print(std::ostream &out = std::cout) const; 00257 00261 virtual void setup(void); 00262 00264 base::ParamSet& params(void) 00265 { 00266 return params_; 00267 } 00268 00270 const base::ParamSet& params(void) const 00271 { 00272 return params_; 00273 } 00274 00275 protected: 00276 00278 base::SpaceInformationPtr si_; 00279 00281 base::ProblemDefinitionPtr pdef_; 00282 00284 base::PlannerPtr planner_; 00285 00287 base::PlannerAllocator pa_; 00288 00290 PathSimplifierPtr psk_; 00291 00293 bool configured_; 00294 00296 double planTime_; 00297 00299 double simplifyTime_; 00300 00302 bool invalid_request_; 00303 00305 base::ParamSet params_; 00306 }; 00307 00309 base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal); 00310 } 00311 00312 } 00313 #endif