Loading...
Searching...
No Matches
RRTConnect.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_PLANNERS_RRT_RRT_CONNECT_
38#define OMPL_GEOMETRIC_PLANNERS_RRT_RRT_CONNECT_
39
40#include "ompl/datastructures/NearestNeighbors.h"
41#include "ompl/geometric/planners/PlannerIncludes.h"
42
43namespace ompl
44{
45 namespace geometric
46 {
59
62 {
63 public:
65 RRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates = false);
66
67 ~RRTConnect() override;
68
69 void getPlannerData(base::PlannerData &data) const override;
70
72
73 void clear() override;
74
78 {
80 }
81
84 void setIntermediateStates(bool addIntermediateStates)
85 {
86 addIntermediateStates_ = addIntermediateStates;
87 }
88
94 void setRange(double distance)
95 {
96 maxDistance_ = distance;
97 }
98
100 double getRange() const
101 {
102 return maxDistance_;
103 }
104
106 template <template <typename T> class NN>
108 {
109 if ((tStart_ && tStart_->size() != 0) || (tGoal_ && tGoal_->size() != 0))
110 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
111 clear();
112 tStart_ = std::make_shared<NN<Motion *>>();
113 tGoal_ = std::make_shared<NN<Motion *>>();
114 setup();
115 }
116
117 void setup() override;
118
119 protected:
121 class Motion
122 {
123 public:
124 Motion() = default;
125
126 Motion(const base::SpaceInformationPtr &si) : state(si->allocState())
127 {
128 }
129
130 ~Motion() = default;
131
132 const base::State *root{nullptr};
133 base::State *state{nullptr};
134 Motion *parent{nullptr};
135 };
136
138 using TreeData = std::shared_ptr<NearestNeighbors<Motion *>>;
139
142 {
143 base::State *xstate;
144 Motion *xmotion;
145 bool start;
146 };
147
158
160 void freeMemory();
161
163 double distanceFunction(const Motion *a, const Motion *b) const
164 {
165 return si_->distance(a->state, b->state);
166 }
167
169 GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion);
170
172 base::StateSamplerPtr sampler_;
173
176
179
181 bool startTree_{true};
182
184 double maxDistance_{0.};
185
188
191
193 std::pair<base::State *, base::State *> connectionPoint_;
194
197 };
198 }
199}
200
201#endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
Definition of an abstract state.
Definition State.h:50
Representation of a motion.
Definition RRTConnect.h:122
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition RRTConnect.h:193
bool startTree_
A flag that toggles between expanding the start tree (true) or goal tree (false).
Definition RRTConnect.h:181
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
Grow a tree towards a random state.
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition RRTConnect.h:184
double getRange() const
Get the range the planner is using.
Definition RRTConnect.h:100
void setRange(double distance)
Set the range the planner is supposed to use.
Definition RRTConnect.h:94
GrowState
The state of the tree after an attempt to extend it.
Definition RRTConnect.h:150
@ ADVANCED
progress has been made towards the randomly sampled state
Definition RRTConnect.h:154
@ TRAPPED
no progress has been made
Definition RRTConnect.h:152
@ REACHED
the randomly sampled state was reached
Definition RRTConnect.h:156
bool addIntermediateStates_
Flag indicating whether intermediate states are added to the built tree of motions.
Definition RRTConnect.h:187
void freeMemory()
Free the memory allocated by this planner.
std::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
Definition RRTConnect.h:138
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
bool getIntermediateStates() const
Return true if the intermediate states generated along motions are to be added to the tree itself.
Definition RRTConnect.h:77
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
RNG rng_
The random number generator.
Definition RRTConnect.h:190
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition RRTConnect.h:107
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setIntermediateStates(bool addIntermediateStates)
Specify whether the intermediate states generated along motions are to be added to the tree itself.
Definition RRTConnect.h:84
TreeData tStart_
The start tree.
Definition RRTConnect.h:175
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
double distanceBetweenTrees_
Distance between the nearest pair of start tree and goal tree nodes.
Definition RRTConnect.h:196
base::StateSamplerPtr sampler_
State sampler.
Definition RRTConnect.h:172
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition RRTConnect.h:163
RRTConnect(const base::SpaceInformationPtr &si, bool addIntermediateStates=false)
Constructor.
TreeData tGoal_
The goal tree.
Definition RRTConnect.h:178
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Information attached to growing a tree of motions (used internally)
Definition RRTConnect.h:142