PathGeometric.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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_GEOMETRIC_PATH_GEOMETRIC_
38 #define OMPL_GEOMETRIC_PATH_GEOMETRIC_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/base/Path.h"
42 #include <vector>
43 #include <utility>
44 
45 namespace ompl
46 {
47  namespace base
48  {
50  OMPL_CLASS_FORWARD(OptimizationObjective);
52  }
53 
55  namespace geometric
56  {
60  class PathGeometric : public base::Path
61  {
62  public:
64  PathGeometric(const base::SpaceInformationPtr &si) : base::Path(si)
65  {
66  }
67 
69  PathGeometric(const PathGeometric &path);
70 
72  PathGeometric(const base::SpaceInformationPtr &si, const base::State *state);
73 
75  PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1, const base::State *state2);
76 
77  ~PathGeometric() override
78  {
79  freeMemory();
80  }
81 
83  PathGeometric &operator=(const PathGeometric &other);
84 
90  base::Cost cost(const base::OptimizationObjectivePtr &obj) const override;
91 
93  double length() const override;
94 
96  bool check() const override;
97 
116  double smoothness() const;
117 
130  double clearance() const;
131 
133  void print(std::ostream &out) const override;
134 
139  virtual void printAsMatrix(std::ostream &out) const;
140 
149  void interpolate(unsigned int count);
150 
155  void interpolate();
156 
158  void subdivide();
159 
161  void reverse();
162 
174  std::pair<bool, bool> checkAndRepair(unsigned int attempts);
175 
186  void overlay(const PathGeometric &over, unsigned int startIndex = 0);
187 
189  void append(const base::State *state);
190 
202  void append(const PathGeometric &path);
203 
205  void prepend(const base::State *state);
206 
209  void keepAfter(const base::State *state);
210 
213  void keepBefore(const base::State *state);
214 
216  void random();
217 
220  bool randomValid(unsigned int attempts);
228  int getClosestIndex(const base::State *state) const;
229 
232  std::vector<base::State *> &getStates()
233  {
234  return states_;
235  }
236 
238  base::State *getState(unsigned int index)
239  {
240  return states_[index];
241  }
242 
244  const base::State *getState(unsigned int index) const
245  {
246  return states_[index];
247  }
248 
250  std::size_t getStateCount() const
251  {
252  return states_.size();
253  }
254 
256  void clear();
257 
260  protected:
262  void freeMemory();
263 
265  void copyFrom(const PathGeometric &other);
266 
268  std::vector<base::State *> states_;
269  };
270  }
271 }
272 
273 #endif
base::Cost cost(const base::OptimizationObjectivePtr &obj) const override
The sum of the costs for the sequence of segments that make up the path, computed using OptimizationO...
void keepAfter(const base::State *state)
Keep the part of the path that is after state (getClosestIndex() is used to find out which way-point ...
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path...
void freeMemory()
Free the memory corresponding to the states on this path.
void interpolate()
Insert a number of states in a path so that the path is made up of (approximately) the states checked...
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
void print(std::ostream &out) const override
Print the path to a stream.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void copyFrom(const PathGeometric &other)
Copy data to this path from another path instance.
void overlay(const PathGeometric &over, unsigned int startIndex=0)
Overlay the path over on top of the current path. States are added to the current path if needed (by ...
void random()
Set this path to a random segment.
const base::State * getState(unsigned int index) const
Get the state located at index along the path.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
PathGeometric(const base::SpaceInformationPtr &si)
Construct a path instance for a given space information.
Definition: PathGeometric.h:64
base::State * getState(unsigned int index)
Get the state located at index along the path.
std::pair< bool, bool > checkAndRepair(unsigned int attempts)
Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invali...
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Abstract definition of a path.
Definition: Path.h:67
void reverse()
Reverse the path.
bool check() const override
Check if the path is valid.
double clearance() const
Compute the clearance of the way-points along the path (no interpolation is performed). Detailed formula follows.
A shared pointer wrapper for ompl::base::SpaceInformation.
void prepend(const base::State *state)
Prepend state to the start of this path. The memory for state is copied.
Definition of an abstract state.
Definition: State.h:49
PathGeometric & operator=(const PathGeometric &other)
Assignment operator.
A shared pointer wrapper for ompl::base::OptimizationObjective.
void clear()
Remove all states and clear memory.
void keepBefore(const base::State *state)
Keep the part of the path that is before state (getClosestIndex() is used to find out which way-point...
void subdivide()
Add a state at the middle of each segment.
Definition of a geometric path.
Definition: PathGeometric.h:60
double smoothness() const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
std::vector< base::State * > states_
The list of states that make up the path.
int getClosestIndex(const base::State *state) const
Get the index of the way-point along the path that is closest to state. Returns -1 for an empty path...
std::vector< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...