cartographer_ros: submap.cc Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer_ros/submap.h"
00018 
00019 #include "absl/memory/memory.h"
00020 #include "cartographer/common/port.h"
00021 #include "cartographer/transform/transform.h"
00022 #include "cartographer_ros/msg_conversion.h"
00023 #include "cartographer_ros_msgs/StatusCode.h"
00024 #include "cartographer_ros_msgs/SubmapQuery.h"
00025 
00026 namespace cartographer_ros {
00027 
00028 std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
00029     const ::cartographer::mapping::SubmapId& submap_id,
00030     ros::ServiceClient* client) {
00031   ::cartographer_ros_msgs::SubmapQuery srv;
00032   srv.request.trajectory_id = submap_id.trajectory_id;
00033   srv.request.submap_index = submap_id.submap_index;
00034   if (!client->call(srv) ||
00035       srv.response.status.code != ::cartographer_ros_msgs::StatusCode::OK) {
00036     return nullptr;
00037   }
00038   if (srv.response.textures.empty()) {
00039     return nullptr;
00040   }
00041   auto response = absl::make_unique<::cartographer::io::SubmapTextures>();
00042   response->version = srv.response.submap_version;
00043   for (const auto& texture : srv.response.textures) {
00044     const std::string compressed_cells(texture.cells.begin(),
00045                                        texture.cells.end());
00046     response->textures.emplace_back(::cartographer::io::SubmapTexture{
00047         ::cartographer::io::UnpackTextureData(compressed_cells, texture.width,
00048                                               texture.height),
00049         texture.width, texture.height, texture.resolution,
00050         ToRigid3d(texture.slice_pose)});
00051   }
00052   return response;
00053 }
00054 
00055 }  // namespace cartographer_ros