From 35389d0adb5f98edad6f05ed95ef79cf550a1163 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Fri, 7 Jul 2017 17:46:59 +0200
Subject: [PATCH] Follow googlecartographer/cartographer#396. (#417)

---
 .../cartographer_ros/assets_writer.cc           | 17 ++++++++++-------
 .../cartographer_ros/occupancy_grid.cc          | 11 +++++------
 .../cartographer_ros/occupancy_grid_test.cc     |  9 ++++-----
 3 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc
index ffb700d..0d4d9ad 100644
--- a/cartographer_ros/cartographer_ros/assets_writer.cc
+++ b/cartographer_ros/cartographer_ros/assets_writer.cc
@@ -67,16 +67,19 @@ void Write3DAssets(
 
   carto::io::NullPointsProcessor null_points_processor;
   carto::io::XRayPointsProcessor xy_xray_points_processor(
-      voxel_size, carto::transform::Rigid3f::Rotation(
-                      Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
+      voxel_size,
+      carto::transform::Rigid3f::Rotation(
+          Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
       {}, stem + "_xray_xy", file_writer_factory, &null_points_processor);
   carto::io::XRayPointsProcessor yz_xray_points_processor(
-      voxel_size, carto::transform::Rigid3f::Rotation(
-                      Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
+      voxel_size,
+      carto::transform::Rigid3f::Rotation(
+          Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
       {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor);
   carto::io::XRayPointsProcessor xz_xray_points_processor(
-      voxel_size, carto::transform::Rigid3f::Rotation(
-                      Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
+      voxel_size,
+      carto::transform::Rigid3f::Rotation(
+          Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
       {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor);
   carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
       file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
@@ -86,7 +89,7 @@ void Write3DAssets(
     for (const auto& node : all_trajectory_nodes[trajectory_id]) {
       const carto::sensor::RangeData range_data =
           carto::sensor::TransformRangeData(
-              carto::sensor::Decompress(node.constant_data->range_data_3d),
+              carto::sensor::Decompress(node.constant_data->range_data),
               node.pose.cast<float>());
       auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
       points_batch->time = node.time();
diff --git a/cartographer_ros/cartographer_ros/occupancy_grid.cc b/cartographer_ros/cartographer_ros/occupancy_grid.cc
index 8d6fc53..00d8513 100644
--- a/cartographer_ros/cartographer_ros/occupancy_grid.cc
+++ b/cartographer_ros/cartographer_ros/occupancy_grid.cc
@@ -36,7 +36,7 @@ Eigen::AlignedBox2f ComputeMapBoundingBox2D(
       const auto& data = *node.constant_data;
       ::cartographer::sensor::RangeData range_data;
       range_data = ::cartographer::sensor::TransformRangeData(
-          data.range_data_2d, node.pose.cast<float>());
+          Decompress(data.range_data), node.pose.cast<float>());
       bounding_box.extend(range_data.origin.head<2>());
       for (const Eigen::Vector3f& hit : range_data.returns) {
         bounding_box.extend(hit.head<2>());
@@ -72,11 +72,10 @@ void BuildOccupancyGrid2D(
         continue;
       }
       latest_time = std::max(latest_time, node.time());
-      CHECK(node.constant_data->range_data_3d.returns.empty());
-      range_data_inserter.Insert(
-          carto::sensor::TransformRangeData(node.constant_data->range_data_2d,
-                                            node.pose.cast<float>()),
-          &probability_grid);
+      range_data_inserter.Insert(carto::sensor::TransformRangeData(
+                                     Decompress(node.constant_data->range_data),
+                                     node.pose.cast<float>()),
+                                 &probability_grid);
     }
   }
   CHECK(latest_time != carto::common::Time::min());
diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc
index 665ae77..70bc104 100644
--- a/cartographer_ros/cartographer_ros/occupancy_grid_test.cc
+++ b/cartographer_ros/cartographer_ros/occupancy_grid_test.cc
@@ -31,12 +31,11 @@ TEST(OccupancyGridTest, ComputeMapLimits) {
   const TrajectoryNode trajectory_node{
       std::make_shared<TrajectoryNode::Data>(TrajectoryNode::Data{
           ::cartographer::common::FromUniversal(52),
-          ::cartographer::sensor::RangeData{Eigen::Vector3f::Zero(),
-                                            {Eigen::Vector3f(-30.f, 1.f, 0.f),
-                                             Eigen::Vector3f(50.f, -10.f, 0.f)},
-                                            {}},
           ::cartographer::sensor::Compress(::cartographer::sensor::RangeData{
-              Eigen::Vector3f::Zero(), {}, {}}),
+              Eigen::Vector3f::Zero(),
+              {Eigen::Vector3f(-30.f, 1.f, 0.f),
+               Eigen::Vector3f(50.f, -10.f, 0.f)},
+              {}}),
           ::cartographer::transform::Rigid3d::Identity()}),
       ::cartographer::transform::Rigid3d::Identity()};
   constexpr double kResolution = 0.05;