Method Enqueue

roborts_costmap/src/inflation_layer.cpp:250–263

* @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation * @param grid The costmap * @param index The index of the cell * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it) * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it) * @param src_x The x

 * @param  src_y The y index of the obstacle point inflation started at
 */
inline void InflationLayer::Enqueue(unsigned int index, unsigned int mx, unsigned int my,
                                    unsigned int src_x, unsigned int src_y) {
  if (!seen_[index]) {
    // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
    double distance = DistanceLookup(mx, my, src_x, src_y);

    // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
    if (distance > cell_inflation_radius_)
      return;

    // push the cell data onto the inflation list and mark
    inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
  }
}

void InflationLayer::ComputeCaches() {
  if (cell_inflation_radius_ == 0)