void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned char* master_array = master_grid.getCharMap(); unsigned int span = master_grid.getSizeInCellsX(); for (int j = min_j; j < max_j; j++) { unsigned int it = j * span + min_i; for (int i = min_i; i < max_i; i++) { if (costmap_[it] == NO_INFORMATION){ it++; continue; } unsigned char old_cost = master_array[it]; if (old_cost == NO_INFORMATION || old_cost < costmap_[it]) master_array[it] = costmap_[it]; it++; } } }
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned char* master_array = master_grid.getCharMap(); unsigned int span = master_grid.getSizeInCellsX(); for (int j = min_j; j < max_j; j++) { unsigned int it = j * span + min_i; for (int i = min_i; i < max_i; i++) { if (costmap_[it] == NO_INFORMATION){ it++; continue; } unsigned char old_cost = master_array[it]; if (old_cost == NO_INFORMATION) master_array[it] = costmap_[it]; else { int sum = old_cost + costmap_[it]; if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; else master_array[it] = sum; } it++; } } }
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned char* master = master_grid.getCharMap(); unsigned int span = master_grid.getSizeInCellsX(); for (int j = min_j; j < max_j; j++) { unsigned int it = span*j+min_i; for (int i = min_i; i < max_i; i++) { if (costmap_[it] != NO_INFORMATION) master[it] = costmap_[it]; it++; } } }
void BallPickerLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; const unsigned char* master_array = master_grid.getCharMap(); for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { if (clear_flag) { master_grid.setCost(i, j, FREE_SPACE); } else { int index = getIndex(i, j); if (costmap_[index] == NO_INFORMATION) continue; unsigned char old_cost = master_array[index]; if (old_cost == NO_INFORMATION || old_cost < costmap_[index]) master_grid.setCost(i, j, costmap_[index]); } } } for (int i=0; i < getSizeInCellsX()*getSizeInCellsY(); i++) costmap_[i] == NO_INFORMATION; if (!obstacle_buffer.empty()) { ROS_INFO("Costmap updated with ball obstacles."); obstacle_buffer.clear(); std_srvs::Empty emptysrv; confirm_update_client.call(emptysrv); } }
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { boost::unique_lock < boost::shared_mutex > lock(*access_); if (!enabled_) return; //make sure the inflation queue is empty at the beginning of the cycle (should always be true) ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation"); unsigned char* master_array = master_grid.getCharMap(); unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); memset(seen_, false, size_x * size_y * sizeof(bool)); // We need to include in the inflation cells outside the bounding // box min_i...max_j, by the amount cell_inflation_radius_. Cells // up to that distance outside the box can still influence the costs // stored in cells inside the box. min_i -= cell_inflation_radius_; min_j -= cell_inflation_radius_; max_i += cell_inflation_radius_; max_j += cell_inflation_radius_; min_i = std::max( 0, min_i ); min_j = std::max( 0, min_j ); max_i = std::min( int( size_x ), max_i ); max_j = std::min( int( size_y ), max_j ); for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { int index = master_grid.getIndex(i, j); unsigned char cost = master_array[index]; if (cost == LETHAL_OBSTACLE) { enqueue(master_array, index, i, j, i, j); } } } while (!inflation_queue_.empty()) { //get the highest priority cell and pop it off the priority queue const CellData& current_cell = inflation_queue_.top(); unsigned int index = current_cell.index_; unsigned int mx = current_cell.x_; unsigned int my = current_cell.y_; unsigned int sx = current_cell.src_x_; unsigned int sy = current_cell.src_y_; //attempt to put the neighbors of the current cell onto the queue if (mx > 0) enqueue(master_array, index - 1, mx - 1, my, sx, sy); if (my > 0) enqueue(master_array, index - size_x, mx, my - 1, sx, sy); if (mx < size_x - 1) enqueue(master_array, index + 1, mx + 1, my, sx, sy); if (my < size_y - 1) enqueue(master_array, index + size_x, mx, my + 1, sx, sy); //remove the current cell from the priority queue inflation_queue_.pop(); } }
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); if (!enabled_ || (cell_inflation_radius_ == 0)) return; // make sure the inflation list is empty at the beginning of the cycle (should always be true) ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation"); unsigned char* master_array = master_grid.getCharMap(); unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); if (seen_ == NULL) { ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL"); seen_size_ = size_x * size_y; seen_ = new bool[seen_size_]; } else if (seen_size_ != size_x * size_y) { ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong"); delete[] seen_; seen_size_ = size_x * size_y; seen_ = new bool[seen_size_]; } memset(seen_, false, size_x * size_y * sizeof(bool)); // We need to include in the inflation cells outside the bounding // box min_i...max_j, by the amount cell_inflation_radius_. Cells // up to that distance outside the box can still influence the costs // stored in cells inside the box. min_i -= cell_inflation_radius_; min_j -= cell_inflation_radius_; max_i += cell_inflation_radius_; max_j += cell_inflation_radius_; min_i = std::max(0, min_i); min_j = std::max(0, min_j); max_i = std::min(int(size_x), max_i); max_j = std::min(int(size_y), max_j); // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle // We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost // Start with lethal obstacles: by definition distance is 0.0 std::vector<CellData>& obs_bin = inflation_cells_[0.0]; for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { int index = master_grid.getIndex(i, j); unsigned char cost = master_array[index]; if (cost == LETHAL_OBSTACLE) { obs_bin.push_back(CellData(index, i, j, i, j)); } } } // Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they // can overtake previously inserted but farther away cells std::map<double, std::vector<CellData> >::iterator bin; for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) { for (int i = 0; i < bin->second.size(); ++i) { // process all cells at distance dist_bin.first const CellData& cell = bin->second[i]; unsigned int index = cell.index_; // ignore if already visited if (seen_[index]) { continue; } seen_[index] = true; unsigned int mx = cell.x_; unsigned int my = cell.y_; unsigned int sx = cell.src_x_; unsigned int sy = cell.src_y_; // assign the cost associated with the distance from an obstacle to the cell unsigned char cost = costLookup(mx, my, sx, sy); unsigned char old_cost = master_array[index]; if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE))) master_array[index] = cost; else master_array[index] = std::max(old_cost, cost); // attempt to put the neighbors of the current cell onto the inflation list if (mx > 0) enqueue(index - 1, mx - 1, my, sx, sy); if (my > 0) enqueue(index - size_x, mx, my - 1, sx, sy); if (mx < size_x - 1) enqueue(index + 1, mx + 1, my, sx, sy); if (my < size_y - 1) enqueue(index + size_x, mx, my + 1, sx, sy); } } inflation_cells_.clear(); }