Refactoring towards a pbstream -> PGM/YAML tool. (#601)

cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc

@@ -137,33 +137,11 @@ void Node::HandleSubmapList(
     submap_slice.height = fetched_texture->height;
     submap_slice.slice_pose = fetched_texture->slice_pose;
     submap_slice.resolution = fetched_texture->resolution;
-
-    // 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 * submap_slice.width;
-    CHECK_EQ(expected_stride,
-             cairo_format_stride_for_width(::cartographer::io::kCairoFormat,
-                                           submap_slice.width));
     submap_slice.cairo_data.clear();
-    for (size_t i = 0; i < fetched_texture->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 = fetched_texture->intensity.at(i);
-      const uint8_t alpha = fetched_texture->alpha.at(i);
-      const uint8_t observed = (intensity == 0 && alpha == 0) ? 0 : 255;
-      submap_slice.cairo_data.push_back((alpha << 24) | (intensity << 16) |
-                                        (observed << 8) | 0);
-    }
-
-    submap_slice.surface = ::cartographer::io::MakeUniqueCairoSurfacePtr(
-        cairo_image_surface_create_for_data(
-            reinterpret_cast<unsigned char*>(submap_slice.cairo_data.data()),
-            ::cartographer::io::kCairoFormat, submap_slice.width,
-            submap_slice.height, expected_stride));
-    CHECK_EQ(cairo_surface_status(submap_slice.surface.get()),
-             CAIRO_STATUS_SUCCESS)
-        << cairo_status_to_string(
-               cairo_surface_status(submap_slice.surface.get()));
+    submap_slice.surface =
+        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/submap.cc

@@ -24,6 +24,55 @@
 
 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(
     const ::cartographer::mapping::SubmapId& submap_id,
     ros::ServiceClient* client) {
@@ -37,24 +86,12 @@ std::unique_ptr<SubmapTextures> FetchSubmapTextures(
   auto response = ::cartographer::common::make_unique<SubmapTextures>();
   response->version = srv.response.submap_version;
   for (const auto& texture : srv.response.textures) {
-    std::string compressed_cells(texture.cells.begin(), texture.cells.end());
-    std::string cells;
-    ::cartographer::common::FastGunzipString(compressed_cells, &cells);
-    const int num_pixels = texture.width * texture.height;
-    CHECK_EQ(cells.size(), 2 * num_pixels);
-    std::vector<char> intensity;
-    intensity.reserve(num_pixels);
-    std::vector<char> alpha;
-    alpha.reserve(num_pixels);
-    for (int i = 0; i < texture.height; ++i) {
-      for (int j = 0; j < texture.width; ++j) {
-        intensity.push_back(cells[(i * texture.width + j) * 2]);
-        alpha.push_back(cells[(i * texture.width + j) * 2 + 1]);
-      }
-    }
-    response->textures.emplace_back(
-        SubmapTexture{intensity, alpha, texture.width, texture.height,
-                      texture.resolution, ToRigid3d(texture.slice_pose)});
+    const std::string compressed_cells(texture.cells.begin(),
+                                       texture.cells.end());
+    response->textures.emplace_back(SubmapTexture{
+        UnpackTextureData(compressed_cells, texture.width, texture.height),
+        texture.width, texture.height, texture.resolution,
+        ToRigid3d(texture.slice_pose)});
   }
   return response;
 }

cartographer_ros/cartographer_ros/submap.h

@@ -18,8 +18,10 @@
 #define CARTOGRAPHER_ROS_SUBMAP_H_
 
 #include <memory>
+#include <string>
 #include <vector>
 
+#include "cartographer/io/image.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "ros/ros.h"
@@ -27,8 +29,11 @@
 namespace cartographer_ros {
 
 struct SubmapTexture {
+  struct Pixels {
     std::vector<char> intensity;
     std::vector<char> alpha;
+  };
+  Pixels pixels;
   int width;
   int height;
   double resolution;
@@ -46,6 +51,16 @@ std::unique_ptr<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/ogre_slice.cc

@@ -88,10 +88,11 @@ void OgreSlice::Update(
   // The call to Ogre's loadRawData below does not work with an RG texture,
   // therefore we create an RGB one whose blue channel is always 0.
   std::vector<char> rgb;
-  CHECK_EQ(submap_texture.intensity.size(), submap_texture.alpha.size());
-  for (size_t i = 0; i < submap_texture.intensity.size(); ++i) {
-    rgb.push_back(submap_texture.intensity[i]);
-    rgb.push_back(submap_texture.alpha[i]);
+  CHECK_EQ(submap_texture.pixels.intensity.size(),
+           submap_texture.pixels.alpha.size());
+  for (size_t i = 0; i < submap_texture.pixels.intensity.size(); ++i) {
+    rgb.push_back(submap_texture.pixels.intensity[i]);
+    rgb.push_back(submap_texture.pixels.alpha[i]);
     rgb.push_back(0);
   }