FetchSubmapTextures returns multiple textures. (#519)

This changes the Ros api that multiple textures will be returned,
which are forwarded from the Ros service call.
Adapts all usages to follow this new api.

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;
 

cartographer_ros/cartographer_ros/submap.cc

@@ -24,7 +24,7 @@
 
 namespace cartographer_ros {
 
-std::unique_ptr<SubmapTexture> FetchSubmapTexture(
+std::unique_ptr<SubmapTextures> FetchSubmapTextures(
     const ::cartographer::mapping::SubmapId& submap_id,
     ros::ServiceClient* client) {
   ::cartographer_ros_msgs::SubmapQuery srv;
@@ -34,26 +34,29 @@ std::unique_ptr<SubmapTexture> 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<char> intensity;
-  intensity.reserve(num_pixels);
-  std::vector<char> 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<SubmapTextures>();
+  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<char> intensity;
+    intensity.reserve(num_pixels);
+    std::vector<char> 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>(SubmapTexture{
-      srv.response.submap_version, intensity, alpha, texture.width,
-      texture.height, texture.resolution, ToRigid3d(texture.slice_pose)});
+  return response;
 }
 
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/submap.h

@@ -27,7 +27,6 @@
 namespace cartographer_ros {
 
 struct SubmapTexture {
-  int version;
   std::vector<char> intensity;
   std::vector<char> alpha;
   int width;
@@ -36,9 +35,14 @@ struct SubmapTexture {
   ::cartographer::transform::Rigid3d slice_pose;
 };
 
+struct SubmapTextures {
+  int version;
+  std::vector<SubmapTexture> textures;
+};
+
 // Fetch 'submap_id' using the 'client' and returning the response or 'nullptr'
 // on error.
-std::unique_ptr<SubmapTexture> FetchSubmapTexture(
+std::unique_ptr<SubmapTextures> FetchSubmapTextures(
     const ::cartographer::mapping::SubmapId& submap_id,
     ros::ServiceClient* client);
 

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::milliseconds>(
           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();
 }
 

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<void> 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_;