Follow googlecartographer/cartographer#782 (#633)

Towards [RFC06](https://github.com/googlecartographer/rfcs/blob/master/text/0006-serve-ros-map-from-pbstream.md).

Migrates
* `FillSubmapSlice` from `pbstream_to_rosmap_main.cc` 
* `SubmapTexture` logics from cartographer_ros

cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc

@@ -138,10 +138,10 @@ void Node::HandleSubmapList(
     submap_slice.slice_pose = fetched_texture->slice_pose;
     submap_slice.resolution = fetched_texture->resolution;
     submap_slice.cairo_data.clear();
-    submap_slice.surface =
-        DrawTexture(fetched_texture->pixels.intensity,
-                    fetched_texture->pixels.alpha, fetched_texture->width,
-                    fetched_texture->height, &submap_slice.cairo_data);
+    submap_slice.surface = ::cartographer::io::DrawTexture(
+        fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,
+        fetched_texture->width, fetched_texture->height,
+        &submap_slice.cairo_data);
   }
 
   // Delete all submaps that didn't appear in the message.

cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc

@@ -39,36 +39,6 @@ DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
 namespace cartographer_ros {
 namespace {
 
-void FillSubmapSlice(
-    const ::cartographer::transform::Rigid3d& global_submap_pose,
-    const ::cartographer::mapping::proto::Submap& proto,
-    ::cartographer::io::SubmapSlice* const submap_slice) {
-  ::cartographer::mapping::proto::SubmapQuery::Response response;
-  ::cartographer::transform::Rigid3d local_pose;
-  if (proto.has_submap_3d()) {
-    ::cartographer::mapping_3d::Submap submap(proto.submap_3d());
-    local_pose = submap.local_pose();
-    submap.ToResponseProto(global_submap_pose, &response);
-  } else {
-    ::cartographer::mapping_2d::Submap submap(proto.submap_2d());
-    local_pose = submap.local_pose();
-    submap.ToResponseProto(global_submap_pose, &response);
-  }
-  submap_slice->pose = global_submap_pose;
-
-  auto& texture_proto = response.textures(0);
-  const SubmapTexture::Pixels pixels = UnpackTextureData(
-      texture_proto.cells(), texture_proto.width(), texture_proto.height());
-  submap_slice->width = texture_proto.width();
-  submap_slice->height = texture_proto.height();
-  submap_slice->resolution = texture_proto.resolution();
-  submap_slice->slice_pose =
-      ::cartographer::transform::ToRigid3(texture_proto.slice_pose());
-  submap_slice->surface =
-      DrawTexture(pixels.intensity, pixels.alpha, texture_proto.width(),
-                  texture_proto.height(), &submap_slice->cairo_data);
-}
-
 void Run(const std::string& pbstream_filename, const std::string& map_filestem,
          const double resolution) {
   ::cartographer::io::ProtoStreamReader reader(pbstream_filename);

cartographer_ros/cartographer_ros/submap.cc

@@ -24,56 +24,7 @@
 
 namespace cartographer_ros {
 
-SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells,
-                                        const int width, const int height) {
-  SubmapTexture::Pixels pixels;
-  std::string cells;
-  ::cartographer::common::FastGunzipString(compressed_cells, &cells);
-  const int num_pixels = width * height;
-  CHECK_EQ(cells.size(), 2 * num_pixels);
-  pixels.intensity.reserve(num_pixels);
-  pixels.alpha.reserve(num_pixels);
-  for (int i = 0; i < height; ++i) {
-    for (int j = 0; j < width; ++j) {
-      pixels.intensity.push_back(cells[(i * width + j) * 2]);
-      pixels.alpha.push_back(cells[(i * width + j) * 2 + 1]);
-    }
-  }
-  return pixels;
-}
-
-::cartographer::io::UniqueCairoSurfacePtr DrawTexture(
-    const std::vector<char>& intensity, const std::vector<char>& alpha,
-    const int width, const int height,
-    std::vector<uint32_t>* const cairo_data) {
-  CHECK(cairo_data->empty());
-
-  // Properly dealing with a non-common stride would make this code much more
-  // complicated. Let's check that it is not needed.
-  const int expected_stride = 4 * width;
-  CHECK_EQ(expected_stride, cairo_format_stride_for_width(
-                                ::cartographer::io::kCairoFormat, width));
-  for (size_t i = 0; i < intensity.size(); ++i) {
-    // We use the red channel to track intensity information. The green
-    // channel we use to track if a cell was ever observed.
-    const uint8_t intensity_value = intensity.at(i);
-    const uint8_t alpha_value = alpha.at(i);
-    const uint8_t observed =
-        (intensity_value == 0 && alpha_value == 0) ? 0 : 255;
-    cairo_data->push_back((alpha_value << 24) | (intensity_value << 16) |
-                          (observed << 8) | 0);
-  }
-
-  auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr(
-      cairo_image_surface_create_for_data(
-          reinterpret_cast<unsigned char*>(cairo_data->data()),
-          ::cartographer::io::kCairoFormat, width, height, expected_stride));
-  CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS)
-      << cairo_status_to_string(cairo_surface_status(surface.get()));
-  return surface;
-}
-
-std::unique_ptr<SubmapTextures> FetchSubmapTextures(
+std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
     const ::cartographer::mapping::SubmapId& submap_id,
     ros::ServiceClient* client) {
   ::cartographer_ros_msgs::SubmapQuery srv;
@@ -83,13 +34,15 @@ std::unique_ptr<SubmapTextures> FetchSubmapTextures(
     return nullptr;
   }
   CHECK(!srv.response.textures.empty());
-  auto response = ::cartographer::common::make_unique<SubmapTextures>();
+  auto response =
+      ::cartographer::common::make_unique<::cartographer::io::SubmapTextures>();
   response->version = srv.response.submap_version;
   for (const auto& texture : srv.response.textures) {
     const std::string compressed_cells(texture.cells.begin(),
                                        texture.cells.end());
-    response->textures.emplace_back(SubmapTexture{
-        UnpackTextureData(compressed_cells, texture.width, texture.height),
+    response->textures.emplace_back(::cartographer::io::SubmapTexture{
+        ::cartographer::io::UnpackTextureData(compressed_cells, texture.width,
+                                              texture.height),
         texture.width, texture.height, texture.resolution,
         ToRigid3d(texture.slice_pose)});
   }

cartographer_ros/cartographer_ros/submap.h

@@ -22,45 +22,19 @@
 #include <vector>
 
 #include "cartographer/io/image.h"
+#include "cartographer/io/submap_painter.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "ros/ros.h"
 
 namespace cartographer_ros {
 
-struct SubmapTexture {
-  struct Pixels {
-    std::vector<char> intensity;
-    std::vector<char> alpha;
-  };
-  Pixels pixels;
-  int width;
-  int height;
-  double resolution;
-  ::cartographer::transform::Rigid3d slice_pose;
-};
-
-struct SubmapTextures {
-  int version;
-  std::vector<SubmapTexture> textures;
-};
-
 // Fetch 'submap_id' using the 'client' and returning the response or 'nullptr'
 // on error.
-std::unique_ptr<SubmapTextures> FetchSubmapTextures(
+std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
     const ::cartographer::mapping::SubmapId& submap_id,
     ros::ServiceClient* client);
 
-// Unpacks cell data as provided by the backend into 'intensity' and 'alpha'.
-SubmapTexture::Pixels UnpackTextureData(const std::string& compressed_cells,
-                                        int width, int height);
-
-// Draw a texture into a cairo surface. 'cairo_data' will store the pixel data
-// for the surface and must therefore outlive the use of the surface.
-::cartographer::io::UniqueCairoSurfacePtr DrawTexture(
-    const std::vector<char>& intensity, const std::vector<char>& alpha,
-    int width, int height, std::vector<uint32_t>* cairo_data);
-
 }  // namespace cartographer_ros
 
 #endif  // CARTOGRAPHER_ROS_SUBMAP_H_

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -134,7 +134,7 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
   query_in_progress_ = true;
   last_query_timestamp_ = now;
   rpc_request_future_ = std::async(std::launch::async, [this, client]() {
-    std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures =
+    std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures =
         ::cartographer_ros::FetchSubmapTextures(id_, client);
     ::cartographer::common::MutexLocker locker(&mutex_);
     query_in_progress_ = false;

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -25,6 +25,7 @@
 #include "OgreSceneManager.h"
 #include "OgreSceneNode.h"
 #include "cartographer/common/mutex.h"
+#include "cartographer/io/submap_painter.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer_ros/submap.h"
@@ -105,7 +106,7 @@ class DrawableSubmap : public QObject {
   bool query_in_progress_ = false GUARDED_BY(mutex_);
   int metadata_version_ = -1 GUARDED_BY(mutex_);
   std::future<void> rpc_request_future_;
-  std::unique_ptr<::cartographer_ros::SubmapTextures> submap_textures_
+  std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures_
       GUARDED_BY(mutex_);
   float current_alpha_ = 0.f;
   std::unique_ptr<::rviz::BoolProperty> visibility_;

cartographer_rviz/cartographer_rviz/ogre_slice.cc

@@ -82,7 +82,7 @@ OgreSlice::~OgreSlice() {
 }
 
 void OgreSlice::Update(
-    const ::cartographer_ros::SubmapTexture& submap_texture) {
+    const ::cartographer::io::SubmapTexture& submap_texture) {
   slice_node_->setPosition(ToOgre(submap_texture.slice_pose.translation()));
   slice_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation()));
   // The call to Ogre's loadRawData below does not work with an RG texture,

cartographer_rviz/cartographer_rviz/ogre_slice.h

@@ -26,8 +26,8 @@
 #include "OgreSceneNode.h"
 #include "OgreTexture.h"
 #include "OgreVector3.h"
+#include "cartographer/io/submap_painter.h"
 #include "cartographer/mapping/id.h"
-#include "cartographer_ros/submap.h"
 
 namespace cartographer_rviz {
 
@@ -50,7 +50,7 @@ class OgreSlice {
 
   // Updates the texture and pose of the submap using new data from
   // 'submap_texture'.
-  void Update(const ::cartographer_ros::SubmapTexture& submap_texture);
+  void Update(const ::cartographer::io::SubmapTexture& submap_texture);
 
   // Changes the opacity of the submap to 'alpha'.
   void SetAlpha(float alpha);