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.
master
gaschler 2017-09-13 16:43:25 +02:00 committed by Wolfgang Hess
parent 44b9e7b531
commit b8d63f3cc9
1 changed files with 15 additions and 10 deletions

View File

@ -95,23 +95,28 @@ bool MapBuilderBridge::HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response) { cartographer_ros_msgs::SubmapQuery::Response& response) {
cartographer::mapping::proto::SubmapQuery::Response response_proto; cartographer::mapping::proto::SubmapQuery::Response response_proto;
const std::string error = map_builder_.SubmapToProto( cartographer::mapping::SubmapId submap_id{request.trajectory_id,
cartographer::mapping::SubmapId{request.trajectory_id, request.submap_index};
request.submap_index}, const std::string error =
&response_proto); map_builder_.SubmapToProto(submap_id, &response_proto);
if (!error.empty()) { if (!error.empty()) {
LOG(ERROR) << error; LOG(ERROR) << error;
return false; return false;
} }
response.submap_version = response_proto.submap_version(); response.submap_version = response_proto.submap_version();
response.cells.insert(response.cells.begin(), response_proto.cells().begin(), CHECK(response_proto.textures_size() > 0)
response_proto.cells().end()); << "empty textures given for submap: " << submap_id;
response.width = response_proto.width();
response.height = response_proto.height(); // TODO(gaschler): Forward all textures, not just the first one.
response.resolution = response_proto.resolution(); 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( response.slice_pose = ToGeometryMsgPose(
cartographer::transform::ToRigid3(response_proto.slice_pose())); cartographer::transform::ToRigid3(texture_proto.slice_pose()));
return true; return true;
} }