All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
ProblemDefinition.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_BASE_PROBLEM_DEFINITION_
38 #define OMPL_BASE_PROBLEM_DEFINITION_
39 
40 #include "ompl/base/State.h"
41 #include "ompl/base/Goal.h"
42 #include "ompl/base/Path.h"
43 #include "ompl/base/SpaceInformation.h"
44 #include "ompl/base/SolutionNonExistenceProof.h"
45 #include "ompl/util/Console.h"
46 #include "ompl/util/ClassForward.h"
47 #include "ompl/base/ScopedState.h"
48 
49 #include <vector>
50 #include <cstdlib>
51 #include <iostream>
52 #include <limits>
53 
54 #include <boost/noncopyable.hpp>
55 
56 namespace ompl
57 {
58  namespace base
59  {
60 
62 
63  ClassForward(ProblemDefinition);
65 
71  {
73  PlannerSolution(const PathPtr &path, bool approximate = false, double difference = -1.0) :
74  index_(-1), path_(path), length_(path->length()), approximate_(approximate), difference_(difference)
75  {
76  }
77 
79  bool operator==(const PlannerSolution& p) const
80  {
81  return path_ == p.path_;
82  }
83 
85  bool operator<(const PlannerSolution &b) const
86  {
87  if (!approximate_ && b.approximate_)
88  return true;
89  if (approximate_ && !b.approximate_)
90  return false;
91  if (approximate_ && b.approximate_)
92  return difference_ < b.difference_;
93  return length_ < b.length_;
94  }
95 
97  int index_;
98 
101 
103  double length_;
104 
107 
109  double difference_;
110  };
111 
115  class ProblemDefinition : private boost::noncopyable
116  {
117  public:
118 
121 
122  virtual ~ProblemDefinition(void)
123  {
125  }
126 
129  {
130  return si_;
131  }
132 
134  void addStartState(const State *state)
135  {
136  startStates_.push_back(si_->cloneState(state));
137  }
138 
140  void addStartState(const ScopedState<> &state)
141  {
142  startStates_.push_back(si_->cloneState(state.get()));
143  }
144 
148  bool hasStartState(const State *state, unsigned int *startIndex = NULL);
149 
151  void clearStartStates(void)
152  {
153  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
154  si_->freeState(startStates_[i]);
155  startStates_.clear();
156  }
157 
159  unsigned int getStartStateCount(void) const
160  {
161  return startStates_.size();
162  }
163 
165  const State* getStartState(unsigned int index) const
166  {
167  return startStates_[index];
168  }
169 
171  State* getStartState(unsigned int index)
172  {
173  return startStates_[index];
174  }
175 
177  void setGoal(const GoalPtr &goal)
178  {
179  goal_ = goal;
180  }
181 
183  void clearGoal(void)
184  {
185  goal_.reset();
186  }
187 
189  const GoalPtr& getGoal(void) const
190  {
191  return goal_;
192  }
193 
198  void getInputStates(std::vector<const State*> &states) const;
199 
207  void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
208 
210  void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
211 
213  void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
214  {
215  setStartAndGoalStates(start.get(), goal.get(), threshold);
216  }
217 
219  void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
220  {
221  setGoalState(goal.get(), threshold);
222  }
223 
229  bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
230 
243  PathPtr isStraightLinePathValid(void) const;
244 
249  bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
250 
252  bool hasSolution(void) const;
253 
257  bool hasApproximateSolution(void) const;
258 
260  double getSolutionDifference(void) const;
261 
266  PathPtr getSolutionPath(void) const;
267 
272  void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0) const;
273 
275  std::size_t getSolutionCount(void) const;
276 
278  std::vector<PlannerSolution> getSolutions(void) const;
279 
281  void clearSolutionPaths(void) const;
282 
284  void print(std::ostream &out = std::cout) const;
285 
287  bool hasSolutionNonExistenceProof(void) const;
288 
291 
294 
296  void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr& nonExistenceProof);
297 
298  protected:
299 
301  bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
302 
305 
307  std::vector<State*> startStates_;
308 
311 
314 
315  private:
316 
318  ClassForward(PlannerSolutionSet);
320 
322  PlannerSolutionSetPtr solutions_;
323  };
324  }
325 }
326 
327 #endif