12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -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;
}