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 
00061         ClassForward(ProblemDefinition);
00063 
00069         class ProblemDefinition : private boost::noncopyable
00070         {
00071         public:
00072 
00074             ProblemDefinition(const SpaceInformationPtr &si) : si_(si)
00075             {
00076             }
00077 
00078             virtual ~ProblemDefinition(void)
00079             {
00080                 clearStartStates();
00081             }
00082 
00084             const SpaceInformationPtr& getSpaceInformation(void) const
00085             {
00086                 return si_;
00087             }
00088 
00090             void addStartState(const State *state)
00091             {
00092                 startStates_.push_back(si_->cloneState(state));
00093             }
00094 
00096             void addStartState(const ScopedState<> &state)
00097             {
00098                 startStates_.push_back(si_->cloneState(state.get()));
00099             }
00100 
00104             bool hasStartState(const State *state, unsigned int *startIndex = NULL);
00105 
00107             void clearStartStates(void)
00108             {
00109                 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00110                     si_->freeState(startStates_[i]);
00111                 startStates_.clear();
00112             }
00113 
00115             unsigned int getStartStateCount(void) const
00116             {
00117                 return startStates_.size();
00118             }
00119 
00121             const State* getStartState(unsigned int index) const
00122             {
00123                 return startStates_[index];
00124             }
00125 
00127             State* getStartState(unsigned int index)
00128             {
00129                 return startStates_[index];
00130             }
00131 
00133             void setGoal(const GoalPtr &goal)
00134             {
00135                 goal_ = goal;
00136             }
00137 
00139             void clearGoal(void)
00140             {
00141                 goal_.reset();
00142             }
00143 
00145             const GoalPtr& getGoal(void) const
00146             {
00147                 return goal_;
00148             }
00149 
00154             void getInputStates(std::vector<const State*> &states) const;
00155 
00163             void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00164 
00166             void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00167 
00169             void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00170             {
00171                 setStartAndGoalStates(start.get(), goal.get(), threshold);
00172             }
00173 
00175             void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00176             {
00177                 setGoalState(goal.get(), threshold);
00178             }
00179 
00185             bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
00186 
00199             PathPtr isStraightLinePathValid(void) const;
00200 
00205             bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
00206 
00208             void print(std::ostream &out = std::cout) const;
00209 
00210         protected:
00211 
00213             bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
00214 
00216             SpaceInformationPtr  si_;
00217 
00219             std::vector<State*>  startStates_;
00220 
00222             GoalPtr              goal_;
00223 
00225             msg::Interface       msg_;
00226         };
00227     }
00228 }
00229 
00230 #endif
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends