Allow multiple textures in ROS message. (#506)

Forward all textures from protobuf to ROS messages.
master
gaschler 2017-10-04 05:44:49 -07:00 committed by Wolfgang Hess
parent 2a9e392023
commit e02e634848
5 changed files with 44 additions and 25 deletions

View File

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

View File

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

View File

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

View File

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

View File

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