search

locusrobotics / robot_navigation

Spiritual successor to ros-planning/navigation.
450 stars 148 forks source link

Fix costmap queue losing enqueued cells #25

Closed crdelsey closed 5 years ago

crdelsey commented 5 years ago

In the CostmapQueue::getNextCell method, the head of the queue is referenced, adjacent cells are enqueued and then the head of the queue is popped. However, enqueuing the adjacent cells can change the head of the queue, resulting in a newly added cell getting popped instead of the cell that was just processed.

When this happens, the newly added cell will never get pulled from the queue and it's adjacent cells may never get expanded.

This will happen whenever a newly added cell has a lower cost than the head of the queue. Normally, this would never happen with a CostmapQueue, but can happen in MapGridQueue as cells are expanded behind obstacles as seen in ros-planning/navigation2/issues/451

DLu commented 5 years ago

Confirmed. I'll commit a test later that proves this bug. Thanks for the attention to detail.