Allow multiple textures in ROS message. (#506)

Forward all textures from protobuf to ROS messages.

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -104,19 +104,21 @@ bool MapBuilderBridge::HandleSubmapQuery(
     return false;
   }
 
-  response.submap_version = response_proto.submap_version();
   CHECK(response_proto.textures_size() > 0)
       << "empty textures given for submap: " << submap_id;
 
-  // TODO(gaschler): Forward all textures, not just the first one.
-  const auto& texture_proto = *response_proto.textures().begin();
-  response.cells.insert(response.cells.begin(), texture_proto.cells().begin(),
-                        texture_proto.cells().end());
-  response.width = texture_proto.width();
-  response.height = texture_proto.height();
-  response.resolution = texture_proto.resolution();
-  response.slice_pose = ToGeometryMsgPose(
-      cartographer::transform::ToRigid3(texture_proto.slice_pose()));
+  response.submap_version = response_proto.submap_version();
+  for (const auto& texture_proto : response_proto.textures()) {
+    response.textures.emplace_back();
+    auto& texture = response.textures.back();
+    texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(),
+                         texture_proto.cells().end());
+    texture.width = texture_proto.width();
+    texture.height = texture_proto.height();
+    texture.resolution = texture_proto.resolution();
+    texture.slice_pose = ToGeometryMsgPose(
+        cartographer::transform::ToRigid3(texture_proto.slice_pose()));
+  }
   return true;
 }
 

cartographer_ros/cartographer_ros/submap.cc

@@ -33,26 +33,27 @@ std::unique_ptr<SubmapTexture> FetchSubmapTexture(
   if (!client->call(srv)) {
     return nullptr;
   }
-  std::string compressed_cells(srv.response.cells.begin(),
-                               srv.response.cells.end());
+  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 = srv.response.width * srv.response.height;
+  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 < srv.response.height; ++i) {
-    for (int j = 0; j < srv.response.width; ++j) {
-      intensity.push_back(cells[(i * srv.response.width + j) * 2]);
-      alpha.push_back(cells[(i * srv.response.width + j) * 2 + 1]);
+  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]);
     }
   }
   return ::cartographer::common::make_unique<SubmapTexture>(SubmapTexture{
-      srv.response.submap_version, intensity, alpha, srv.response.width,
-      srv.response.height, srv.response.resolution,
-      ToRigid3d(srv.response.slice_pose)});
+      srv.response.submap_version, intensity, alpha, texture.width,
+      texture.height, texture.resolution, ToRigid3d(texture.slice_pose)});
 }
 
 }  // namespace cartographer_ros

cartographer_ros_msgs/CMakeLists.txt

@@ -26,6 +26,7 @@ add_message_files(
   FILES
     SubmapList.msg
     SubmapEntry.msg
+    SubmapTexture.msg
     SensorTopics.msg
     TrajectoryOptions.msg
 )

cartographer_ros_msgs/msg/SubmapTexture.msg

@@ -0,0 +1,19 @@
+# Copyright 2017 The Cartographer Authors
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+uint8[] cells
+int32 width
+int32 height
+float64 resolution
+geometry_msgs/Pose slice_pose

cartographer_ros_msgs/srv/SubmapQuery.srv

@@ -16,9 +16,5 @@ int32 trajectory_id
 int32 submap_index
 ---
 int32 submap_version
-uint8[] cells
-int32 width
-int32 height
-float64 resolution
-geometry_msgs/Pose slice_pose
+SubmapTexture[] textures
 string error_message