diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index c56578a..624c144 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/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; } diff --git a/cartographer_ros/cartographer_ros/submap.cc b/cartographer_ros/cartographer_ros/submap.cc index ff79a3d..6d5c3a2 100644 --- a/cartographer_ros/cartographer_ros/submap.cc +++ b/cartographer_ros/cartographer_ros/submap.cc @@ -33,26 +33,27 @@ std::unique_ptr 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 intensity; intensity.reserve(num_pixels); std::vector 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{ - 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 diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index 2f36e2f..2cb8bd0 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -26,6 +26,7 @@ add_message_files( FILES SubmapList.msg SubmapEntry.msg + SubmapTexture.msg SensorTopics.msg TrajectoryOptions.msg ) diff --git a/cartographer_ros_msgs/msg/SubmapTexture.msg b/cartographer_ros_msgs/msg/SubmapTexture.msg new file mode 100644 index 0000000..31a2e91 --- /dev/null +++ b/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 diff --git a/cartographer_ros_msgs/srv/SubmapQuery.srv b/cartographer_ros_msgs/srv/SubmapQuery.srv index f57bdda..0f40dcc 100644 --- a/cartographer_ros_msgs/srv/SubmapQuery.srv +++ b/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