OMPL
Overview
Download
Documentation
Primer
Installation
Tutorials
Demos
Python Bindings
Available Planners
Available State Spaces
FAQ
External links:
OMPL ROS Interface
OMPL ROS Tutorial
Code
API Overview
Classes
Files
Browse Repository
TeamCity Build Server
Issues
Community
Developers
Contributions
Education
Gallery
About
License
Citations
Acknowledgments
Contact Us
Blog
All
Classes
Namespaces
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
All
Classes
Namespaces
Files
Functions
Variables
Typedefs
Enumerations
Enumerator
Friends
Groups
Pages
src
ompl
geometric
planners
rrt
TRRT.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: Dave Coleman */
36
37
#ifndef OMPL_GEOMETRIC_PLANNERS_RRT_TRRT_
38
#define OMPL_GEOMETRIC_PLANNERS_RRT_TRRT_
39
40
#include "ompl/geometric/planners/PlannerIncludes.h"
41
#include "ompl/datastructures/NearestNeighbors.h"
42
43
/*
44
NOTES:
45
**Variable Names that have been converted to longer versions from standards:
46
nearest_neighbors_ -> nn_
47
planner_termination_condition -> ptc
48
49
**Inherited Member Variables Key:
50
si_ -> SpaceInformation
51
pdef_ -> ProblemDefinition
52
pis_ -> PlannerInputStates - Utility class to extract valid input states
53
*/
54
55
56
namespace
ompl
57
{
58
59
namespace
geometric
60
{
61
77
class
TRRT
:
public
base::Planner
78
{
79
public
:
80
82
TRRT
(
const
base::SpaceInformationPtr
&si);
83
84
virtual
~
TRRT
(
void
);
85
86
virtual
void
getPlannerData
(
base::PlannerData
&data)
const
;
87
88
virtual
base::PlannerStatus
solve
(
const
base::PlannerTerminationCondition
&plannerTerminationCondition);
89
90
virtual
void
clear
(
void
);
91
101
void
setGoalBias
(
double
goalBias)
102
{
103
goalBias_
= goalBias;
104
}
105
107
double
getGoalBias
(
void
)
const
108
{
109
return
goalBias_
;
110
}
111
117
void
setRange
(
double
distance)
118
{
119
maxDistance_
= distance;
120
}
121
123
double
getRange
(
void
)
const
124
{
125
return
maxDistance_
;
126
}
127
129
void
setMaxStatesFailed
(
double
maxStatesFailed )
130
{
131
maxStatesFailed_
= maxStatesFailed;
132
}
133
135
double
getMaxStatesFailed
(
void
)
const
136
{
137
return
maxStatesFailed_
;
138
}
139
141
void
setTempChangeFactor
(
double
tempChangeFactor )
142
{
143
tempChangeFactor_
= tempChangeFactor;
144
}
145
147
double
getTempChangeFactor
(
void
)
const
148
{
149
return
tempChangeFactor_
;
150
}
151
153
void
setMinTemperature
(
double
minTemperature )
154
{
155
minTemperature_
= minTemperature;
156
}
157
159
double
getMinTemperature
(
void
)
const
160
{
161
return
minTemperature_
;
162
}
163
165
void
setInitTemperature
(
double
initTemperature )
166
{
167
initTemperature_
= initTemperature;
168
}
169
171
double
getInitTemperature
(
void
)
const
172
{
173
return
initTemperature_
;
174
}
175
178
void
setFrontierThreshold
(
double
frontier_threshold )
179
{
180
frontierThreshold_
= frontier_threshold;
181
}
182
185
double
getFrontierThreshold
(
void
)
const
186
{
187
return
frontierThreshold_
;
188
}
189
192
void
setFrontierNodeRatio
(
double
frontierNodeRatio )
193
{
194
frontierNodeRatio_
= frontierNodeRatio;
195
}
196
199
double
getFrontierNodeRatio
(
void
)
const
200
{
201
return
frontierNodeRatio_
;
202
}
203
205
void
setKConstant
(
double
kConstant )
206
{
207
kConstant_
= kConstant;
208
}
209
211
double
getKConstant
(
void
)
const
212
{
213
return
kConstant_
;
214
}
215
217
template
<
template
<
typename
T>
class
NN>
218
void
setNearestNeighbors
(
void
)
219
{
220
nearestNeighbors_
.reset(
new
NN<Motion*>());
221
}
222
223
virtual
void
setup
(
void
);
224
225
protected
:
226
227
232
class
Motion
233
{
234
public
:
235
236
Motion
(
void
) :
state
(NULL),
parent
(NULL)
237
{
238
}
239
241
Motion
(
const
base::SpaceInformationPtr
&si) :
state
(si->allocState()),
parent
(NULL)
242
{
243
}
244
245
~
Motion
(
void
)
246
{
247
}
248
250
base::State
*
state
;
251
253
Motion
*
parent
;
254
256
double
distance
;
257
259
double
cost
;
260
261
};
262
264
void
freeMemory
(
void
);
265
267
double
distanceFunction
(
const
Motion
* a,
const
Motion
* b)
const
268
{
269
return
si_
->distance(a->
state
, b->
state
);
270
}
271
277
bool
transitionTest
(
double
childCost,
double
parentCost,
double
distance );
278
280
bool
minExpansionControl
(
double
randMotionDistance );
281
283
base::StateSamplerPtr
sampler_
;
284
286
boost::shared_ptr< NearestNeighbors<Motion*> >
nearestNeighbors_
;
287
289
double
goalBias_
;
290
292
double
maxDistance_
;
293
295
RNG
rng_
;
296
298
Motion
*
lastGoalMotion_
;
299
301
bool
verbose_
;
302
303
// *********************************************************************************************************
304
// TRRT-Specific Variables
305
// *********************************************************************************************************
306
307
// Transtion Test -----------------------------------------------------------------------
308
312
double
temp_
;
313
317
double
kConstant_
;
318
320
unsigned
int
maxStatesFailed_
;
321
323
double
tempChangeFactor_
;
324
326
double
minTemperature_
;
327
329
double
initTemperature_
;
330
332
unsigned
int
numStatesFailed_
;
333
334
335
// Minimum Expansion Control --------------------------------------------------------------
336
338
double
nonfrontierCount_
;
339
double
frontierCount_;
340
342
double
frontierThreshold_
;
343
345
double
frontierNodeRatio_
;
346
347
};
348
}
349
}
350
351
#endif