Fix a bug with costmap_queue getNextCell (#25)

Carl Delsey · DLu · commit c24cfa9461bc · 2019-01-09T17:35:11.000-05:00
diff --git a/costmap_queue/src/costmap_queue.cpp b/costmap_queue/src/costmap_queue.cpp
@@ -76,6 +76,7 @@ CellData CostmapQueue::getNextCell()
 {
   // get the highest priority cell and pop it off the priority queue
   CellData current_cell = front();
+  pop();
 
   unsigned int mx = current_cell.x_;
   unsigned int my = current_cell.y_;
@@ -92,8 +93,6 @@ CellData CostmapQueue::getNextCell()
   if (my < costmap_.getHeight() - 1)
     enqueueCell(mx, my + 1, sx, sy);
 
-  // pop once we have our cell info
-  pop();
   return current_cell;
 }
 

Original file line number	Diff line number	Diff line change
`@@ -76,6 +76,7 @@ CellData CostmapQueue::getNextCell()`
`76`	`76`	`{`
`77`	`77`	`// get the highest priority cell and pop it off the priority queue`
`78`	`78`	`CellData current_cell = front();`
	`79`	`+ pop();`
`79`	`80`
`80`	`81`	`unsigned int mx = current_cell.x_;`
`81`	`82`	`unsigned int my = current_cell.y_;`
`@@ -92,8 +93,6 @@ CellData CostmapQueue::getNextCell()`
`92`	`93`	`if (my < costmap_.getHeight() - 1)`
`93`	`94`	`enqueueCell(mx, my + 1, sx, sy);`
`94`	`95`
`95`		`- // pop once we have our cell info`
`96`		`- pop();`
`97`	`96`	`return current_cell;`
`98`	`97`	`}`
`99`	`98`