From b8d63f3cc906c73fcfb23f71fd15555a8c411d65 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 13 Sep 2017 16:43:25 +0200 Subject: [PATCH] Handle multiple textures in protobuf. (#503) Handle the protobuf change from googlecartographer/cartographer#519 by forwarding the first SubmapTexture if multiple are available. * Nit. * Check. * Nit. --- .../cartographer_ros/map_builder_bridge.cc | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index c054355..c56578a 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -95,23 +95,28 @@ bool MapBuilderBridge::HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response) { cartographer::mapping::proto::SubmapQuery::Response response_proto; - const std::string error = map_builder_.SubmapToProto( - cartographer::mapping::SubmapId{request.trajectory_id, - request.submap_index}, - &response_proto); + cartographer::mapping::SubmapId submap_id{request.trajectory_id, + request.submap_index}; + const std::string error = + map_builder_.SubmapToProto(submap_id, &response_proto); if (!error.empty()) { LOG(ERROR) << error; return false; } response.submap_version = response_proto.submap_version(); - response.cells.insert(response.cells.begin(), response_proto.cells().begin(), - response_proto.cells().end()); - response.width = response_proto.width(); - response.height = response_proto.height(); - response.resolution = response_proto.resolution(); + 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(response_proto.slice_pose())); + cartographer::transform::ToRigid3(texture_proto.slice_pose())); return true; }