| 99 | } |
| 100 | |
| 101 | void MultiMapManager::updateMapChunk(const vector<uint32_t>& adrs) |
| 102 | { |
| 103 | adr_buffer_.insert(adr_buffer_.end(), adrs.begin(), adrs.end()); |
| 104 | |
| 105 | if (adr_buffer_.size() >= chunk_size_) { |
| 106 | // Insert chunk from too long buffer |
| 107 | int i = 0; |
| 108 | for (; i + chunk_size_ < adr_buffer_.size(); i += chunk_size_) { |
| 109 | MapChunk chunk; |
| 110 | chunk.voxel_adrs_.insert( |
| 111 | chunk.voxel_adrs_.end(), adr_buffer_.begin() + i, adr_buffer_.begin() + i + chunk_size_); |
| 112 | chunk.idx_ = multi_map_chunks_[drone_id_ - 1].chunks_.size() + 1; |
| 113 | // if (drone_id_ == 1) std::cout << "Drone 1 insert chunk " << chunk.idx_ << std::endl; |
| 114 | chunk.need_query_ = true; |
| 115 | chunk.empty_ = false; |
| 116 | multi_map_chunks_[drone_id_ - 1].chunks_.push_back(chunk); |
| 117 | } |
| 118 | if (multi_map_chunks_[drone_id_ - 1].idx_list_.empty()) { |
| 119 | multi_map_chunks_[drone_id_ - 1].idx_list_ = { 1, 1 }; |
| 120 | } |
| 121 | multi_map_chunks_[drone_id_ - 1].idx_list_.back() = |
| 122 | multi_map_chunks_[drone_id_ - 1].chunks_.back().idx_; |
| 123 | |
| 124 | // std::cout << "idx list: "; |
| 125 | // for (auto id : multi_map_chunks_[drone_id_ - 1].idx_list_) std::cout << id << ", "; |
| 126 | // std::cout << "" << std::endl; |
| 127 | // std::cout << "chunk size: " << multi_map_chunks_[drone_id_ - 1].chunks_.size() << |
| 128 | // std::endl; |
| 129 | |
| 130 | // Remove already inserted data |
| 131 | vector<uint32_t> tmp; |
| 132 | tmp.insert(tmp.end(), adr_buffer_.begin() + i, adr_buffer_.end()); |
| 133 | adr_buffer_ = tmp; |
| 134 | } |
| 135 | } |
| 136 | |
| 137 | void MultiMapManager::stampTimerCallback(const ros::TimerEvent& e) |
| 138 | { |