All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/base/ProblemDefinition.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_BASE_PROBLEM_DEFINITION_
00038 #define OMPL_BASE_PROBLEM_DEFINITION_
00039 
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/Goal.h"
00042 #include "ompl/base/Path.h"
00043 #include "ompl/base/SpaceInformation.h"
00044 #include "ompl/base/SolutionNonExistenceProof.h"
00045 #include "ompl/util/Console.h"
00046 #include "ompl/util/ClassForward.h"
00047 #include "ompl/base/ScopedState.h"
00048 
00049 #include <vector>
00050 #include <cstdlib>
00051 #include <iostream>
00052 #include <limits>
00053 
00054 #include <boost/noncopyable.hpp>
00055 
00056 namespace ompl
00057 {
00058     namespace base
00059     {
00060 
00062 
00063         ClassForward(ProblemDefinition);
00065 
00070         struct PlannerSolution
00071         {
00073             PlannerSolution(const PathPtr &path, bool approximate = false, double difference = -1.0) :
00074                 index_(-1), path_(path), length_(path->length()), approximate_(approximate), difference_(difference)
00075             {
00076             }
00077 
00079             bool operator==(const PlannerSolution& p) const
00080             {
00081                 return path_ == p.path_;
00082             }
00083 
00085             bool operator<(const PlannerSolution &b) const
00086             {
00087                 if (!approximate_ && b.approximate_)
00088                     return true;
00089                 if (approximate_ && !b.approximate_)
00090                     return false;
00091                 if (approximate_ && b.approximate_)
00092                     return difference_ < b.difference_;
00093                 return length_ < b.length_;
00094             }
00095 
00097             int     index_;
00098 
00100             PathPtr path_;
00101 
00103             double  length_;
00104 
00106             bool    approximate_;
00107 
00109             double  difference_;
00110         };
00111 
00115         class ProblemDefinition : private boost::noncopyable
00116         {
00117         public:
00118 
00120             ProblemDefinition(const SpaceInformationPtr &si);
00121 
00122             virtual ~ProblemDefinition(void)
00123             {
00124                 clearStartStates();
00125             }
00126 
00128             const SpaceInformationPtr& getSpaceInformation(void) const
00129             {
00130                 return si_;
00131             }
00132 
00134             void addStartState(const State *state)
00135             {
00136                 startStates_.push_back(si_->cloneState(state));
00137             }
00138 
00140             void addStartState(const ScopedState<> &state)
00141             {
00142                 startStates_.push_back(si_->cloneState(state.get()));
00143             }
00144 
00148             bool hasStartState(const State *state, unsigned int *startIndex = NULL);
00149 
00151             void clearStartStates(void)
00152             {
00153                 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00154                     si_->freeState(startStates_[i]);
00155                 startStates_.clear();
00156             }
00157 
00159             unsigned int getStartStateCount(void) const
00160             {
00161                 return startStates_.size();
00162             }
00163 
00165             const State* getStartState(unsigned int index) const
00166             {
00167                 return startStates_[index];
00168             }
00169 
00171             State* getStartState(unsigned int index)
00172             {
00173                 return startStates_[index];
00174             }
00175 
00177             void setGoal(const GoalPtr &goal)
00178             {
00179                 goal_ = goal;
00180             }
00181 
00183             void clearGoal(void)
00184             {
00185                 goal_.reset();
00186             }
00187 
00189             const GoalPtr& getGoal(void) const
00190             {
00191                 return goal_;
00192             }
00193 
00198             void getInputStates(std::vector<const State*> &states) const;
00199 
00207             void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00208 
00210             void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00211 
00213             void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00214             {
00215                 setStartAndGoalStates(start.get(), goal.get(), threshold);
00216             }
00217 
00219             void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00220             {
00221                 setGoalState(goal.get(), threshold);
00222             }
00223 
00229             bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
00230 
00243             PathPtr isStraightLinePathValid(void) const;
00244 
00249             bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
00250 
00252             bool hasSolution(void) const;
00253 
00257             bool hasApproximateSolution(void) const;
00258 
00260             double getSolutionDifference(void) const;
00261 
00266             PathPtr getSolutionPath(void) const;
00267 
00272             void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0) const;
00273 
00275             std::size_t getSolutionCount(void) const;
00276 
00278             std::vector<PlannerSolution> getSolutions(void) const;
00279 
00281             void clearSolutionPaths(void) const;
00282 
00284             void print(std::ostream &out = std::cout) const;
00285 
00287             bool hasSolutionNonExistenceProof(void) const;
00288 
00290             void clearSolutionNonExistenceProof(void);
00291 
00293             const SolutionNonExistenceProofPtr& getSolutionNonExistenceProof(void) const;
00294 
00296             void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr& nonExistenceProof);
00297 
00298         protected:
00299 
00301             bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
00302 
00304             SpaceInformationPtr          si_;
00305 
00307             std::vector<State*>          startStates_;
00308 
00310             GoalPtr                      goal_;
00311 
00313             SolutionNonExistenceProofPtr nonExistenceProof_;
00314 
00315         private:
00316 
00318             ClassForward(PlannerSolutionSet);
00320 
00322             PlannerSolutionSetPtr        solutions_;
00323         };
00324     }
00325 }
00326 
00327 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines