ompl/control/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_CONTROL_SIMPLE_SETUP_ 00038 #define OMPL_CONTROL_SIMPLE_SETUP_ 00039 00040 #include "ompl/base/Planner.h" 00041 #include "ompl/control/SpaceInformation.h" 00042 #include "ompl/control/PlannerData.h" 00043 #include "ompl/base/ProblemDefinition.h" 00044 #include "ompl/control/PathControl.h" 00045 #include "ompl/geometric/PathGeometric.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 control 00054 { 00056 OMPL_CLASS_FORWARD(SimpleSetup); 00058 00064 class SimpleSetup 00065 { 00066 public: 00067 00069 explicit 00070 SimpleSetup(const SpaceInformationPtr &si); 00071 00073 explicit 00074 SimpleSetup(const ControlSpacePtr &space); 00075 00076 virtual ~SimpleSetup() 00077 { 00078 } 00079 00081 const SpaceInformationPtr& getSpaceInformation() const 00082 { 00083 return si_; 00084 } 00085 00087 const base::ProblemDefinitionPtr& getProblemDefinition() const 00088 { 00089 return pdef_; 00090 } 00091 00093 const base::StateSpacePtr& getStateSpace() const 00094 { 00095 return si_->getStateSpace(); 00096 } 00097 00099 const ControlSpacePtr& getControlSpace() const 00100 { 00101 return si_->getControlSpace(); 00102 } 00103 00105 const base::StateValidityCheckerPtr& getStateValidityChecker() const 00106 { 00107 return si_->getStateValidityChecker(); 00108 } 00109 00111 const StatePropagatorPtr& getStatePropagator() const 00112 { 00113 return si_->getStatePropagator(); 00114 } 00115 00117 const base::GoalPtr& getGoal() const 00118 { 00119 return pdef_->getGoal(); 00120 } 00121 00123 const base::PlannerPtr& getPlanner() const 00124 { 00125 return planner_; 00126 } 00127 00129 const base::PlannerAllocator& getPlannerAllocator() const 00130 { 00131 return pa_; 00132 } 00133 00135 bool haveExactSolutionPath() const; 00136 00137 00139 bool haveSolutionPath() const 00140 { 00141 return pdef_->getSolutionPath().get(); 00142 } 00143 00145 PathControl& getSolutionPath() const; 00146 00148 void getPlannerData(base::PlannerData &pd) const; 00149 00151 void setStateValidityChecker(const base::StateValidityCheckerPtr &svc) 00152 { 00153 si_->setStateValidityChecker(svc); 00154 } 00155 00157 void setStateValidityChecker(const base::StateValidityCheckerFn &svc) 00158 { 00159 si_->setStateValidityChecker(svc); 00160 } 00161 00163 void setStatePropagator(const StatePropagatorFn &sp) 00164 { 00165 si_->setStatePropagator(sp); 00166 } 00167 00169 void setStatePropagator(const StatePropagatorPtr &sp) 00170 { 00171 si_->setStatePropagator(sp); 00172 } 00173 00175 void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective) 00176 { 00177 pdef_->setOptimizationObjective(optimizationObjective); 00178 } 00179 00181 void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon()) 00182 { 00183 pdef_->setStartAndGoalStates(start, goal, threshold); 00184 } 00185 00187 void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon()) 00188 { 00189 pdef_->setGoalState(goal, threshold); 00190 } 00191 00194 void addStartState(const base::ScopedState<> &state) 00195 { 00196 pdef_->addStartState(state); 00197 } 00198 00200 void clearStartStates() 00201 { 00202 pdef_->clearStartStates(); 00203 } 00204 00206 void setStartState(const base::ScopedState<> &state) 00207 { 00208 clearStartStates(); 00209 addStartState(state); 00210 } 00211 00214 void setGoal(const base::GoalPtr &goal) 00215 { 00216 pdef_->setGoal(goal); 00217 } 00218 00223 void setPlanner(const base::PlannerPtr &planner) 00224 { 00225 if (planner && planner->getSpaceInformation().get() != si_.get()) 00226 throw Exception("Planner instance does not match space information"); 00227 planner_ = planner; 00228 configured_ = false; 00229 } 00230 00234 void setPlannerAllocator(const base::PlannerAllocator &pa) 00235 { 00236 pa_ = pa; 00237 planner_.reset(); 00238 configured_ = false; 00239 } 00240 00242 virtual base::PlannerStatus solve(double time = 1.0); 00243 00245 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc); 00246 00248 base::PlannerStatus getLastPlannerStatus() const 00249 { 00250 return last_status_; 00251 } 00252 00254 double getLastPlanComputationTime() const 00255 { 00256 return planTime_; 00257 } 00258 00262 virtual void clear(); 00263 00265 virtual void print(std::ostream &out = std::cout) const; 00266 00270 virtual void setup(); 00271 00272 protected: 00273 00275 SpaceInformationPtr si_; 00276 00278 base::ProblemDefinitionPtr pdef_; 00279 00281 base::PlannerPtr planner_; 00282 00284 base::PlannerAllocator pa_; 00285 00287 bool configured_; 00288 00290 double planTime_; 00291 00293 base::PlannerStatus last_status_; 00294 }; 00295 00298 OMPL_DEPRECATED base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal); 00299 } 00300 00301 } 00302 #endif