Adapt to changes in cartographer and write PLY files on exporting. (#128)

Also use reflectivity in the 'PointsBatch'es created in 'WriteAssets'.
In the online case, we throw away reflectivities during SLAMing to save
on memory, so there will be none.

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -1,3 +1,11 @@
+google_library(assets_writer
+  USES_CARTOGRAPHER
+  SRCS
+    assets_writer.cc
+  HDRS
+    assets_writer.h
+)
+
 google_library(map_writer
   USES_GLOG
   USES_ROS
@@ -87,14 +95,6 @@ google_library(time_conversion
     time_conversion.h
 )
 
-google_library(xray
-  USES_CARTOGRAPHER
-  SRCS
-    xray.cc
-  HDRS
-    xray.h
-)
-
 google_catkin_test(time_conversion_test
   USES_CARTOGRAPHER
   USES_ROS
@@ -113,6 +113,7 @@ google_binary(cartographer_node
   SRCS
     node_main.cc
   DEPENDS
+    assets_writer
     map_writer
     msg_conversion
     node_options
@@ -121,7 +122,6 @@ google_binary(cartographer_node
     sensor_bridge
     tf_bridge
     time_conversion
-    xray
 )
 
 install(TARGETS cartographer_node

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -14,9 +14,11 @@
  * limitations under the License.
  */
 
-#include "cartographer_ros/xray.h"
+#include "cartographer_ros/assets_writer.h"
 
+#include "cartographer/common/make_unique.h"
 #include "cartographer/io/null_points_processor.h"
+#include "cartographer/io/ply_writing_points_processor.h"
 #include "cartographer/io/points_processor.h"
 #include "cartographer/io/xray_points_processor.h"
 
@@ -24,9 +26,9 @@ namespace cartographer_ros {
 
 namespace carto = ::cartographer;
 
-void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
-                         trajectory_nodes,
-                     const double voxel_size, const std::string& stem) {
+void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
+                     trajectory_nodes,
+                 const double voxel_size, const std::string& stem) {
   carto::io::NullPointsProcessor null_points_processor;
   carto::io::XRayPointsProcessor xy_xray_points_processor(
       voxel_size, carto::transform::Rigid3f::Rotation(
@@ -40,18 +42,25 @@ void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
       voxel_size, carto::transform::Rigid3f::Rotation(
                       Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
       stem + "_xray_xz.png", &yz_xray_points_processor);
+  carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
+      stem + ".ply", &xz_xray_points_processor);
 
   for (const auto& node : trajectory_nodes) {
     const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan(
         carto::sensor::Decompress(node.constant_data->laser_fan_3d),
         node.pose.cast<float>());
 
-    carto::io::PointsBatch points_batch;
-    points_batch.origin = laser_fan.origin;
-    points_batch.points = laser_fan.returns;
-    xz_xray_points_processor.Process(points_batch);
+    auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
+    points_batch->origin = laser_fan.origin;
+    points_batch->points = laser_fan.returns;
+    for (const uint8 reflectivity :
+         node.constant_data->laser_fan_3d.reflectivities) {
+      points_batch->colors.push_back(
+          carto::io::Color{{reflectivity, reflectivity, reflectivity}});
+    }
+    ply_writing_points_processor.Process(std::move(points_batch));
   }
-  xz_xray_points_processor.Flush();
+  ply_writing_points_processor.Flush();
 }
 
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/assets_writer.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_ROS_XRAY_H_
-#define CARTOGRAPHER_ROS_XRAY_H_
+#ifndef CARTOGRAPHER_ROS_ASSETS_WRITER_H_
+#define CARTOGRAPHER_ROS_ASSETS_WRITER_H_
 
 #include <string>
 #include <vector>
@@ -24,12 +24,12 @@
 
 namespace cartographer_ros {
 
-// Writes X-ray images from the 'trajectory_nodes'. The filenames will all start
-// with 'stem'.
-void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
-                         trajectory_nodes,
-                     double voxel_size, const std::string& stem);
+// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames
+// will all start with 'stem'.
+void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
+                     trajectory_nodes,
+                 double voxel_size, const std::string& stem);
 
 }  // namespace cartographer_ros
 
-#endif  // CARTOGRAPHER_ROS_XRAY_H_
+#endif  // CARTOGRAPHER_ROS_ASSETS_WRITER_H_

cartographer_ros/cartographer_ros/node_main.cc

@@ -47,6 +47,7 @@
 #include "cartographer/sensor/proto/sensor.pb.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
+#include "cartographer_ros/assets_writer.h"
 #include "cartographer_ros/map_writer.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/node_options.h"
@@ -55,7 +56,6 @@
 #include "cartographer_ros/sensor_bridge.h"
 #include "cartographer_ros/tf_bridge.h"
 #include "cartographer_ros/time_conversion.h"
-#include "cartographer_ros/xray.h"
 #include "cartographer_ros_msgs/FinishTrajectory.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapList.h"
@@ -378,11 +378,11 @@ bool Node::HandleFinishTrajectory(
   }
 
   if (options_.map_builder_options.use_trajectory_builder_3d()) {
-    WriteXRayImages(trajectory_nodes,
-                    options_.map_builder_options.trajectory_builder_3d_options()
-                        .submaps_options()
-                        .high_resolution(),
-                    request.stem);
+    WriteAssets(trajectory_nodes,
+                options_.map_builder_options.trajectory_builder_3d_options()
+                    .submaps_options()
+                    .high_resolution(),
+                request.stem);
   }
   return true;
 }