Revert "Some changes to avoid a falling into a bug that produces the task's fail."
[rock-planning:vfh_star.git] / src / TreeSearch.cpp
1 #include "TreeSearch.h"
2 #include <Eigen/Core>
3 #include <map>
4 #include <stdexcept>
5 #include <iostream>
6 #include <base/angle.h>
7 #include <base/float.h>
8
9 using namespace vfh_star;
10
11 TreeSearchConf::TreeSearchConf()
12     : maxTreeSize(10000)
13     , stepDistance(0.3)
14     , angularSamplingMin(2 * M_PI / 120)
15     , angularSamplingMax(2 * M_PI / 20)
16     , angularSamplingNominalCount(5)
17     , discountFactor(1.0)
18     , obstacleSafetyDistance(0.1)
19     , robotWidth(0.5)
20     , identityPositionThreshold(0.06)
21     , identityYawThreshold(0.05)
22     , maxStepSize(0.2)
23     {}
24
25             
26 void TreeSearchConf::computePosAndYawThreshold()
27 {
28     if(identityPositionThreshold < 0)
29     {
30         identityPositionThreshold = stepDistance / 5.0;
31     }
32     
33     if(identityYawThreshold < 0)
34     {
35         identityYawThreshold = 3.0 * 180.0 / M_PI;
36     }
37
38 }
39     
40     
41 TreeSearch::TreeSearch(): nnLookup(NULL)
42 {
43     search_conf.computePosAndYawThreshold();
44 }
45
46 TreeSearch::~TreeSearch()
47 {
48     delete nnLookup;
49 }
50
51 void TreeSearch::setSearchConf(const TreeSearchConf& conf)
52 {
53     this->search_conf = conf;
54     search_conf.computePosAndYawThreshold();
55
56     //trigger update of nearest neighbour lookup 
57     //will be reconstructed on next search
58     delete nnLookup;
59     nnLookup = 0;
60 }
61
62 const TreeSearchConf& TreeSearch::getSearchConf() const
63 {
64     return search_conf;
65 }
66
67 TreeSearch::Angles TreeSearch::getDirectionsFromIntervals(double curDir, const TreeSearch::AngleIntervals& intervals)
68 {
69     std::vector< double > ret;
70
71     double minStep = search_conf.angularSamplingMin;
72     double maxStep = search_conf.angularSamplingMax;
73     int minNodes = search_conf.angularSamplingNominalCount;
74     
75     // double size = intervals.size();
76     // std::cout << "Interval vector size " << size << std::endl;
77     bool straight = false;
78     
79     for (AngleIntervals::const_iterator it = intervals.begin(); it != intervals.end(); it++) 
80     {
81         double start = (it->first);
82         double end  = (it->second);
83
84         double intervalOpening;
85
86         // Special case, wrapping interval
87         if (start > end)
88         {
89             if ((curDir > start && curDir < end - 2 * M_PI) || (curDir < end && curDir + 2 * M_PI > start))
90                 straight = true;
91
92             intervalOpening = end + 2 * M_PI - start;
93         }
94         else
95         {
96             if (curDir > start && curDir < end)
97                 straight = true;
98
99             intervalOpening = end - start;
100         }
101
102         double step = intervalOpening / minNodes;
103         if (step < minStep)
104             step = minStep;
105         else if (step > maxStep)
106             step = maxStep;
107
108         int intervalSize = floor(intervalOpening / step);
109         double delta = (intervalOpening - (intervalSize * step)) / 2.0;
110         for (int i = 0; i < intervalSize + 1; ++i)
111         {
112             double angle = start + delta + i * step;
113             if (angle > 2 * M_PI)
114                 ret.push_back(angle - 2 * M_PI);
115             else
116                 ret.push_back(angle);
117         }
118     }
119
120     if (straight)
121         ret.push_back(curDir);
122     //std::cerr << "found " << ret.size() << " possible directions" << std::endl;
123     
124     return ret;
125 }
126
127 base::geometry::Spline<3> TreeSearch::waypointsToSpline(const std::vector<base::Waypoint>& waypoints)
128 {
129     if (waypoints.empty())
130         return base::geometry::Spline<3>();
131
132     std::vector<base::Vector3d> as_points;
133     for(std::vector<base::Waypoint>::const_iterator it = waypoints.begin();
134             it != waypoints.end(); it++)
135         as_points.push_back(it->position);
136
137     base::geometry::Spline<3> spline;
138     spline.interpolate(as_points);
139     return spline;
140 }
141
142 base::geometry::Spline<3> TreeSearch::getTrajectory(const base::Pose& start)
143 {
144     std::vector<base::Waypoint> waypoints =
145         getWaypoints(start);
146
147     return waypointsToSpline(waypoints);
148 }
149
150 TreeNode const* TreeSearch::compute(const base::Pose& start)
151 {
152     tree.clear();
153     if(!nnLookup)
154         nnLookup = new NNLookup(1.0, search_conf.identityPositionThreshold / 2.0 , search_conf.identityYawThreshold / 2.0);
155     
156     nnLookup->clear();
157     TreeNode *curNode = tree.createRoot(start, start.getYaw());
158     curNode->setHeuristic(getHeuristic(*curNode));
159     curNode->setCost(0.0);
160     nnLookup->setNode(curNode);
161
162     curNode->candidate_it = expandCandidates.insert(std::make_pair(curNode->getHeuristicCost(), curNode));
163     
164     int max_depth = search_conf.maxTreeSize;
165     
166     base::Time startTime = base::Time::now();
167     
168     while(!expandCandidates.empty()) 
169     {
170         curNode = expandCandidates.begin()->second;
171         expandCandidates.erase(expandCandidates.begin());
172         curNode->candidate_it = expandCandidates.end();
173
174         if(!search_conf.maxSeekTime.isNull() && base::Time::now() - startTime > search_conf.maxSeekTime)
175             break;
176         
177         if (!validateNode(*curNode))
178         {
179             curNode->heuristic = -1;
180             nnLookup->clearIfSame(curNode);
181             continue;
182         }
183
184         bool terminal = isTerminalNode(*curNode);
185         if (!curNode->updated_cost && updateCost(*curNode, terminal))
186         {
187             curNode->updated_cost = true;
188
189             double hcost = curNode->getHeuristicCost();
190             if (hcost > expandCandidates.begin()->first)
191             {
192                 // reinsert in the candidate queue and start again
193                 curNode->candidate_it = expandCandidates.insert(std::make_pair(hcost, curNode));
194                 continue;
195             }
196         }
197
198         if (terminal)
199         {
200             tree.setFinalNode(curNode);
201             break;
202         }
203
204         if (max_depth > 0 && tree.getSize() > max_depth)
205             continue;
206
207         // Get possible ways to go out of this node
208         AngleIntervals driveIntervals =
209             getNextPossibleDirections(*curNode,
210                     search_conf.obstacleSafetyDistance, search_conf.robotWidth);
211
212         if (driveIntervals.empty())
213             continue;
214
215         Angles driveDirections =
216             getDirectionsFromIntervals(curNode->getDirection(), driveIntervals);
217         if (driveDirections.empty())
218             continue;
219
220         double curDiscount = pow(search_conf.discountFactor, curNode->getDepth());
221
222         // Expand the node: add children in the directions returned by
223         // driveDirections
224         for (Angles::const_iterator it = driveDirections.begin(); it != driveDirections.end(); it++)
225         {
226             if (max_depth > 0 && tree.getSize() >= max_depth)
227                 break;
228
229             const double curDirection(*it);
230
231             //generate new node
232             std::pair<base::Pose, bool> projected =
233                 getProjectedPose(*curNode, curDirection,
234                         search_conf.stepDistance);
235             if (!projected.second)
236                 continue;
237
238             //compute cost for it
239             double nodeCost = curDiscount * getCostForNode(projected.first, curDirection, *curNode);
240
241             // Check that we are not doing the same work multiple times.
242             //
243             // searchNode should be used only here !
244             TreeNode searchNode(projected.first, curDirection);
245             
246             const double searchNodeCost = nodeCost + curNode->getCost();
247             TreeNode *closest_node = nnLookup->getNodeWithinBounds(searchNode);
248             if(closest_node)
249             {
250                 if(closest_node->getCost() <= searchNodeCost)
251                 {
252                     //Existing node is better than current node
253                     //discard the current node
254                     continue;
255                 } 
256                 else
257                 {
258                     //remove from parent
259                     closest_node->parent->removeChild(closest_node);
260                     
261                     //remove closest node and subnodes
262                     removeSubtreeFromSearch(closest_node);                    
263                 }
264             }
265             
266
267             // Finally, create the new node and add it in the tree
268             TreeNode *newNode = tree.createChild(curNode, projected.first, curDirection);
269             newNode->setCost(curNode->getCost() + nodeCost);
270             newNode->setCostFromParent(nodeCost);
271             newNode->setPositionTolerance(search_conf.obstacleSafetyDistance);
272             newNode->setHeadingTolerance(std::numeric_limits< double >::signaling_NaN());
273             newNode->setHeuristic(curDiscount * getHeuristic(*newNode));
274
275             // Add it to the expand list
276             newNode->candidate_it = expandCandidates.insert(std::make_pair(newNode->getHeuristicCost(), newNode));
277
278             //add new node to nearest neighbour lookup
279             nnLookup->setNode(newNode);
280         }
281     }
282
283     expandCandidates.clear();
284     
285     curNode = tree.getFinalNode();
286        
287     if (curNode)
288     {
289         std::cerr << "TreeSearch: found solution at c=" << curNode->getCost() << std::endl;
290         tree.verifyHeuristicConsistency(curNode);
291         return curNode;
292     }
293     return 0;
294 }
295
296 void TreeSearch::updateNodeCosts(TreeNode* node)
297 {
298     const std::vector<TreeNode *> &childs(node->getChildren());
299     
300     for(std::vector<TreeNode *>::const_iterator it = childs.begin(); it != childs.end();it++)
301     {
302         const double costToNode = (*it)->getCostFromParent();
303         (*it)->setCost(node->getCost() + costToNode);
304         //if this node is in the expand list remove and reenter it
305         //so that the position in the queue gets updated
306         if((*it)->candidate_it != expandCandidates.end())
307         {
308             expandCandidates.erase((*it)->candidate_it);
309             (*it)->candidate_it = expandCandidates.insert(std::make_pair((*it)->getHeuristicCost(), (*it)));
310         };
311         
312         updateNodeCosts(*it);
313     }
314 }
315
316 void TreeSearch::removeSubtreeFromSearch(TreeNode* node)
317 {
318     if(node->candidate_it != expandCandidates.end())
319     {
320         expandCandidates.erase(node->candidate_it);
321         node->candidate_it = expandCandidates.end();
322     };
323     
324     nnLookup->clearIfSame(node);
325     
326     const std::vector<TreeNode *> &childs(node->getChildren());
327     
328     for(std::vector<TreeNode *>::const_iterator it = childs.begin(); it != childs.end();it++)
329     {
330         removeSubtreeFromSearch(*it);
331     }
332     //all children are invalid now we can just clear them
333     node->childs.clear();
334     
335     tree.removeNode(node);
336 }
337
338 std::vector< base::Waypoint > TreeSearch::getWaypoints(const base::Pose& start)
339 {
340     TreeNode const* curNode = compute(start);
341     if (curNode)
342         return tree.buildTrajectoryTo(curNode);
343     else
344         return std::vector<base::Waypoint>();
345 }
346
347 bool TreeSearch::validateNode(const TreeNode& node) const
348 {
349     return true;
350 }
351
352 bool TreeSearch::updateCost(TreeNode& node, bool is_terminal) const
353 {
354     return false;
355 }
356
357 const Tree& TreeSearch::getTree() const
358 {
359     return tree;
360 }
361
362 Tree::Tree()
363     : size(0)
364     , final_node(0)
365     , root_node(0)    
366 {
367 }
368
369 Tree::~Tree()
370 {
371 }
372
373 Tree::Tree(Tree const& other)
374 {
375     *this = other;
376 }
377
378 void Tree::copyNodeChilds(const TreeNode* otherNode, TreeNode *ownNode, Tree const& other)
379 {
380     for(std::vector<TreeNode *>::const_iterator it = otherNode->getChildren().begin();
381         it != otherNode->getChildren().end(); it++)
382         {
383             TreeNode const* orig_node = *it;
384             TreeNode* new_node = createChild(ownNode, orig_node->getPose(), orig_node->getDirection());
385             new_node->setCost(orig_node->getCost());
386             new_node->setHeuristic(orig_node->getHeuristic());
387             new_node->setHeadingTolerance(orig_node->getHeadingTolerance());
388             new_node->setPositionTolerance(orig_node->getPositionTolerance());
389
390             if(other.getFinalNode() == orig_node)
391                 setFinalNode(new_node);
392
393             copyNodeChilds(*it, new_node, other);
394         }
395 }
396
397 Tree& Tree::operator = (Tree const& other)
398 {
399     if (this == &other)
400         return *this;
401
402     clear();
403
404     if(other.root_node)
405     {
406         root_node = createRoot(other.root_node->getPose(), other.root_node->getDirection());    
407         copyNodeChilds(other.root_node, root_node, other);
408     }
409     
410     
411     return *this;
412 }
413
414 std::vector< base::Trajectory > Tree::buildTrajectoriesTo(const vfh_star::TreeNode* node, const Eigen::Affine3d& body2Trajectory) const
415 {
416     std::vector<const vfh_star::TreeNode *> nodes;
417     const vfh_star::TreeNode* nodeTmp = node;
418     int size = node->getDepth() + 1;
419     for (int i = 0; i < size; ++i)
420     {
421         nodes.insert(nodes.begin(), nodeTmp);
422         if (nodeTmp->isRoot() && i != size - 1)
423             throw std::runtime_error("internal error in buildTrajectoryTo: found a root node even though the trajectory is not finished");
424         nodeTmp = nodeTmp->getParent();
425     }    
426
427     return buildTrajectoriesTo(nodes, body2Trajectory);
428 }
429
430 std::vector< base::Trajectory > Tree::buildTrajectoriesTo(std::vector<const vfh_star::TreeNode *> nodes, const Eigen::Affine3d &body2Trajectory) const
431 {    
432     std::vector<base::Trajectory> result;
433         
434     if(nodes.empty())
435         return result;
436     
437     std::vector<const vfh_star::TreeNode *>::const_iterator it = nodes.begin();
438     std::vector<base::Vector3d> as_points;
439     
440     base::Angle posDir;
441     base::Angle nodeDir;
442     bool lastNodeIsForward = true;
443     if(!nodes.empty())
444     {
445         posDir = base::Angle::fromRad ((*it)->getPose().getYaw());
446         nodeDir = base::Angle::fromRad((*it)->getDirection());
447         lastNodeIsForward = fabs((posDir - nodeDir).rad) < 4.0/5.0 * M_PI;
448
449         const Eigen::Affine3d body2Planner((*it)->getPose().toTransform());
450         const Eigen::Affine3d trajectory2Planner(body2Planner * body2Trajectory.inverse());
451         as_points.push_back((trajectory2Planner * Eigen::Vector3d(0,0,0)));
452         it++;
453
454     }
455  
456     for(;it != nodes.end(); it++)
457     {
458         const vfh_star::TreeNode *curNode = *it;
459         bool curNodeIsForward = fabs((base::Angle::fromRad(curNode->getPose().getYaw()) - base::Angle::fromRad(curNode->getDirection())).rad) < 4.0/5.0 * M_PI;
460         //check if direction changed
461         if(lastNodeIsForward != curNodeIsForward)
462         {
463             base::Trajectory tr;
464             if(lastNodeIsForward)
465                 //forward
466                 tr.speed = 1.0;
467             else
468                 tr.speed = -1.0;
469                 
470             tr.spline.interpolate(as_points);
471             
472             result.push_back(tr);
473         }
474
475         const Eigen::Affine3d body2Planner((*it)->getPose().toTransform());
476         const Eigen::Affine3d trajectory2Planner(body2Planner * body2Trajectory.inverse());
477         as_points.push_back((trajectory2Planner * Eigen::Vector3d(0,0,0)));
478
479         lastNodeIsForward = curNodeIsForward;
480     }
481
482     base::Trajectory tr;
483     if(lastNodeIsForward)
484         //forward
485         tr.speed = 1.0;
486     else
487         //backwards
488         tr.speed = -1.0;
489         
490     tr.spline.interpolate(as_points);
491     result.push_back(tr);
492
493     return result;
494 }
495
496 std::vector<base::Waypoint> Tree::buildTrajectoryTo(TreeNode const* node) const
497 {
498     int size = node->getDepth() + 1;
499     std::vector< base::Waypoint > result; 
500     result.resize(size);
501     for (int i = 0; i < size; ++i)
502     {
503         base::Pose p = node->getPose();
504         base::Waypoint& wp = result[size-1-i];
505         wp.heading  = p.getYaw();
506         wp.position = p.position;
507         wp.tol_position = node->getPositionTolerance();
508         wp.tol_heading  = node->getHeadingTolerance();
509
510         if (node->isRoot() && i != size - 1)
511             throw std::runtime_error("internal error in buildTrajectoryTo: found a root node even though the trajectory is not finished");
512         node = node->getParent();
513     }
514
515     // Small sanity check. The last node should be the root node
516     if (!node->isRoot())
517         throw std::runtime_error("internal error in buildTrajectoryTo: final node is not root");
518
519     return result;
520 }
521
522 const TreeNode* Tree::getRootNode() const
523 {
524     return root_node;
525 }
526
527 TreeNode* Tree::getRootNode()
528 {
529     return root_node;
530 }
531
532 TreeNode* Tree::getFinalNode() const
533 {
534     return final_node;
535 }
536
537 void Tree::setFinalNode(TreeNode* node)
538 {
539     final_node = node;
540 }
541
542 void Tree::reserve(int size)
543 {
544     int diff = nodes.size() - size;
545     for (int i = 0; i < diff; ++i)
546         nodes.push_back(TreeNode());
547 }
548
549 TreeNode* Tree::createNode(base::Pose const& pose, double dir)
550 {
551     TreeNode* n;
552     if (!free_nodes.empty())
553     {
554         n = free_nodes.front();
555         free_nodes.pop_front();
556     }
557     else
558     {
559         nodes.push_back(TreeNode());
560         n = &nodes.back();
561     }
562
563     n->clear();
564     n->pose = pose;
565     n->yaw = pose.getYaw();
566     n->direction = dir;
567     n->index  = size;
568     ++size;
569     return n;
570 }
571
572 TreeNode* Tree::createRoot(base::Pose const& pose, double dir)
573 {
574     if (root_node)
575         throw std::runtime_error("trying to create a root node of an non-empty tree");
576
577     root_node = createNode(pose, dir);
578     
579     return root_node;
580 }
581
582 TreeNode* Tree::createChild(TreeNode* parent, base::Pose const& pose, double dir)
583 {
584     TreeNode* child = createNode(pose, dir);
585     child->depth  = parent->depth + 1;
586     parent->addChild(child);
587     return child;
588 }
589
590 void Tree::removeNode(TreeNode* node)
591 {
592     node->childs.clear();
593     free_nodes.push_back(node);
594 }
595
596 TreeNode *Tree::getParent(TreeNode* child)
597 {
598     return child->parent;
599 }
600
601 int Tree::getSize() const
602 {
603     return size;
604 }
605
606 void Tree::clear()
607 {
608     free_nodes.clear();
609     for(std::list<TreeNode>::iterator it = nodes.begin(); it != nodes.end(); it++)
610     {
611         free_nodes.push_back(&(*it));
612     }
613     final_node = 0;
614     root_node = 0;
615     size = 0;
616 }
617
618 std::list<TreeNode> const& Tree::getNodes() const
619 {
620     return nodes;
621 }
622
623 void Tree::verifyHeuristicConsistency(const TreeNode* from) const
624 {
625     bool alright = true;
626
627     base::Position leaf_p = from->getPose().position;
628
629     const TreeNode* node = from;
630     double actual_cost = node->getCost();
631     while (!node->isRoot())
632     {
633         if (node->getHeuristicCost() > actual_cost && fabs(node->getHeuristicCost() - actual_cost) > 0.0001)
634         {
635             base::Position p = node->getPose().position;
636             std::cerr << "found invalid heuristic cost\n"
637                 << "  node at " << p.x() << " " << p.y() << " " << p.z() << " has c=" << node->getCost() << " h=" << node->getHeuristic() << " hc=" << node->getHeuristicCost() << "\n"
638                 << "  the corresponding leaf at " << leaf_p.x() << " " << leaf_p.y() << " " << leaf_p.z() << " has cost " << actual_cost << std::endl;
639             alright = false;
640         }
641         node = node->getParent();
642     }
643
644     if (!alright)
645         std::cerr << "WARN: the chosen heuristic does not seem to be a minorant" << std::endl;
646 }
647
648 NNLookup::NNLookup(double boxSize, double boxResolutionXY, double boxResolutionTheta): curSize(0), curSizeHalf(0), boxSize(boxSize), boxResolutionXY(boxResolutionXY), boxResolutionTheta(boxResolutionTheta)
649 {
650 }
651
652 NNLookup::~NNLookup()
653 {
654     for(std::list<NNLookupBox *>::const_iterator it = freeBoxes.begin(); it != freeBoxes.end(); it++)
655     {
656         delete *it;
657     }
658
659     for(std::list<NNLookupBox *>::const_iterator it = usedBoxes.begin(); it != usedBoxes.end(); it++)
660     {
661         delete *it;
662     }
663 }
664
665 void NNLookup::clear()
666 {
667     freeBoxes.splice(freeBoxes.begin(), usedBoxes);
668     for(std::vector<std::vector<NNLookupBox *> >::iterator it = globalGrid.begin(); it != globalGrid.end(); it++)
669     {
670         for(std::vector<NNLookupBox *>::iterator it2 = it->begin(); it2 != it->end(); it2++)
671             *it2 = NULL;
672     }
673     usedBoxes.clear();
674 }
675
676 bool NNLookup::getIndex(const vfh_star::TreeNode& node, int& x, int& y)
677 {
678     //calculate position in global grid
679     x = floor(node.getPosition().x() / boxSize) + curSizeHalf;
680     y = floor(node.getPosition().y() / boxSize) + curSizeHalf;
681
682     if((x < 0) || (x >= curSize) || (y < 0) || (y >= curSize))
683         return false;
684
685     return true;
686 }
687
688 TreeNode* NNLookup::getNodeWithinBounds(const vfh_star::TreeNode& node)
689 {
690     int x,y;
691     if(!getIndex(node, x, y))
692         return NULL;
693     
694     NNLookupBox *box = globalGrid[x][y];
695     if(box == NULL)
696         return NULL;
697     
698     return box->getNearestNode(node);
699 }
700
701
702 void NNLookup::clearIfSame(const vfh_star::TreeNode* node)
703 {
704     int x,y;
705     if(!getIndex(*node, x, y))
706         return;
707     
708     NNLookupBox *box = globalGrid[x][y];
709     if(box == NULL)
710         return;
711
712     box->clearIfSame(node);
713 }
714
715 void NNLookup::extendGlobalGrid(int requestedSize)
716 {
717     const int oldSize = curSize;
718     int newSize = requestedSize * 2;
719     curSize = newSize;
720     curSizeHalf = requestedSize;
721     globalGrid.resize(newSize);
722     
723     std::vector<std::vector<NNLookupBox *> >newGrid;
724     newGrid.resize(newSize);    
725     for(std::vector<std::vector<NNLookupBox *> >:: iterator it = newGrid.begin(); it != newGrid.end(); it++)
726     {
727         (*it).resize(newSize, NULL);
728     }
729     
730     const int diffHalf = (newSize - oldSize) / 2;
731     
732     for(int x = 0; x < oldSize; x++)
733     {
734         const int newX = x + diffHalf;
735         for(int y = 0; y < oldSize; y++)
736         {
737             const int newY = y + diffHalf;
738             newGrid[newX][newY] = globalGrid[x][y];
739         }
740     }
741     
742     globalGrid.swap(newGrid);
743     
744 }
745
746
747 void NNLookup::setNode(TreeNode* node)
748 {
749     int x,y;
750     if(!getIndex(*node, x, y))
751     {
752         extendGlobalGrid(std::max(abs(x),abs(y)) + 1);
753         assert(getIndex(*node, x, y));
754     }
755
756     NNLookupBox *box = globalGrid[x][y];
757     if(box == NULL)
758     {
759         const Eigen::Vector3d boxPos = Eigen::Vector3d(floor(node->getPosition().x()),
760                                                  floor(node->getPosition().y()),
761                                                 0) + Eigen::Vector3d(boxSize / 2.0, boxSize / 2.0, 0);
762                                                 
763         if(!freeBoxes.empty())
764         {
765             box = (freeBoxes.front());
766             freeBoxes.pop_front();
767             box->clear();
768             box->setNewPosition(boxPos);
769         }
770         else
771         {
772             box = new NNLookupBox(boxResolutionXY, boxResolutionTheta, boxSize, boxPos);
773         }
774         usedBoxes.push_back(box);
775         globalGrid[x][y] = box;
776     }
777     
778     box->setNode(node);
779 }
780
781
782 NNLookupBox::NNLookupBox(double resolutionXY, double angularReosultion, double size, const Eigen::Vector3d& centerPos) : resolutionXY(resolutionXY), angularResolution(angularReosultion), size(size)
783 {
784     xCells = size / resolutionXY + 1;
785     yCells = xCells;
786     aCells = M_PI / angularReosultion * 2 + 1;
787
788     hashMap.resize(xCells);
789     for(int x = 0; x < xCells; x++)
790     {
791         hashMap[x].resize(yCells);
792         for(int y = 0; y < yCells; y++)
793         {
794             hashMap[x][y].resize(aCells, NULL);
795         }
796     }
797     
798     toWorld = centerPos - Eigen::Vector3d(size/2.0, size/2.0,0);
799 }
800
801 void NNLookupBox::setNewPosition(const Eigen::Vector3d& centerPos)
802 {
803     toWorld = centerPos - Eigen::Vector3d(size/2.0, size/2.0,0);
804 }
805
806 void NNLookupBox::clear()
807 {
808     for(int x = 0; x < xCells; x++)
809     {
810         for(int y = 0; y < yCells; y++)
811         {
812             for(int a = 0; a < aCells; a++)
813             {
814                 hashMap[x][y][a] = NULL;
815             }
816         }
817     }
818 }
819
820 bool NNLookupBox::getIndixes(const vfh_star::TreeNode &node, int& x, int& y, int& a) const
821 {
822     const Eigen::Vector3d mapPos = node.getPosition() - toWorld;
823     x = mapPos.x() / resolutionXY;
824     y = mapPos.y() / resolutionXY;
825     a = node.getYaw() / angularResolution;
826     if(a < 0)
827         a+= M_PI/angularResolution * 2.0;
828     
829     assert((x >= 0) && (x < xCells) &&
830             (y >= 0) && (y < yCells) &&
831             (a >= 0) && (a < aCells));
832     
833     return true;
834 }
835
836 void NNLookupBox::clearIfSame(const vfh_star::TreeNode* node)
837 {
838     int x, y, a;
839     bool valid = getIndixes(*node, x, y, a);
840     if(!valid)
841         throw std::runtime_error("NNLookup::clearIfSame:Error, accessed node outside of lookup box");
842
843     if(hashMap[x][y][a] == node)
844         hashMap[x][y][a] = NULL;
845 }
846
847 TreeNode* NNLookupBox::getNearestNode(const vfh_star::TreeNode& node)
848 {
849     int x, y, a;
850     bool valid = getIndixes(node, x, y, a);
851     if(!valid)
852         throw std::runtime_error("NNLookup::getNearestNode:Error, accessed node outside of lookup box");
853     
854     TreeNode *ret = hashMap[x][y][a];
855     return ret;
856 }
857
858 void NNLookupBox::setNode(TreeNode* node)
859 {
860     int x, y, a;
861     bool valid = getIndixes(*node, x, y, a);
862     if(!valid)
863         throw std::runtime_error("NNLookup::setNode:Error, accessed node outside of lookup box");
864     
865     hashMap[x][y][a] = node;
866 }
867
868
869 TreeNode::TreeNode()
870 {
871     clear();
872 }
873
874 TreeNode::TreeNode(const base::Pose& pose, double dir)
875     : parent(this)
876     , is_leaf(true)
877     , pose(pose)
878     , yaw(pose.getYaw())
879     , direction(dir)
880     , cost(0)
881     , heuristic(0)
882     , depth(0)
883     , index(0)
884     , updated_cost(false)
885     , positionTolerance(0)
886     , headingTolerance(0)
887 {
888     clear();
889     direction = dir;
890     this->pose = pose;
891     yaw = pose.getYaw();
892 }
893
894 void TreeNode::clear()
895 {
896     parent = this;
897     pose = base::Pose();
898     yaw = base::unset<double>();
899     is_leaf = true;
900     cost = 0;
901     heuristic = 0;
902     depth = 0;
903     index = 0;
904     updated_cost = false;
905     positionTolerance = 0;
906     headingTolerance = 0;
907     direction = 0;
908     childs.clear();
909 }
910
911 const base::Vector3d TreeNode::getPosition() const
912 {
913     return pose.position;
914 }
915
916 double TreeNode::getYaw() const
917 {
918     return yaw;
919 }
920
921 double TreeNode::getHeuristic() const
922 {
923     return heuristic;
924 }
925
926 void TreeNode::setHeuristic(double value)
927 {
928     heuristic = value;
929 }
930
931 double TreeNode::getHeuristicCost() const
932 {
933     return cost + heuristic;
934 }
935
936 double TreeNode::getCost() const
937 {
938     return cost;
939 }
940 void TreeNode::setCost(double value)
941 {
942     cost = value;
943 }
944
945 void TreeNode::setCostFromParent(double value)
946 {
947     costFromParent = value;
948 }
949
950 double TreeNode::getCostFromParent() const
951 {
952     return costFromParent;
953 }
954
955 void TreeNode::addChild(TreeNode* child)
956 {
957     is_leaf = false;
958     child->parent = this;
959     childs.push_back(child);
960 }
961
962 void TreeNode::removeChild(TreeNode* child)
963 {
964     std::vector<TreeNode *>::iterator it = std::find(childs.begin(), childs.end(), child);
965     if(it != childs.end())
966     {
967         childs.erase(it);
968     }
969     if(childs.empty())
970         is_leaf = true;
971 }
972
973 const std::vector< TreeNode* >& TreeNode::getChildren() const
974 {
975     return childs;
976 }
977
978 const TreeNode* TreeNode::getParent() const
979 {
980     return parent;
981 }
982
983 const base::Pose& TreeNode::getPose() const
984 {
985     return pose;
986 }
987
988 int TreeNode::getDepth() const
989 {
990     return depth;
991 }
992
993 bool TreeNode::isRoot() const
994 {
995     return parent == this;
996 }
997
998 bool TreeNode::isLeaf() const
999 {
1000     return is_leaf;
1001 }
1002
1003 double TreeNode::getDirection() const
1004 {
1005     return direction;
1006 }
1007
1008 int TreeNode::getIndex() const
1009 {
1010     return index;
1011 }
1012
1013 double TreeNode::getPositionTolerance() const
1014 {
1015     return positionTolerance;
1016 }
1017 void TreeNode::setPositionTolerance(double tol)
1018 {
1019     positionTolerance = tol;
1020 }
1021 double TreeNode::getHeadingTolerance() const
1022 {
1023     return headingTolerance;
1024 }
1025 void TreeNode::setHeadingTolerance(double tol)
1026 {
1027     headingTolerance = tol;
1028 }
1029