Commit 646b8365522c9dfe08322430d8828ab6ba86200f

Authored by Antonio Carlos Domínguez Brito
1 parent 9d03b731
Exists in master

NFPlanner's function members added, to find initially the skeleton, using best f…

…irst planning algorithm at the beginning of the NF2 algorithm pass
Showing 2 changed files with 163 additions and 18 deletions   Show diff stats
planner/nf-planner.cpp
... ... @@ -102,6 +102,15 @@ namespace PlannerSpace
102 102 throw NoMemory(stringStream.str());
103 103 }
104 104  
  105 + auto_ptr<_DTreeHeap_> dTreeHeap(new _DTreeHeap_(_ALLOCATION_BLOCK_));
  106 + if(!dTreeHeap.get())
  107 + {
  108 + ostringstream stringStream;
  109 + stringStream<<"PlannerSpace::NFPlanner::NFPlanner(): No memory\n\t"
  110 + <<"\t"<<__FILE__<<":"<<__LINE__<<endl;
  111 + throw NoMemory(stringStream.str());
  112 + }
  113 +
105 114 auto_ptr<_DHeap_> dHeap(new _DHeap_(_ALLOCATION_BLOCK_));
106 115 if(!dHeap.get())
107 116 {
... ... @@ -132,6 +141,7 @@ namespace PlannerSpace
132 141 freeNodes.release(_freeNodes_);
133 142 freePathPoints.release(_freePathPoints_);
134 143  
  144 + _pDTreeHeap_=dTreeHeap.release();
135 145 _pDHeap_=dHeap.release();
136 146 _pUHeap_=uHeap.release();
137 147 }
... ... @@ -211,19 +221,32 @@ namespace PlannerSpace
211 221  
212 222 _skeletonMainPass_(limits,gridMap,list,passingThreshold);
213 223  
214   - if(!_nf2InitialPass_(limits,gridMap,robot2D))
  224 + //if(!_nf2InitialPass_(limits,gridMap,robot2D))
  225 + if(!_nf2InitialPassBFP_(limits,gridMap,robot2D))
215 226 {
216 227 cout<<"Ups!. Skeleton not found near the robot."<<endl;
217 228 return false;
218 229 }
219 230  
220   - if(!_nf2InitialPass_(limits,gridMap,goal2D))
  231 + //if(!_nf2InitialPass_(limits,gridMap,goal2D))
  232 + if(!_nf2InitialPassBFP_(limits,gridMap,goal2D))
221 233 {
222 234 cout<<"Ups!. Skeleton not found near the goal."<<endl;
223 235 return false;
224 236 }
225 237  
226   -// _nf2MainPass_(limits,gridMap,goal2D,list);
  238 + // _nf2MainPass_: computes the potential U in the skeleton
  239 + // following Latombe's (Chapter 7, p. 325). It promotes paths
  240 + // farther from obstacles, but not necessarily shorter
  241 +
  242 + //_nf2MainPass_(limits,gridMap,goal2D,list);
  243 +
  244 + // _nf2MainPassBis_: computes the potential U in the skeleton
  245 + // not taking into account the distance to obstacles, instead
  246 + // it only takes into account neighborhood. To accomplish that
  247 + // it does not classify skeleton points in a heap, just uses
  248 + // a list where neighbors get appended at the end. It achieves
  249 + // shorter and more "natural" paths that Latombe's proposal
227 250 _nf2MainPassBis_(limits,gridMap,goal2D,list);
228 251  
229 252 _nf2FinalPass_(limits,gridMap,list);
... ... @@ -469,6 +492,105 @@ namespace PlannerSpace
469 492 }
470 493 }
471 494  
  495 + bool NFPlanner::_nf2InitialPassBFP_(
  496 + const Region2D<int>& limits,
  497 + const GridMapPacket& gridMap,
  498 + Point2D<int>& goal
  499 + )
  500 + {
  501 + _BFPNode_* pRoot=_getNode_(_freeNodes_);
  502 + pRoot->set(goal);
  503 +
  504 + _DTreeHeapItem_ item;
  505 + item.set(
  506 + (*_pDGrid_)[goal.y()][goal.x()],
  507 + pRoot
  508 + );
  509 + _pushHeapItem_(*_pDTreeHeap_,item);
  510 +
  511 + Point2D<int> q,qp;
  512 + _BFPNode_* pQ;
  513 + _BFPNode_* pLast=NULL;
  514 +
  515 + _pVGrid_->set(false);
  516 +
  517 + while( _pDTreeHeap_->items() && !pLast )
  518 + {
  519 + _pDTreeHeap_->pop(item);
  520 + pQ=item.data();
  521 + q=pQ->get();
  522 +
  523 + qp.set(q.x(),q.y()-1); // up neighbour
  524 + if(pLast=_nf2InitialNeighbourPassBFP_(limits,gridMap,qp,pQ)) continue;
  525 +
  526 + qp.set(q.x()+1,q.y()-1); // up-right neighbour
  527 + if(pLast=_nf2InitialNeighbourPassBFP_(limits,gridMap,qp,pQ)) continue;
  528 +
  529 + qp.set(q.x()+1,q.y()); // right neighbour
  530 + if(pLast=_nf2InitialNeighbourPassBFP_(limits,gridMap,qp,pQ)) continue;
  531 +
  532 + qp.set(q.x()+1,q.y()+1); // down-right neighbour
  533 + if(pLast=_nf2InitialNeighbourPassBFP_(limits,gridMap,qp,pQ)) continue;
  534 +
  535 + qp.set(q.x(),q.y()+1); // down neighbour
  536 + if(pLast=_nf2InitialNeighbourPassBFP_(limits,gridMap,qp,pQ)) continue;
  537 +
  538 + qp.set(q.x()-1,q.y()+1); // down-left neighbour
  539 + if(pLast=_nf2InitialNeighbourPassBFP_(limits,gridMap,qp,pQ)) continue;
  540 +
  541 + qp.set(q.x()-1,q.y()); // left neighbour
  542 + if(pLast=_nf2InitialNeighbourPassBFP_(limits,gridMap,qp,pQ)) continue;
  543 +
  544 + qp.set(q.x()-1,q.y()-1); // up-left neighbour
  545 + if(pLast=_nf2InitialNeighbourPassBFP_(limits,gridMap,qp,pQ)) continue;
  546 + }
  547 +
  548 + if(pLast)
  549 + {
  550 + for(pQ=pLast; pQ; pQ=static_cast<_BFPNode_*>(pQ->getParent()))
  551 + { q=pQ->get(); (*_pSGrid_)[q.y()][q.x()]=true; }
  552 + }
  553 +
  554 + _pDTreeHeap_->clear();
  555 + _releaseTree_(pRoot);
  556 +
  557 + return (pLast!=NULL);
  558 + }
  559 +
  560 + inline NFPlanner::_BFPNode_* NFPlanner::_nf2InitialNeighbourPassBFP_(
  561 + const Region2D<int>& limits,
  562 + const GridMapPacket& gridMap,
  563 + Point2D<int>& neighbour,
  564 + _BFPNode_* pParent
  565 + )
  566 + {
  567 + if(
  568 + limits.contains(neighbour) &&
  569 + ((*gridMap.grid())[neighbour.y()][neighbour.x()]!=GridMapPacket::CELL_OCCUPIED) &&
  570 + !((*_pVGrid_)[neighbour.y()][neighbour.x()]) &&
  571 + ((*_pDGrid_)[neighbour.y()][neighbour.x()]!=numeric_limits<DCellType>::max())
  572 + )
  573 + {
  574 + _BFPNode_* pNode=_getNode_(_freeNodes_);
  575 + pNode->set(neighbour);
  576 + pNode->setParent(pParent);
  577 + pParent->pushNode(pNode);
  578 +
  579 + _DTreeHeapItem_ item;
  580 + item.set(
  581 + (*_pDGrid_)[neighbour.y()][neighbour.x()],
  582 + pNode
  583 + );
  584 + _pushHeapItem_(*_pDTreeHeap_,item);
  585 +
  586 + (*_pVGrid_)[neighbour.y()][neighbour.x()]=true;
  587 +
  588 + if((*_pSGrid_)[neighbour.y()][neighbour.x()]) { return pNode; }
  589 + }
  590 +
  591 + return NULL;
  592 + }
  593 +
472 594 bool NFPlanner::_nf2InitialPass_(
473 595 const Region2D<int>& limits,
474 596 const GridMapPacket& gridMap,
... ... @@ -543,10 +665,10 @@ namespace PlannerSpace
543 665 }
544 666  
545 667 inline void NFPlanner::_nf2InitialNeighbourPass_(
546   - const Region2D<int>& limits,
547   - Point2D<int>& neighbour,
548   - Point2D<int>& qp,
549   - DCellType& max
  668 + const Region2D<int>& limits,
  669 + Point2D<int>& neighbour,
  670 + Point2D<int>& qp,
  671 + DCellType& max
550 672 )
551 673 {
552 674 if(
... ...
planner/nf-planner.h
... ... @@ -147,13 +147,6 @@ namespace PlannerSpace
147 147 _SPLINE_SEGMENT_=4 // usually in robot diameters
148 148 };
149 149  
150   - typedef HeapItem<
151   - DCellType,
152   - Point2D<int>
153   - > _DHeapItem_;
154   -
155   - typedef Heap<_DHeapItem_> _DHeap_;
156   -
157 150 class _BFPNode_:
158 151 public ListNodeInterface<_BFPNode_>,
159 152 public TreeNode<
... ... @@ -161,8 +154,22 @@ namespace PlannerSpace
161 154 _CELL_NEIGHBOURS_
162 155 >
163 156 { /* nothing */ };
  157 +
  158 + typedef HeapItem<
  159 + DCellType,
  160 + _BFPNode_*
  161 + > _DTreeHeapItem_;
  162 +
  163 + typedef Heap<_DTreeHeapItem_> _DTreeHeap_;
164 164  
165 165 typedef HeapItem<
  166 + DCellType,
  167 + Point2D<int>
  168 + > _DHeapItem_;
  169 +
  170 + typedef Heap<_DHeapItem_> _DHeap_;
  171 +
  172 + typedef HeapItem<
166 173 UCellType,
167 174 _BFPNode_*
168 175 > _UHeapItem_;
... ... @@ -182,6 +189,7 @@ namespace PlannerSpace
182 189  
183 190 FastList<Point2DNode> _freePoints_;
184 191  
  192 + _DTreeHeap_* _pDTreeHeap_;
185 193 _DHeap_* _pDHeap_;
186 194  
187 195 _UHeap_* _pUHeap_;
... ... @@ -202,7 +210,8 @@ namespace PlannerSpace
202 210 _pDistGrid_=NULL;
203 211 _pVGrid_=NULL;
204 212  
205   - _pDHeap_=NULL;
  213 + _pDTreeHeap_=NULL;
  214 + _pDHeap_=NULL;
206 215 _pUHeap_=NULL;
207 216 }
208 217  
... ... @@ -220,6 +229,7 @@ namespace PlannerSpace
220 229 _releaseNodes_(_freeNodes_);
221 230 _releaseNodes_(_freePathPoints_);
222 231  
  232 + auto_ptr<_DTreeHeap_> dTreeHeap(_pDTreeHeap_);
223 233 auto_ptr<_DHeap_> dHeap(_pDHeap_);
224 234 auto_ptr<_UHeap_> uHeap(_pUHeap_);
225 235 }
... ... @@ -328,9 +338,9 @@ namespace PlannerSpace
328 338 );
329 339  
330 340 bool _nf2InitialPass_(
331   - const Region2D<int>& limits,
332   - const GridMapPacket& gridMap,
333   - Point2D<int>& goal
  341 + const Region2D<int>& limits,
  342 + const GridMapPacket& gridMap,
  343 + Point2D<int>& goal
334 344 );
335 345  
336 346 void _nf2InitialNeighbourPass_(
... ... @@ -340,6 +350,19 @@ namespace PlannerSpace
340 350 DCellType& max
341 351 );
342 352  
  353 + bool _nf2InitialPassBFP_(
  354 + const Region2D<int>& limits,
  355 + const GridMapPacket& gridMap,
  356 + Point2D<int>& goal
  357 + );
  358 +
  359 + _BFPNode_* _nf2InitialNeighbourPassBFP_(
  360 + const Region2D<int>& limits,
  361 + const GridMapPacket& gridMap,
  362 + Point2D<int>& neighbour,
  363 + _BFPNode_* pParent
  364 + );
  365 +
343 366 void _nf2MainPass_(
344 367 const Region2D<int>& limits,
345 368 const GridMapPacket& gridMap,
... ...