/** * @brief Given an index of a cell in the costmap, place it into a priority queue 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 index of the obstacle point inflation started at * @param src_y The y index of the obstacle point inflation started at */ inline void InflationLayer::enqueue(unsigned char* grid, unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y) { //set the cost of the cell being inserted 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 queue if it is within the inflation radius of the obstacle point if (distance > cell_inflation_radius_) return; //assign the cost associated with the distance from an obstacle to the cell unsigned char cost = costLookup(mx, my, src_x, src_y); unsigned char old_cost = grid[index]; if (old_cost == NO_INFORMATION && cost >= INSCRIBED_INFLATED_OBSTACLE) grid[index] = cost; else grid[index] = std::max(old_cost, cost); //push the cell data onto the queue and mark seen_[index] = true; CellData data(distance, index, mx, my, src_x, src_y); inflation_queue_.push(data); } }
/** * @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 index of the obstacle point inflation started at * @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)); } }