Write the trajectory on exporting. (#134)

And some refactorings around writing assets.

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -1,9 +1,14 @@
 google_library(assets_writer
   USES_CARTOGRAPHER
+  USES_ROS
   SRCS
     assets_writer.cc
   HDRS
     assets_writer.h
+  DEPENDS
+    map_writer
+    node_options
+    occupancy_grid
 )
 
 google_library(map_writer
@@ -114,7 +119,6 @@ google_binary(cartographer_node
     node_main.cc
   DEPENDS
     assets_writer
-    map_writer
     msg_conversion
     node_options
     occupancy_grid

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -21,14 +21,32 @@
 #include "cartographer/io/ply_writing_points_processor.h"
 #include "cartographer/io/points_processor.h"
 #include "cartographer/io/xray_points_processor.h"
+#include "cartographer/mapping_2d/proto/laser_fan_inserter_options.pb.h"
+#include "cartographer/proto/trajectory.pb.h"
+#include "cartographer_ros/map_writer.h"
+#include "cartographer_ros/occupancy_grid.h"
+#include "nav_msgs/OccupancyGrid.h"
 
 namespace cartographer_ros {
 
+namespace {
+
 namespace carto = ::cartographer;
 
-void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
+// Writes an occupany grid.
-                     trajectory_nodes,
+void Write2DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
-                 const double voxel_size, const std::string& stem) {
+                       trajectory_nodes,
+                   const NodeOptions& options, const std::string& stem) {
+  ::nav_msgs::OccupancyGrid occupancy_grid;
+  BuildOccupancyGrid(trajectory_nodes, options, &occupancy_grid);
+  WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem);
+}
+
+// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames
+// will all start with 'stem'.
+void Write3DAssets(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(
@@ -63,4 +81,32 @@ void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
   ply_writing_points_processor.Flush();
 }
 
+}  // namespace
+
+void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
+                     trajectory_nodes,
+                 const NodeOptions& options, const std::string& stem) {
+  // Write the trajectory.
+  std::ofstream proto_file(stem + ".pb",
+                           std::ios_base::out | std::ios_base::binary);
+  const carto::proto::Trajectory trajectory =
+      carto::mapping::ToProto(trajectory_nodes);
+  CHECK(trajectory.SerializeToOstream(&proto_file))
+      << "Could not serialize trajectory.";
+  proto_file.close();
+  CHECK(proto_file) << "Could not write trajectory.";
+
+  if (options.map_builder_options.use_trajectory_builder_2d()) {
+    Write2DAssets(trajectory_nodes, options, stem);
+  }
+
+  if (options.map_builder_options.use_trajectory_builder_3d()) {
+    Write3DAssets(trajectory_nodes,
+                  options.map_builder_options.trajectory_builder_3d_options()
+                      .submaps_options()
+                      .high_resolution(),
+                  stem);
+  }
+}
+
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/assets_writer.h

@@ -21,14 +21,14 @@
 #include <vector>
 
 #include "cartographer/mapping/trajectory_node.h"
+#include "cartographer_ros/node_options.h"
 
 namespace cartographer_ros {
 
-// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames
+// Writes all assets derivable from the 'trajectory_nodes' and 'options'.
-// will all start with 'stem'.
 void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
                      trajectory_nodes,
-                 double voxel_size, const std::string& stem);
+                 const NodeOptions& options, const std::string& stem);
 
 }  // namespace cartographer_ros
 

cartographer_ros/cartographer_ros/node_main.cc

@@ -47,7 +47,6 @@
 #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"
 #include "cartographer_ros/occupancy_grid.h"
@@ -357,20 +356,7 @@ bool Node::HandleFinishTrajectory(
     LOG(WARNING) << "Map is empty and will not be saved.";
     return true;
   }
-
+  WriteAssets(trajectory_nodes, options_, request.stem);
-  if (options_.map_builder_options.use_trajectory_builder_2d()) {
-    ::nav_msgs::OccupancyGrid occupancy_grid;
-    BuildOccupancyGrid(trajectory_nodes, options_, &occupancy_grid);
-    WriteOccupancyGridToPgmAndYaml(occupancy_grid, request.stem);
-  }
-
-  if (options_.map_builder_options.use_trajectory_builder_3d()) {
-    WriteAssets(trajectory_nodes,
-                options_.map_builder_options.trajectory_builder_3d_options()
-                    .submaps_options()
-                    .high_resolution(),
-                request.stem);
-  }
   return true;
 }