Method updateGridData

swarm_exploration/active_perception/src/hgrid.cpp:71–155

}

void HGrid::updateGridData(const int& drone_id, vector<int>& grid_ids, bool reallocated,
    const vector<int>& last_grid_ids, vector<int>& first_ids, vector<int>& second_ids) {

  // Convert grid_ids to the ids of bi-level uniform grid
  vector<int> grid_ids1, grid_ids2;
  const int grid_num1 = grid1_->grid_data_.size();
  for (auto id : grid_ids) {
    if (id < grid_num1)
      grid_ids1.push_back(id);
    else
      grid_ids2.push_back(id - grid_num1);  // Id of level 2 grid
  }

  // std::cout << "Input ids: ";
  // for (auto id : grid_ids)
  //   std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // std::cout << "level 1 ids: ";
  // for (auto id : grid_ids1)
  //   std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // std::cout << "level 2 ids: ";
  // for (auto id : grid_ids2)
  //   std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // Update at level 1
  vector<int> tmp_ids1 = grid_ids1;
  vector<int> parti_ids1, parti_ids1_all;
  grid1_->updateGridData(drone_id, grid_ids1, parti_ids1, parti_ids1_all);

  // std::cout << "updated level 1 ids: ";
  // for (auto id : grid_ids1)
  //   std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // std::cout << "divided level 1 ids: ";
  // for (auto id : parti_ids1)
  //   std::cout << id << ", ";
  // std::cout << "" << std::endl;

  // Merge the newly partitioned and original grid ids
  vector<int> fine_ids;
  for (auto id : parti_ids1) {
    vector<int> tmp_ids;
    coarseToFineId(id, tmp_ids);
    fine_ids.insert(fine_ids.end(), tmp_ids.begin(), tmp_ids.end());
  }
  grid_ids2.insert(grid_ids2.end(), fine_ids.begin(), fine_ids.end());

  // Activate newly divided grids
  vector<int> fine_ids_all;
  for (auto id : parti_ids1_all) {
    vector<int> tmp_ids;
    coarseToFineId(id, tmp_ids);
    fine_ids_all.insert(fine_ids_all.end(), tmp_ids.begin(), tmp_ids.end());