PathSimplifier.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, Ryan Luna */
36 
37 #ifndef OMPL_GEOMETRIC_PATH_SIMPLIFIER_
38 #define OMPL_GEOMETRIC_PATH_SIMPLIFIER_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/geometric/PathGeometric.h"
42 #include "ompl/base/PlannerTerminationCondition.h"
43 #include "ompl/base/OptimizationObjective.h"
44 #include "ompl/base/goals/GoalSampleableRegion.h"
45 #include "ompl/util/ClassForward.h"
46 #include "ompl/util/RandomNumbers.h"
47 #include "ompl/util/Console.h"
48 #include <limits>
49 
50 namespace ompl
51 {
52  namespace geometric
53  {
55 
56  OMPL_CLASS_FORWARD(PathSimplifier);
58 
66  {
67  public:
71 
72  virtual ~PathSimplifier() = default;
73 
93  bool reduceVertices(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0,
94  double rangeRatio = 0.33);
95 
125  bool shortcutPath(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0,
126  double rangeRatio = 0.33, double snapToVertex = 0.005);
127 
160  bool perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0, double snapToVertex = 0.005);
161 
177  bool collapseCloseVertices(PathGeometric &path, unsigned int maxSteps = 0, unsigned int maxEmptySteps = 0);
178 
188  void smoothBSpline(PathGeometric &path, unsigned int maxSteps = 5,
189  double minChange = std::numeric_limits<double>::epsilon());
190 
196  bool simplifyMax(PathGeometric &path);
197 
200  bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce = true);
201 
204  bool simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc, bool atLeastOnce = true);
205 
219  bool findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts = 10,
220  double rangeRatio = 0.33, double snapToVertex = 0.005);
221 
236  unsigned int samplingAttempts = 10, double rangeRatio = 0.33,
237  double snapToVertex = 0.005);
238 
242  void freeStates(bool flag);
243 
246  bool freeStates() const;
247 
248  protected:
249 
250  int selectAlongPath(std::vector<double> dists, std::vector<base::State *> states,
251  double distTo, double threshold, base::State *select_state, int &pos);
252 
255 
257  std::shared_ptr<base::GoalSampleableRegion> gsr_;
258 
262 
265 
268  };
269  }
270 }
271 
272 #endif
bool simplifyMax(PathGeometric &path)
Given a path, attempt to remove vertices from it while keeping the path valid. Then, try to smooth the path. This function applies the same set of default operations to the path, except in non-metric spaces, with the intention of simplifying it. In non-metric spaces, some operations are skipped because they do not work correctly when the triangle inequality may not hold. Return false iff the simplified path is not valid.
bool freeStates() const
Return true if the memory of states is freed when they are removed from a path during simplification...
bool findBetterGoal(PathGeometric &path, double maxTime, unsigned int samplingAttempts=10, double rangeRatio=0.33, double snapToVertex=0.005)
Attempt to improve the solution path by sampling a new goal state and connecting this state to the so...
bool shortcutPath(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33, double snapToVertex=0.005)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
RNG rng_
Instance of random number generator.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
base::OptimizationObjectivePtr obj_
The optimization objective to use when making improvements. Will be used on all methods except reduce...
std::shared_ptr< base::GoalSampleableRegion > gsr_
The goal object for the path simplifier. Used for end-of-path improvements.
bool perturbPath(PathGeometric &path, double stepSize, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double snapToVertex=0.005)
Given a path, attempt to improve the cost by randomly perturbing a randomly selected point on the pat...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
bool reduceVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
PathSimplifier(base::SpaceInformationPtr si, const base::GoalPtr &goal=ompl::base::GoalPtr(), base::OptimizationObjectivePtr obj=nullptr)
Create an instance for a specified space information. Optionally, a GoalSampleableRegion may be passe...
base::SpaceInformationPtr si_
The space information this path simplifier uses.
This class contains routines that attempt to simplify geometric paths.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
A shared pointer wrapper for ompl::base::OptimizationObjective.
A shared pointer wrapper for ompl::base::Goal.
Definition of a geometric path.
Definition: PathGeometric.h:60
bool collapseCloseVertices(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0)
Given a path, attempt to remove vertices from it while keeping the path valid. This is an iterative p...
bool freeStates_
Flag indicating whether the states removed from a motion should be freed.
void smoothBSpline(PathGeometric &path, unsigned int maxSteps=5, double minChange=std::numeric_limits< double >::epsilon())
Given a path, attempt to smooth it (the validity of the path is maintained).
bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce=true)
Run simplification algorithms on the path for at most maxTime seconds, and at least once if atLeastOn...