diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index 6a51fb1..30c070d 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -162,13 +162,18 @@ void Node::HandleSubmapList( continue; } - auto fetched_texture = ::cartographer_ros::FetchSubmapTexture(id, &client_); - if (fetched_texture == nullptr) { + auto fetched_textures = + ::cartographer_ros::FetchSubmapTextures(id, &client_); + if (fetched_textures == nullptr) { continue; } + CHECK(!fetched_textures->textures.empty()); + submap_state.version = fetched_textures->version; + + // TODO(gaschler): Handle more textures than just the first one. + const auto fetched_texture = fetched_textures->textures.begin(); submap_state.width = fetched_texture->width; submap_state.height = fetched_texture->height; - submap_state.version = fetched_texture->version; submap_state.slice_pose = fetched_texture->slice_pose; submap_state.resolution = fetched_texture->resolution; diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/cartographer_ros/submap.cc index 6d5c3a2..eff5877 100644 --- a/cartographer_ros/cartographer_ros/submap.cc +++ b/cartographer_ros/cartographer_ros/submap.cc @@ -24,7 +24,7 @@ namespace cartographer_ros { -std::unique_ptr FetchSubmapTexture( +std::unique_ptr FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, ros::ServiceClient* client) { ::cartographer_ros_msgs::SubmapQuery srv; @@ -34,26 +34,29 @@ std::unique_ptr FetchSubmapTexture( return nullptr; } CHECK(!srv.response.textures.empty()); - // TODO(gaschler): Forward all the textures. - const auto& texture = srv.response.textures[0]; - std::string compressed_cells(texture.cells.begin(), texture.cells.end()); - std::string cells; - ::cartographer::common::FastGunzipString(compressed_cells, &cells); - const int num_pixels = texture.width * texture.height; - CHECK_EQ(cells.size(), 2 * num_pixels); - std::vector intensity; - intensity.reserve(num_pixels); - std::vector alpha; - alpha.reserve(num_pixels); - for (int i = 0; i < texture.height; ++i) { - for (int j = 0; j < texture.width; ++j) { - intensity.push_back(cells[(i * texture.width + j) * 2]); - alpha.push_back(cells[(i * texture.width + j) * 2 + 1]); + auto response = ::cartographer::common::make_unique(); + response->version = srv.response.submap_version; + for (const auto& texture : srv.response.textures) { + std::string compressed_cells(texture.cells.begin(), texture.cells.end()); + std::string cells; + ::cartographer::common::FastGunzipString(compressed_cells, &cells); + const int num_pixels = texture.width * texture.height; + CHECK_EQ(cells.size(), 2 * num_pixels); + std::vector intensity; + intensity.reserve(num_pixels); + std::vector alpha; + alpha.reserve(num_pixels); + for (int i = 0; i < texture.height; ++i) { + for (int j = 0; j < texture.width; ++j) { + intensity.push_back(cells[(i * texture.width + j) * 2]); + alpha.push_back(cells[(i * texture.width + j) * 2 + 1]); + } } + response->textures.emplace_back( + SubmapTexture{intensity, alpha, texture.width, texture.height, + texture.resolution, ToRigid3d(texture.slice_pose)}); } - return ::cartographer::common::make_unique(SubmapTexture{ - srv.response.submap_version, intensity, alpha, texture.width, - texture.height, texture.resolution, ToRigid3d(texture.slice_pose)}); + return response; } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/submap.h b/cartographer_ros/cartographer_ros/submap.h index 883b158..13fe291 100644 --- a/cartographer_ros/cartographer_ros/submap.h +++ b/cartographer_ros/cartographer_ros/submap.h @@ -27,7 +27,6 @@ namespace cartographer_ros { struct SubmapTexture { - int version; std::vector intensity; std::vector alpha; int width; @@ -36,9 +35,14 @@ struct SubmapTexture { ::cartographer::transform::Rigid3d slice_pose; }; +struct SubmapTextures { + int version; + std::vector textures; +}; + // Fetch 'submap_id' using the 'client' and returning the response or 'nullptr' // on error. -std::unique_ptr FetchSubmapTexture( +std::unique_ptr FetchSubmapTextures( const ::cartographer::mapping::SubmapId& submap_id, ros::ServiceClient* client); diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 9d275e8..20f3917 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -117,8 +117,8 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { ::cartographer::common::MutexLocker locker(&mutex_); // Received metadata version can also be lower if we restarted Cartographer. const bool newer_version_available = - submap_texture_ == nullptr || - submap_texture_->version != metadata_version_; + submap_textures_ == nullptr || + submap_textures_->version != metadata_version_; const std::chrono::milliseconds now = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()); @@ -130,15 +130,15 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { query_in_progress_ = true; last_query_timestamp_ = now; rpc_request_future_ = std::async(std::launch::async, [this, client]() { - std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture = - ::cartographer_ros::FetchSubmapTexture(id_, client); + std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures = + ::cartographer_ros::FetchSubmapTextures(id_, client); ::cartographer::common::MutexLocker locker(&mutex_); query_in_progress_ = false; - if (submap_texture != nullptr) { + if (submap_textures != nullptr) { // We emit a signal to update in the right thread, and pass via the // 'submap_texture_' member to simplify the signal-slot connection // slightly. - submap_texture_ = std::move(submap_texture); + submap_textures_ = std::move(submap_textures); Q_EMIT RequestSucceeded(); } }); @@ -168,7 +168,8 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) { void DrawableSubmap::UpdateSceneNode() { ::cartographer::common::MutexLocker locker(&mutex_); - ogre_submap_.Update(*submap_texture_); + // TODO(gaschler): Add UI feature to show all textures. + ogre_submap_.Update(submap_textures_->textures[0]); display_context_->queueRender(); } diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index fa82020..8461e8b 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -101,7 +101,7 @@ class DrawableSubmap : public QObject { bool query_in_progress_ = false GUARDED_BY(mutex_); int metadata_version_ = -1 GUARDED_BY(mutex_); std::future rpc_request_future_; - std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture_ + std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures_ GUARDED_BY(mutex_); float current_alpha_ = 0.f; std::unique_ptr<::rviz::BoolProperty> visibility_;