All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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/SpaceInformation.h"
00043 #include "ompl/util/Console.h"
00044 #include "ompl/util/ClassForward.h"
00045 #include "ompl/base/ScopedState.h"
00046 
00047 #include <vector>
00048 #include <cstdlib>
00049 #include <iostream>
00050 #include <limits>
00051 
00052 #include <boost/noncopyable.hpp>
00053 
00054 namespace ompl
00055 {
00056     namespace base
00057     {
00058 
00060         ClassForward(ProblemDefinition);
00061 
00067         class ProblemDefinition : private boost::noncopyable
00068         {
00069         public:
00070 
00072             ProblemDefinition(const SpaceInformationPtr &si) : si_(si)
00073             {
00074             }
00075 
00076             virtual ~ProblemDefinition(void)
00077             {
00078                 clearStartStates();
00079             }
00080 
00082             void addStartState(const State *state)
00083             {
00084                 startStates_.push_back(si_->cloneState(state));
00085             }
00086 
00088             void addStartState(const ScopedState<> &state)
00089             {
00090                 startStates_.push_back(si_->cloneState(state.get()));
00091             }
00092 
00096             bool hasStartState(const State *state, unsigned int *startIndex = NULL);
00097 
00099             void clearStartStates(void)
00100             {
00101                 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00102                     si_->freeState(startStates_[i]);
00103                 startStates_.clear();
00104             }
00105 
00107             unsigned int getStartStateCount(void) const
00108             {
00109                 return startStates_.size();
00110             }
00111 
00113             const State* getStartState(unsigned int index) const
00114             {
00115                 return startStates_[index];
00116             }
00117 
00119             State* getStartState(unsigned int index)
00120             {
00121                 return startStates_[index];
00122             }
00123 
00125             void setGoal(const GoalPtr &goal)
00126             {
00127                 goal_ = goal;
00128             }
00129 
00131             void clearGoal(void)
00132             {
00133                 goal_.reset();
00134             }
00135 
00137             const GoalPtr& getGoal(void) const
00138             {
00139                 return goal_;
00140             }
00141 
00146             void getInputStates(std::vector<const State*> &states) const;
00147 
00155             void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00156 
00158             void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00159 
00161             void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00162             {
00163                 setStartAndGoalStates(start.get(), goal.get(), threshold);
00164             }
00165 
00167             void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00168             {
00169                 setGoalState(goal.get(), threshold);
00170             }
00171 
00177             bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
00178 
00191             PathPtr isStraightLinePathValid(void) const;
00192 
00197             bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
00198 
00200             void print(std::ostream &out = std::cout) const;
00201 
00202         protected:
00203 
00205             bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
00206 
00208             SpaceInformationPtr  si_;
00209 
00211             std::vector<State*>  startStates_;
00212 
00214             GoalPtr              goal_;
00215 
00217             msg::Interface       msg_;
00218         };
00219     }
00220 }
00221 
00222 #endif