| 348 | } |
| 349 | |
| 350 | void MultiMapManager::chunkTimerCallback(const ros::TimerEvent& e) |
| 351 | { |
| 352 | if (drone_id_ == 1) |
| 353 | return; |
| 354 | // Not process chunk until swarm basecoor transform is available |
| 355 | Eigen::Vector4d tmp; |
| 356 | // if (!map_->getBaseCoor(1, tmp)) { |
| 357 | // ROS_WARN("basecoor not available yet."); |
| 358 | // return; |
| 359 | // } |
| 360 | |
| 361 | // auto t1 = ros::Time::now(); |
| 362 | |
| 363 | // Process chunks in buffers |
| 364 | for (int i = 0; i < chunk_buffer_.size(); ++i) { |
| 365 | auto& buffer = chunk_buffer_[i]; |
| 366 | if (buffer.empty()) |
| 367 | continue; |
| 368 | |
| 369 | // Compute the idx list of buffered chunks |
| 370 | sort(buffer.begin(), buffer.end(), |
| 371 | [](const plan_env::ChunkData& chunk1, const plan_env::ChunkData& chunk2) { |
| 372 | return chunk1.idx < chunk2.idx; |
| 373 | }); |
| 374 | vector<int> idx_list = { int(buffer.front().idx) }; |
| 375 | int last_idx = idx_list[0]; |
| 376 | for (int j = 1; j < buffer.size(); ++j) { |
| 377 | if (buffer[j].idx - last_idx > 1) { |
| 378 | idx_list.push_back(last_idx); |
| 379 | idx_list.push_back(buffer[j].idx); |
| 380 | } |
| 381 | last_idx = buffer[j].idx; |
| 382 | } |
| 383 | idx_list.push_back(last_idx); |
| 384 | |
| 385 | // std::cout << "process drone " << i + 1 << "'s chunks, input idx list: "; |
| 386 | // for (auto id : idx_list) std::cout << id << ", "; |
| 387 | // std::cout << "" << std::endl; |
| 388 | |
| 389 | // Update ChunksData's chunks_ |
| 390 | auto& chunks_data = multi_map_chunks_[i]; |
| 391 | |
| 392 | // std::cout << "self idx list " << i + 1 << ": "; |
| 393 | // for (auto id : chunks_data.idx_list_) std::cout << id << ", "; |
| 394 | // std::cout << "" << std::endl; |
| 395 | |
| 396 | // Add placeholder for chunks |
| 397 | int len_inc = last_idx - chunks_data.chunks_.size(); |
| 398 | for (int j = 0; j < len_inc; ++j) { |
| 399 | chunks_data.chunks_.push_back(MapChunk()); |
| 400 | auto& back_chunk = chunks_data.chunks_.back(); |
| 401 | back_chunk.idx_ = chunks_data.chunks_.size(); |
| 402 | back_chunk.empty_ = true; |
| 403 | back_chunk.need_query_ = false; |
| 404 | } |
| 405 | |
| 406 | // Process data in buffer |
| 407 | for (auto msg : buffer) { |