Bench-MR
A Motion Planning Benchmark for Wheeled Mobile Robots
gnode.h
Go to the documentation of this file.
1#pragma once
2
3#include <ompl/util/Console.h>
4
5#include "../../base/PlannerSettings.h"
6#include "../../utils/PlannerUtils.hpp"
7
8#include "gnode_base.h"
9#include "stl_thetastar.h"
10
11// Gnode class which includes useful methods for the Thetastar search
12class GNode : public GNode_base {
13 public:
15 GNode() {
16 x = 0;
17 y = 0;
18 theta = 0;
19 steer_cost = 0;
20 steer = false;
21
22 nEdges = global::settings.thetaStar.number_edges;
23 READ_OR = 0;
24 CHECK_SUCCESSOR = 0;
25
26 orientations = new double[global::settings.thetaStar.number_edges.value()];
27 costs = new double[global::settings.thetaStar.number_edges.value()];
28
29 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
30 costs[i] = 0;
31
32 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
33 orientations[i] = 0;
34
35 goal_x = (int)global::settings.environment->goal().x;
36 goal_y = (int)global::settings.environment->goal().y;
37 start_x = (int)global::settings.environment->start().x;
38 start_y = (int)global::settings.environment->start().y;
39 hasParent = false;
40 parent = new GNode_base();
41 child = new GNode_base();
42 }
43
44 GNode(double xx, double yy) {
45 x = (int)xx;
46 y = (int)yy;
47
48 theta = 0;
49 steer_cost = 0;
50 steer = false;
51 nEdges = global::settings.thetaStar.number_edges;
52 READ_OR = 0;
53 CHECK_SUCCESSOR = 0;
54 x_r = xx;
55 y_r = yy;
56
57 orientations = new double[global::settings.thetaStar.number_edges.value()];
58 costs = new double[global::settings.thetaStar.number_edges.value()];
59
60 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
61 costs[i] = 1000;
62
63 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
64 orientations[i] = 0;
65
66 goal_x = (int)global::settings.environment->goal().x;
67 goal_y = (int)global::settings.environment->goal().y;
68 start_x = (int)global::settings.environment->start().x;
69 start_y = (int)global::settings.environment->start().y;
70 hasParent = false;
71 parent = new GNode_base();
72 child = new GNode_base();
73 }
74
75 GNode(double xx, double yy, double tt) {
76 x = (int)xx;
77 y = (int)yy;
78 theta = tt;
79
80 steer_cost = 0;
81 steer = false;
82 nEdges = global::settings.thetaStar.number_edges;
83 READ_OR = 0;
84 CHECK_SUCCESSOR = 0;
85 x_r = xx;
86 y_r = yy;
87
88 orientations = new double[global::settings.thetaStar.number_edges.value()];
89 costs = new double[global::settings.thetaStar.number_edges.value()];
90
91 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
92 costs[i] = 1000;
93
94 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
95 orientations[i] = 0;
96
97 goal_x = (int)global::settings.environment->goal().x;
98 goal_y = (int)global::settings.environment->goal().y;
99 start_x = (int)global::settings.environment->start().x;
100 start_y = (int)global::settings.environment->start().y;
101 hasParent = false;
102 parent = new GNode_base();
103 child = new GNode_base();
104 }
105
106 GNode(double xx, double yy, double tt, int type) {
107 x = (int)xx;
108 y = (int)yy;
109 theta = tt;
110
111 steer_cost = 0;
112 steer = type;
113 nEdges = global::settings.thetaStar.number_edges;
114 READ_OR = 0;
115 CHECK_SUCCESSOR = 0;
116 x_r = xx;
117 y_r = yy;
118
119 orientations = new double[global::settings.thetaStar.number_edges.value()];
120 costs = new double[global::settings.thetaStar.number_edges.value()];
121
122 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
123 costs[i] = 1000;
124
125 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
126 orientations[i] = 0;
127
128 parent = new GNode_base();
129 child = new GNode_base();
130
131 goal_x = (int)global::settings.environment->goal().x;
132 goal_y = (int)global::settings.environment->goal().y;
133 start_x = (int)global::settings.environment->start().x;
134 start_y = (int)global::settings.environment->start().y;
135 hasParent = false;
136 }
137
138 GNode(double xx, double yy, double tt, int type, double steer_c) {
139 x = (int)xx;
140 y = (int)yy;
141 theta = tt;
142
143 steer_cost = steer_c;
144 steer = type;
145 nEdges = global::settings.thetaStar.number_edges;
146 READ_OR = 0;
147 CHECK_SUCCESSOR = 0;
148 x_r = xx;
149 y_r = yy;
150
151 orientations = new double[global::settings.thetaStar.number_edges.value()];
152 costs = new double[global::settings.thetaStar.number_edges.value()];
153
154 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
155 costs[i] = 1000;
156
157 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
158 orientations[i] = 0;
159
160 parent = new GNode_base();
161 child = new GNode_base();
162
163 goal_x = (int)global::settings.environment->goal().x;
164 goal_y = (int)global::settings.environment->goal().y;
165 start_x = (int)global::settings.environment->start().x;
166 start_y = (int)global::settings.environment->start().y;
167 hasParent = false;
168 }
169
170 GNode(double xx, double yy, double tt, int type, double steer_c, double *c,
171 double *orien) {
172 x = (int)xx;
173 y = (int)yy;
174 theta = (tt);
175
176 steer_cost = steer_c;
177 steer = type;
178 nEdges = global::settings.thetaStar.number_edges;
179 READ_OR = 0;
180 CHECK_SUCCESSOR = 0;
181 x_r = xx;
182 y_r = yy;
183
184 orientations = new double[global::settings.thetaStar.number_edges.value()];
185 costs = new double[global::settings.thetaStar.number_edges.value()];
186
187 for (int i = 0; i < global::settings.thetaStar.number_edges; i++) {
188 costs[i] = c[i];
189 }
190
191 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
192 orientations[i] = orien[i];
193
194 parent = new GNode_base();
195 child = new GNode_base();
196
197 goal_x = (int)global::settings.environment->goal().x;
198 goal_y = (int)global::settings.environment->goal().y;
199 start_x = (int)global::settings.environment->start().x;
200 start_y = (int)global::settings.environment->start().y;
201 hasParent = false;
202 }
203
204 GNode(double xx, double yy, double tt, int type, double steer_c, double *c,
205 double *orien, double xxx, double yyy) {
206 hasParent = false;
207 x = (int)xx;
208 y = (int)yy;
209 theta = tt;
210 x_r = xxx;
211 y_r = yyy;
212
213 steer_cost = steer_c;
214 steer = type;
215 nEdges = global::settings.thetaStar.number_edges;
216 READ_OR = 0;
217 CHECK_SUCCESSOR = 0;
218
219 orientations = new double[global::settings.thetaStar.number_edges.value()];
220 costs = new double[global::settings.thetaStar.number_edges.value()];
221
222 for (int i = 0; i < global::settings.thetaStar.number_edges; i++) {
223 costs[i] = c[i];
224 }
225
226 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
227 orientations[i] = orien[i];
228
229 parent = new GNode_base();
230 child = new GNode_base();
231
232 goal_x = (int)global::settings.environment->goal().x;
233 goal_y = (int)global::settings.environment->goal().y;
234 start_x = (int)global::settings.environment->start().x;
235 start_y = (int)global::settings.environment->start().y;
236 }
237
238 GNode(const GNode &n) {
239 hasParent = n.hasParent;
240 x = n.x;
241 y = n.y;
242 theta = n.theta;
243 x_r = n.x_r;
244 y_r = n.y_r;
245 goal_x = n.goal_x;
246 goal_y = n.goal_y;
247 start_x = n.start_x;
248 start_y = n.start_y;
249
250 steer_cost = n.steer_cost;
251 steer = n.steer;
252 nEdges = n.nEdges;
253 READ_OR = n.READ_OR;
254 CHECK_SUCCESSOR = n.CHECK_SUCCESSOR;
255
256 orientations = new double[global::settings.thetaStar.number_edges.value()];
257 costs = new double[global::settings.thetaStar.number_edges.value()];
258
259 for (int i = 0; i < global::settings.thetaStar.number_edges; i++) {
260 costs[i] = n.costs[i];
261 }
262
263 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
264 orientations[i] = n.orientations[i];
265
266 parent = n.parent;
267 child = n.child;
268 }
269
270 ~GNode() override {
271 delete[] costs;
272 delete[] orientations;
273 }
274
275 GNode &operator=(const GNode &n) {
276 if (this == &n) return *this;
277
278 hasParent = n.hasParent;
279 x = n.x;
280 y = n.y;
281 theta = n.theta;
282 x_r = n.x_r;
283 y_r = n.y_r;
284 goal_x = n.goal_x;
285 goal_y = n.goal_y;
286 start_x = n.start_x;
287 start_y = n.start_y;
288
289 steer_cost = n.steer_cost;
290 steer = n.steer;
291 nEdges = n.nEdges;
292 READ_OR = n.READ_OR;
293 CHECK_SUCCESSOR = n.CHECK_SUCCESSOR;
294
295 orientations = new double[global::settings.thetaStar.number_edges.value()];
296 costs = new double[global::settings.thetaStar.number_edges.value()];
297
298 for (int i = 0; i < global::settings.thetaStar.number_edges; i++) {
299 costs[i] = 0;
300 }
301
302 for (int i = 0; i < global::settings.thetaStar.number_edges; i++)
303 orientations[i] = 0;
304
305 parent = n.parent;
306 child = n.child;
307
308 return *this;
309 }
310
311 bool operator==(const GNode &n) {
312 return (x == n.x && y == n.y && theta == n.theta && x_r == n.x_r &&
313 y_r == n.y_r);
314 }
315
316 bool operator!=(const GNode &n) {
317 return (x != n.x || y != n.y || theta != n.theta || x_r != n.x_r ||
318 y_r != n.y_r);
319 }
320
321 bool operator<(const GNode &n) const { return (steer_cost < n.steer_cost); }
322
326 double *getLineCost() {
327 double *local_cost;
328 local_cost = new double[global::settings.thetaStar.number_edges.value()];
329 local_cost[0] = 1;
330 for (int i = 1; i < global::settings.thetaStar.number_edges; i++) {
331 local_cost[i] = 1000;
332 }
333
334 return local_cost;
335 }
336
342 bool setOrientation(GNode *parent_node) {
343 double parent_x = parent_node->x;
344 double parent_y = parent_node->y;
345 double dt = .1;
346 theta = atan2((y - parent_y) / dt, (x - parent_x) / dt);
347
348 OMPL_INFORM("setOrientation %f instead of %f", theta, parent_node->theta);
349 return true;
350 }
351
356 bool IsSameState(GNode &rhs) {
357 return (this->x == rhs.x) && (this->y == rhs.y);
358 }
359
362 // Print state of the Node, can be improved
364 void PrintNodeInfo() {
365 char str[100];
366 sprintf(str, "Node position : (%d,%d,%f)\n", x, y, theta);
367 cout << str;
368 }
369
372 // Here's the heuristic function that estimates the distance from a Node
373 // to the Goal.
375 float GoalDistanceEstimate(GNode &nodeGoal) {
376 goal_x = nodeGoal.x;
377 goal_y = nodeGoal.y;
378
379 float xd = ((float)x - (float)nodeGoal.x);
380 float yd = ((float)y - (float)nodeGoal.y);
381 // return fabsf(x - nodeGoal.x) + fabsf(y - nodeGoal.y);
382
383 return sqrt(xd * xd + yd * yd);
384 }
385
390 bool IsGoal(const GNode &nodeGoal) {
391 goal_x = nodeGoal.x;
392 goal_y = nodeGoal.y;
393
394 if ((x == nodeGoal.x) && (y == nodeGoal.y)) {
395 OMPL_DEBUG("Goal reached");
396 return true;
397 }
398
399 return false;
400 }
401
410 bool GetSuccessors_or(ThetaStarSearch<GNode> *thetastarsearch,
411 GNode *parent_node) {
412 int x = this->x;
413 int y = this->y;
414 double th = this->theta;
415 int parent_x = -1;
416 int parent_y = -1;
417
418 if (parent_node) {
419 parent_x = parent_node->x;
420 parent_y = parent_node->y;
421 } else {
422 OMPL_ERROR("GNode has no parent node.");
423 return false;
424 }
425
426 //
427 // Try to find successors for each of the 8 directions (NW to SE)
428 //
429
430 // mod
431 GNode NewNode;
432 int cnt_successors = 0;
433
434 auto *th1 = new GNode(x, y, th);
435 auto *pr1 = new GNode(x - 1, y, 1.0 * M_PI);
436 pr1->CHECK_SUCCESSOR = 1;
437 th1->CHECK_SUCCESSOR = 1;
438 if (lineofsight(th1, pr1) && !((parent_x == x - 1) && (parent_y == y))) {
439 cnt_successors++;
440 NewNode = GNode(x - 1, y, pr1->theta);
441 NewNode.steer_cost = pr1->steer_cost;
442 // NewNode.setParent(this);
443 thetastarsearch->AddSuccessor(NewNode);
444 }
445 delete (th1);
446 delete (pr1);
447
448 auto *th2 = new GNode(x, y, th);
449 auto *pr2 = new GNode(x, y - 1, 1.5 * M_PI);
450
451 pr2->CHECK_SUCCESSOR = 1;
452 th2->CHECK_SUCCESSOR = 1;
453 if (lineofsight(th2, pr2) && !((parent_x == x) && (parent_y == y - 1))) {
454 cnt_successors++;
455 NewNode = GNode(x, y - 1, pr2->theta);
456 NewNode.steer_cost = pr2->steer_cost;
457 // NewNode.setParent(this);
458
459 thetastarsearch->AddSuccessor(NewNode);
460 }
461 delete (th2);
462 delete (pr2);
463
464 auto *th3 = new GNode(x, y, th);
465 auto *pr3 = new GNode(x + 1, y, 0.0 * M_PI);
466
467 pr3->CHECK_SUCCESSOR = 1;
468 th3->CHECK_SUCCESSOR = 1;
469 if (lineofsight(th3, pr3) && !((parent_x == x + 1) && (parent_y == y))) {
470 cnt_successors++;
471 NewNode = GNode(x + 1, y, pr3->theta);
472 NewNode.steer_cost = pr3->steer_cost;
473 // NewNode.setParent(this);
474
475 thetastarsearch->AddSuccessor(NewNode);
476 }
477 delete (th3);
478 delete (pr3);
479
480 auto *th4 = new GNode(x, y, th);
481 auto *pr4 = new GNode(x + 1, y + 1, 0.25 * M_PI);
482
483 pr4->CHECK_SUCCESSOR = 1;
484 th4->CHECK_SUCCESSOR = 1;
485 if (lineofsight(th4, pr4) &&
486 !((parent_x == x + 1) && (parent_y == y + 1))) {
487 cnt_successors++;
488 NewNode = GNode(x + 1, y + 1, pr4->theta);
489 NewNode.steer_cost = pr4->steer_cost;
490 // NewNode.setParent(this);
491
492 thetastarsearch->AddSuccessor(NewNode);
493 }
494 delete (th4);
495 delete (pr4);
496
497 auto *th5 = new GNode(x, y, th);
498 auto *pr5 = new GNode(x - 1, y - 1, 1.25 * M_PI);
499
500 pr5->CHECK_SUCCESSOR = 1;
501 th5->CHECK_SUCCESSOR = 1;
502 if (lineofsight(th5, pr5) &&
503 !((parent_x == x - 1) && (parent_y == y - 1))) {
504 cnt_successors++;
505
506 NewNode = GNode(x - 1, y - 1, pr5->theta);
507 NewNode.steer_cost = pr5->steer_cost;
508 // NewNode.setParent(this);
509
510 thetastarsearch->AddSuccessor(NewNode);
511 }
512 delete (th5);
513 delete (pr5);
514
515 auto *th6 = new GNode(x, y, th);
516 auto *pr6 = new GNode(x + 1, y - 1, 1.75 * M_PI);
517
518 pr6->CHECK_SUCCESSOR = 1;
519 th6->CHECK_SUCCESSOR = 1;
520 if (lineofsight(th6, pr6) &&
521 !((parent_x == x + 1) && (parent_y == y - 1))) {
522 cnt_successors++;
523
524 NewNode = GNode(x + 1, y - 1, pr6->theta);
525 NewNode.steer_cost = pr6->steer_cost;
526 // NewNode.setParent(this);
527
528 thetastarsearch->AddSuccessor(NewNode);
529 }
530 delete (th6);
531 delete (pr6);
532
533 auto *th7 = new GNode(x, y, th);
534 auto *pr7 = new GNode(x - 1, y + 1, 0.75 * M_PI);
535
536 pr7->CHECK_SUCCESSOR = 1;
537 th7->CHECK_SUCCESSOR = 1;
538 if (lineofsight(th7, pr7) &&
539 !((parent_x == x - 1) && (parent_y == y + 1))) {
540 cnt_successors++;
541
542 NewNode = GNode(x - 1, y + 1, pr7->theta);
543 NewNode.steer_cost = pr7->steer_cost;
544 // NewNode.setParent(this);
545
546 thetastarsearch->AddSuccessor(NewNode);
547 }
548 delete (th7);
549 delete (pr7);
550
551 auto *th8 = new GNode(x, y, th);
552 auto *pr8 = new GNode(x, y + 1, 0.5 * M_PI);
553
554 pr8->CHECK_SUCCESSOR = 1;
555 th8->CHECK_SUCCESSOR = 1;
556 if (lineofsight(th8, pr8) && !((parent_x == x) && (parent_y == y + 1))) {
557 cnt_successors++;
558
559 NewNode = GNode(x, y + 1, pr8->theta);
560 NewNode.steer_cost = pr8->steer_cost;
561 // NewNode.setParent(this);
562
563 thetastarsearch->AddSuccessor(NewNode);
564 }
565 delete (th8);
566 delete (pr8);
567
568 if (cnt_successors < 1) {
569 // OMPL_DEBUG("GNode has no successors!");
570 return false;
571 } else {
572 return true;
573 }
574 }
575
580 double GetCost(GNode &successor) {
581 double unit = global::settings.environment->unit();
582 double dx = (successor.x - x) * unit;
583 double dy = (successor.y - y) * unit;
584
585 return std::sqrt(dx * dx + dy * dy);
586 }
587
588 // ============================================================================================
593 float GetCostTraj(GNode &successor) {
594 // return successor.steer_cost;
595
596 double unit = global::settings.environment->unit();
597 return static_cast<float>(
598 global::settings.ompl.objective
599 ->motionCost(
600 base::StateFromXY(x * unit, y * unit),
601 base::StateFromXY(successor.x * unit, successor.y * unit))
602 .value());
603
604 // double res = global::settings.steering->getBestCost(
605 // this->x, this->y, successor.x, successor.y);
606 // if (res < 0) {
607 // OMPL_ERROR(
608 // "Failing to compute the cost.. strange no lineofsight %d %d %d
609 // %d " "cost: %f, yaw: %f", this->x, this->y, successor.x,
610 // successor.y, res, global::settings.steering->getBestYaw(this->x,
611 // this->y, successor.x,
612 // successor.y));
613 // return 0;
614 // } else
615 // return res;
616 }
617
618 // ============================================================================================
623 double GetCostTrajFromParent(GNode &parent, GNode &successor) {
624 double unit = global::settings.environment->unit();
625 return static_cast<double>(
626 global::settings.ompl.objective
627 ->motionCost(
628 base::StateFromXY(parent.x * unit, parent.y * unit),
629 base::StateFromXY(successor.x * unit, successor.y * unit))
630 .value());
631 // double res = global::settings.steering->getBestCost(
632 // parent.x, parent.y, successor.x, successor.y);
633 //
634 // if (res < 0) {
635 // OMPL_ERROR(
636 // "Failing to compute the cost.. strange no PARENTlineofsight %d
637 // %d %d "
638 // "%d cost: %f, yaw: %f",
639 // parent.x, parent.y, successor.x, successor.y, res,
640 // global::settings.steering->getBestYaw(parent.x, parent.y,
641 // successor.x,
642 // successor.y));
643 // return 0;
644 // } else
645 // return res;
646 }
647
653 bool setType(int type) {
654 this->steer = type;
655 return true;
656 }
657
666 bool GetSuccessors(ThetaStarSearch<GNode> *thetastarsearch,
667 GNode *parent_node) {
668 return GetSuccessors_or(thetastarsearch, parent_node);
669 }
670
678 bool lineofsight(GNode *parent_node, GNode *successor) {
679 return line(parent_node, successor);
680 // return PlannerUtils::collides(
681 // Point(parent_node->x_r,
682 // parent_node->y_r).toState(parent_node->theta),
683 // Point(successor->x_r,
684 // successor->y_r).toState(successor->theta));
685 }
686};
GNode_base
Definition: gnode_base.h:9
GNode_base::x_r
double x_r
Definition: gnode_base.h:39
GNode_base::theta
double theta
Orientation associated to the best node.
Definition: gnode_base.h:45
GNode_base::hasParent
bool hasParent
Definition: gnode_base.h:47
GNode_base::start_y
int start_y
Definition: gnode_base.h:34
GNode_base::steer_cost
double steer_cost
Cost associated to the best Node selected.
Definition: gnode_base.h:14
GNode_base::CHECK_SUCCESSOR
int CHECK_SUCCESSOR
Definition: gnode_base.h:28
GNode_base::goal_x
int goal_x
Definition: gnode_base.h:30
GNode_base::parent
GNode_base * parent
Definition: gnode_base.h:49
GNode_base::x
int x
X and Y coordinates of the Node.
Definition: gnode_base.h:37
GNode_base::nEdges
int nEdges
Number of outgoing edges per node.
Definition: gnode_base.h:17
GNode_base::line
static bool line(const GNode_base *parent_node, const GNode_base *successor)
line(GNode *successor,GNode *parent_node)
Definition: gnode_base.cpp:43
GNode_base::orientations
double * orientations
array of possible orientations
Definition: gnode_base.h:23
GNode_base::goal_y
int goal_y
Definition: gnode_base.h:31
GNode_base::steer
bool steer
Flag to indicate if node hase been generated by a steering function.
Definition: gnode_base.h:42
GNode_base::y_r
double y_r
Definition: gnode_base.h:39
GNode_base::READ_OR
int READ_OR
flag to read orientations from Map
Definition: gnode_base.h:26
GNode_base::y
int y
Definition: gnode_base.h:37
GNode_base::start_x
int start_x
Definition: gnode_base.h:33
GNode_base::costs
double * costs
Cost per each orientation associated to the node.
Definition: gnode_base.h:20
GNode_base::child
GNode_base * child
Definition: gnode_base.h:50
GNode
Definition: gnode.h:12
GNode::GNode
GNode(double xx, double yy, double tt, int type)
Definition: gnode.h:106
GNode::GNode
GNode(double xx, double yy, double tt, int type, double steer_c, double *c, double *orien, double xxx, double yyy)
Definition: gnode.h:204
GNode::GNode
GNode()
Different constructors.
Definition: gnode.h:15
GNode::GNode
GNode(double xx, double yy, double tt, int type, double steer_c)
Definition: gnode.h:138
GNode::IsGoal
bool IsGoal(const GNode &nodeGoal)
============================================================================================ bool IsG...
Definition: gnode.h:390
GNode::operator<
bool operator<(const GNode &n) const
Definition: gnode.h:321
GNode::operator!=
bool operator!=(const GNode &n)
Definition: gnode.h:316
GNode::GoalDistanceEstimate
float GoalDistanceEstimate(GNode &nodeGoal)
============================================================================================ GoalDist...
Definition: gnode.h:375
GNode::IsSameState
bool IsSameState(GNode &rhs)
============================================================================================ bool IsS...
Definition: gnode.h:356
GNode::lineofsight
bool lineofsight(GNode *parent_node, GNode *successor)
============================================================================================ lineofsi...
Definition: gnode.h:678
GNode::GetCostTraj
float GetCostTraj(GNode &successor)
float GetCostTraj( GNode &successor ) get the cost of the trajectory that connect the current node to...
Definition: gnode.h:593
GNode::GetSuccessors_or
bool GetSuccessors_or(ThetaStarSearch< GNode > *thetastarsearch, GNode *parent_node)
=========================================== bool GetSuccessors( ThetaStarSearch<GNode> <em>thetastars...
Definition: gnode.h:410
GNode::operator==
bool operator==(const GNode &n)
Definition: gnode.h:311
GNode::GNode
GNode(double xx, double yy)
Definition: gnode.h:44
GNode::getLineCost
double * getLineCost()
============================================================================================
Definition: gnode.h:326
GNode::GNode
GNode(double xx, double yy, double tt, int type, double steer_c, double *c, double *orien)
Definition: gnode.h:170
GNode::GetCost
double GetCost(GNode &successor)
============================================================================================ float Ge...
Definition: gnode.h:580
GNode::setOrientation
bool setOrientation(GNode *parent_node)
============================================================================================ setOrien...
Definition: gnode.h:342
GNode::setType
bool setType(int type)
============================================================================================ setType(...
Definition: gnode.h:653
GNode::GNode
GNode(double xx, double yy, double tt)
Definition: gnode.h:75
GNode::GetSuccessors
bool GetSuccessors(ThetaStarSearch< GNode > *thetastarsearch, GNode *parent_node)
=========================================== bool GetSuccessors( ThetaStarSearch<GNode> <em>thetastars...
Definition: gnode.h:666
GNode::GNode
GNode(const GNode &n)
Definition: gnode.h:238
GNode::operator=
GNode & operator=(const GNode &n)
Definition: gnode.h:275
GNode::PrintNodeInfo
void PrintNodeInfo()
============================================================================================ void Pri...
Definition: gnode.h:364
GNode::GetCostTrajFromParent
double GetCostTrajFromParent(GNode &parent, GNode &successor)
float GetCostTraj( GNode &successor ) get the cost of the trajectory that connect the current node to...
Definition: gnode.h:623
GNode::~GNode
~GNode() override
Definition: gnode.h:270
ThetaStarSearch
Definition: stl_thetastar.h:58
ThetaStarSearch::AddSuccessor
virtual bool AddSuccessor(UserState &State)
Definition: stl_thetastar.h:678
gnode_base.h
base::StateFromXY
ob::State * StateFromXY(double x, double y)
Definition: Primitives.cpp:14
stl_thetastar.h
PlannerSettings::GlobalSettings::OmplSettings::objective
ompl::base::OptimizationObjectivePtr objective
Definition: PlannerSettings.h:409
PlannerSettings::GlobalSettings::environment
std::shared_ptr< Environment > environment
Environment used for planning.
Definition: PlannerSettings.h:73
PlannerSettings::GlobalSettings::ompl
PlannerSettings::GlobalSettings::OmplSettings ompl
global::settings
static PlannerSettings::GlobalSettings settings
Definition: PlannerSettings.h:699
Generated by doxygen 1.9.4