| 21 | } |
| 22 | |
| 23 | void MultiMapManager::init() |
| 24 | { |
| 25 | node_.param("exploration/drone_id", drone_id_, 1); |
| 26 | node_.param("exploration/vis_drone_id", vis_drone_id_, -1); |
| 27 | node_.param("exploration/drone_num", map_num_, 2); |
| 28 | node_.param("multi_map_manager/chunk_size", chunk_size_, 200); |
| 29 | local_updated_ = false; |
| 30 | esdf_need_update_ = false; |
| 31 | drone_id_ += 1; |
| 32 | // ROS_ERROR("DDDrone_id %d",drone_id_); |
| 33 | stamp_timer_ = node_.createTimer(ros::Duration(0.1), &MultiMapManager::stampTimerCallback, this); |
| 34 | chunk_timer_ = node_.createTimer(ros::Duration(0.1), &MultiMapManager::chunkTimerCallback, this); |
| 35 | |
| 36 | stamp_pub_ = node_.advertise<plan_env::ChunkStamps>("/multi_map_manager/chunk_stamps_send", 10); |
| 37 | chunk_pub_ = node_.advertise<plan_env::ChunkData>("/multi_map_manager/chunk_data_send", 5000); |
| 38 | marker_pub_ = node_.advertise<visualization_msgs::Marker>( |
| 39 | "/multi_map_manager/marker_" + std::to_string(drone_id_), 10); |
| 40 | |
| 41 | stamp_sub_ = node_.subscribe( |
| 42 | "/multi_map_manager/chunk_stamps_recv", 10, &MultiMapManager::stampMsgCallback, this); |
| 43 | chunk_sub_ = node_.subscribe("/multi_map_manager/chunk_data_recv", 5000, |
| 44 | &MultiMapManager::chunkCallback, this, ros::TransportHints().tcpNoDelay()); |
| 45 | |
| 46 | multi_map_chunks_.resize(map_num_); |
| 47 | for (auto& data : multi_map_chunks_) { |
| 48 | data.idx_list_ = {}; |
| 49 | } |
| 50 | chunk_boxes_.resize(map_num_); |
| 51 | for (auto& box : chunk_boxes_) { |
| 52 | box.valid_ = false; |
| 53 | } |
| 54 | chunk_buffer_.resize(map_num_); |
| 55 | buffer_map_.resize(map_num_); |
| 56 | last_chunk_stamp_time_.resize(map_num_); |
| 57 | for (auto time : last_chunk_stamp_time_) time = 0.0; |
| 58 | |
| 59 | // // Test the idx list operation |
| 60 | |
| 61 | // // Find missed |
| 62 | // vector<int> self1 = { 1, 1000 }; |
| 63 | // vector<int> self2 = { 5, 100, 105, 105, 120, 300, 400, 600, 700, 1000 }; |
| 64 | // vector<vector<int>> selfs = { self1, self2 }; |
| 65 | |
| 66 | // vector<int> other1 = {}; |
| 67 | // vector<int> other2 = { 10, 80 }; |
| 68 | // vector<int> other3 = { 10, 80, 150, 200, 501, 550 }; |
| 69 | // vector<vector<int>> others = { other1, other2, other3 }; |
| 70 | |
| 71 | // for (auto sf : selfs) { |
| 72 | // for (auto ot : others) { |
| 73 | // vector<int> missed; |
| 74 | // findMissedChunkIds(sf, ot, missed); |
| 75 | // std::cout << "Missed result: "; |
| 76 | // for (auto id : missed) std::cout << id << ", "; |
| 77 | // std::cout << "" << std::endl; |
| 78 | // } |
| 79 | // } |
| 80 | |