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/base/OptimizationObjective.h"
46 #include "ompl/util/Console.h"
47 #include "ompl/util/ClassForward.h"
48 #include "ompl/base/ScopedState.h"
49 
50 #include <vector>
51 #include <cstdlib>
52 #include <iostream>
53 #include <limits>
54 
55 #include <boost/noncopyable.hpp>
56 
57 namespace ompl
58 {
59  namespace base
60  {
61 
63 
64  OMPL_CLASS_FORWARD(ProblemDefinition);
66 
72  {
74  PlannerSolution(const PathPtr &path, bool approximate = false, double difference = -1.0) :
75  index_(-1), path_(path), length_(path->length()), approximate_(approximate), difference_(difference)
76  {
77  }
78 
80  bool operator==(const PlannerSolution& p) const
81  {
82  return path_ == p.path_;
83  }
84 
86  bool operator<(const PlannerSolution &b) const
87  {
88  if (!approximate_ && b.approximate_)
89  return true;
90  if (approximate_ && !b.approximate_)
91  return false;
92  if (approximate_ && b.approximate_)
93  return difference_ < b.difference_;
94  return length_ < b.length_;
95  }
96 
98  int index_;
99 
102 
104  double length_;
105 
108 
110  double difference_;
111  };
112 
116  class ProblemDefinition : private boost::noncopyable
117  {
118  public:
119 
122 
123  virtual ~ProblemDefinition(void)
124  {
126  }
127 
130  {
131  return si_;
132  }
133 
135  void addStartState(const State *state)
136  {
137  startStates_.push_back(si_->cloneState(state));
138  }
139 
141  void addStartState(const ScopedState<> &state)
142  {
143  startStates_.push_back(si_->cloneState(state.get()));
144  }
145 
149  bool hasStartState(const State *state, unsigned int *startIndex = NULL);
150 
152  void clearStartStates(void)
153  {
154  for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
155  si_->freeState(startStates_[i]);
156  startStates_.clear();
157  }
158 
160  unsigned int getStartStateCount(void) const
161  {
162  return startStates_.size();
163  }
164 
166  const State* getStartState(unsigned int index) const
167  {
168  return startStates_[index];
169  }
170 
172  State* getStartState(unsigned int index)
173  {
174  return startStates_[index];
175  }
176 
178  void setGoal(const GoalPtr &goal)
179  {
180  goal_ = goal;
181  }
182 
184  void clearGoal(void)
185  {
186  goal_.reset();
187  }
188 
190  const GoalPtr& getGoal(void) const
191  {
192  return goal_;
193  }
194 
199  void getInputStates(std::vector<const State*> &states) const;
200 
208  void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
209 
211  void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
212 
214  void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
215  {
216  setStartAndGoalStates(start.get(), goal.get(), threshold);
217  }
218 
220  void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
221  {
222  setGoalState(goal.get(), threshold);
223  }
224 
226  bool hasOptimizationObjective(void) const
227  {
228  return optimizationObjective_;
229  }
230 
233  {
234  return optimizationObjective_;
235  }
236 
238  void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
239  {
240  optimizationObjective_ = optimizationObjective;
241  }
242 
248  bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
249 
262  PathPtr isStraightLinePathValid(void) const;
263 
268  bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
269 
271  bool hasSolution(void) const;
272 
276  bool hasApproximateSolution(void) const;
277 
279  double getSolutionDifference(void) const;
280 
285  PathPtr getSolutionPath(void) const;
286 
291  void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0) const;
292 
294  std::size_t getSolutionCount(void) const;
295 
297  std::vector<PlannerSolution> getSolutions(void) const;
298 
300  void clearSolutionPaths(void) const;
301 
303  bool hasSolutionNonExistenceProof(void) const;
304 
307 
310 
312  void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr& nonExistenceProof);
313 
315  void print(std::ostream &out = std::cout) const;
316 
317  protected:
318 
320  bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
321 
324 
326  std::vector<State*> startStates_;
327 
330 
333 
336 
337  private:
338 
340  OMPL_CLASS_FORWARD(PlannerSolutionSet);
342 
344  PlannerSolutionSetPtr solutions_;
345  };
346  }
347 }
348 
349 #endif