Track Cartographer changes. (#329)

Change googlecartographer/cartographer#248 introduced a separation
between MapBuilder and TrajectoryOptions. The ROS repositiories require
changes to track this modification.

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -35,12 +35,41 @@ namespace {
 
 namespace carto = ::cartographer;
 
-// Writes an occupancy grid.
+void WriteTrajectory(const std::vector<::cartographer::mapping::TrajectoryNode>&
-void Write2DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
                          trajectory_nodes,
-                   const NodeOptions& options, const std::string& stem) {
+                     const std::string& stem) {
+  carto::mapping::proto::Trajectory trajectory;
+  for (const auto& node : trajectory_nodes) {
+    const auto& data = *node.constant_data;
+    auto* node_proto = trajectory.add_node();
+    node_proto->set_timestamp(carto::common::ToUniversal(data.time));
+    *node_proto->mutable_pose() =
+        carto::transform::ToProto(node.pose * data.tracking_to_pose);
+  }
+
+  // Write the trajectory.
+  std::ofstream proto_file(stem + ".pb",
+                           std::ios_base::out | std::ios_base::binary);
+  CHECK(trajectory.SerializeToOstream(&proto_file))
+      << "Could not serialize trajectory.";
+  proto_file.close();
+  CHECK(proto_file) << "Could not write trajectory.";
+}
+
+}  // namespace
+
+// Writes an occupancy grid.
+void Write2DAssets(
+    const std::vector<::cartographer::mapping::TrajectoryNode>&
+        trajectory_nodes,
+    const string& map_frame,
+    const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
+    const std::string& stem) {
+  WriteTrajectory(trajectory_nodes, stem);
+
   ::nav_msgs::OccupancyGrid occupancy_grid;
-  BuildOccupancyGrid(trajectory_nodes, options, &occupancy_grid);
+  BuildOccupancyGrid2D(trajectory_nodes, map_frame, submaps_options,
+                       &occupancy_grid);
   WriteOccupancyGridToPgmAndYaml(occupancy_grid, stem);
 }
 
@@ -49,6 +78,8 @@ void Write2DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
 void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
                        trajectory_nodes,
                    const double voxel_size, const std::string& stem) {
+  WriteTrajectory(trajectory_nodes, stem);
+
   const auto file_writer_factory = [](const string& filename) {
     return carto::common::make_unique<carto::io::StreamFileWriter>(filename);
   };
@@ -86,39 +117,4 @@ void Write3DAssets(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) {
-  carto::mapping::proto::Trajectory trajectory;
-  for (const auto& node : trajectory_nodes) {
-    const auto& data = *node.constant_data;
-    auto* node_proto = trajectory.add_node();
-    node_proto->set_timestamp(carto::common::ToUniversal(data.time));
-    *node_proto->mutable_pose() =
-        carto::transform::ToProto(node.pose * data.tracking_to_pose);
-  }
-
-  // Write the trajectory.
-  std::ofstream proto_file(stem + ".pb",
-                           std::ios_base::out | std::ios_base::binary);
-  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

@@ -25,10 +25,19 @@
 
 namespace cartographer_ros {
 
-// Writes all assets derivable from the 'trajectory_nodes' and 'options'.
+// Writes a trajectory proto and an occupancy grid.
-void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
+void Write2DAssets(
+    const std::vector<::cartographer::mapping::TrajectoryNode>&
         trajectory_nodes,
-                 const NodeOptions& options, const std::string& stem);
+    const string& map_frame,
+    const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
+    const std::string& stem);
+
+// Writes X-ray images, trajectory proto, 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);
 
 }  // namespace cartographer_ros
 

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -32,8 +32,8 @@ MapBuilderBridge::MapBuilderBridge(const NodeOptions& options,
 int MapBuilderBridge::AddTrajectory(
     const std::unordered_set<string>& expected_sensor_ids,
     const string& tracking_frame) {
-  const int trajectory_id =
+  const int trajectory_id = map_builder_.AddTrajectoryBuilder(
-      map_builder_.AddTrajectoryBuilder(expected_sensor_ids);
+      expected_sensor_ids, options_.trajectory_builder_options);
   LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
 
   CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
@@ -60,7 +60,22 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
     LOG(WARNING) << "No data was collected and no assets will be written.";
   } else {
     LOG(INFO) << "Writing assets with stem '" << stem << "'...";
-    cartographer_ros::WriteAssets(trajectory_nodes, options_, stem);
+    if (options_.map_builder_options.use_trajectory_builder_2d()) {
+      Write2DAssets(
+          trajectory_nodes, options_.map_frame,
+          options_.trajectory_builder_options.trajectory_builder_2d_options()
+              .submaps_options(),
+          stem);
+    }
+
+    if (options_.map_builder_options.use_trajectory_builder_3d()) {
+      Write3DAssets(
+          trajectory_nodes,
+          options_.trajectory_builder_options.trajectory_builder_3d_options()
+              .submaps_options()
+              .high_resolution(),
+          stem);
+    }
   }
 }
 
@@ -114,13 +129,18 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
 
 std::unique_ptr<nav_msgs::OccupancyGrid>
 MapBuilderBridge::BuildOccupancyGrid() {
+  CHECK(options_.map_builder_options.use_trajectory_builder_2d())
+      << "Publishing OccupancyGrids for 3D data is not yet supported";
   const auto trajectory_nodes =
       map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
   std::unique_ptr<nav_msgs::OccupancyGrid> occupancy_grid;
   if (!trajectory_nodes.empty()) {
     occupancy_grid =
         cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
-    cartographer_ros::BuildOccupancyGrid(trajectory_nodes, options_,
+    BuildOccupancyGrid2D(
+        trajectory_nodes, options_.map_frame,
+        options_.trajectory_builder_options.trajectory_builder_2d_options()
+            .submaps_options(),
         occupancy_grid.get());
   }
   return occupancy_grid;

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -22,6 +22,7 @@
 #include <unordered_set>
 
 #include "cartographer/mapping/map_builder.h"
+#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/sensor_bridge.h"
 #include "cartographer_ros/tf_bridge.h"

cartographer_ros/cartographer_ros/node_main.cc

@@ -113,7 +113,7 @@ void Run() {
   ::ros::Subscriber imu_subscriber;
   if (options.map_builder_options.use_trajectory_builder_3d() ||
       (options.map_builder_options.use_trajectory_builder_2d() &&
-       options.map_builder_options.trajectory_builder_2d_options()
+       options.trajectory_builder_options.trajectory_builder_2d_options()
            .use_imu_data())) {
     imu_subscriber = node.node_handle()->subscribe(
         kImuTopic, kInfiniteSubscriberQueueSize,

cartographer_ros/cartographer_ros/node_options.cc

@@ -27,6 +27,9 @@ NodeOptions CreateNodeOptions(
   options.map_builder_options =
       ::cartographer::mapping::CreateMapBuilderOptions(
           lua_parameter_dictionary->GetDictionary("map_builder").get());
+  options.trajectory_builder_options =
+      ::cartographer::mapping::CreateTrajectoryBuilderOptions(
+          lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
   options.map_frame = lua_parameter_dictionary->GetString("map_frame");
   options.tracking_frame =
       lua_parameter_dictionary->GetString("tracking_frame");

cartographer_ros/cartographer_ros/node_options.h

@@ -29,6 +29,8 @@ namespace cartographer_ros {
 // Top-level options of Cartographer's ROS integration.
 struct NodeOptions {
   ::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
+  ::cartographer::mapping::proto::TrajectoryBuilderOptions
+      trajectory_builder_options;
   string map_frame;
   string tracking_frame;
   string published_frame;

cartographer_ros/cartographer_ros/occupancy_grid.cc

@@ -16,22 +16,21 @@
 
 #include "cartographer_ros/occupancy_grid.h"
 
+#include "cartographer/mapping_2d/map_limits.h"
+#include "cartographer/mapping_2d/probability_grid.h"
+#include "cartographer/mapping_2d/range_data_inserter.h"
 #include "cartographer_ros/time_conversion.h"
 #include "glog/logging.h"
 
 namespace cartographer_ros {
 
-void BuildOccupancyGrid(
+void BuildOccupancyGrid2D(
     const std::vector<::cartographer::mapping::TrajectoryNode>&
         trajectory_nodes,
-    const NodeOptions& options,
+    const string& map_frame,
+    const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
     ::nav_msgs::OccupancyGrid* const occupancy_grid) {
   namespace carto = ::cartographer;
-  CHECK(options.map_builder_options.use_trajectory_builder_2d())
-      << "Publishing OccupancyGrids for 3D data is not yet supported";
-  const auto& submaps_options =
-      options.map_builder_options.trajectory_builder_2d_options()
-          .submaps_options();
   const carto::mapping_2d::MapLimits map_limits =
       carto::mapping_2d::MapLimits::ComputeMapLimits(
           submaps_options.resolution(), trajectory_nodes);
@@ -47,7 +46,7 @@ void BuildOccupancyGrid(
   }
 
   occupancy_grid->header.stamp = ToRos(trajectory_nodes.back().time());
-  occupancy_grid->header.frame_id = options.map_frame;
+  occupancy_grid->header.frame_id = map_frame;
   occupancy_grid->info.map_load_time = occupancy_grid->header.stamp;
 
   Eigen::Array2i offset;

cartographer_ros/cartographer_ros/occupancy_grid.h

@@ -20,15 +20,17 @@
 #include <vector>
 
 #include "cartographer/mapping/trajectory_node.h"
-#include "cartographer_ros/node_options.h"
+#include "cartographer/mapping_2d/proto/submaps_options.pb.h"
 #include "nav_msgs/OccupancyGrid.h"
 
 namespace cartographer_ros {
 
-void BuildOccupancyGrid(
+void BuildOccupancyGrid2D(
     const std::vector<::cartographer::mapping::TrajectoryNode>&
         trajectory_nodes,
-    const NodeOptions& options, ::nav_msgs::OccupancyGrid* occupancy_grid);
+    const string& map_frame,
+    const ::cartographer::mapping_2d::proto::SubmapsOptions& submaps_options,
+    ::nav_msgs::OccupancyGrid* const occupancy_grid);
 
 }  // namespace cartographer_ros
 

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -140,7 +140,7 @@ void Run(const std::vector<string>& bag_filenames) {
   // required.
   if (options.map_builder_options.use_trajectory_builder_3d() ||
       (options.map_builder_options.use_trajectory_builder_2d() &&
-       options.map_builder_options.trajectory_builder_2d_options()
+       options.trajectory_builder_options.trajectory_builder_2d_options()
            .use_imu_data())) {
     check_insert(kImuTopic);
   }

cartographer_ros/configuration_files/backpack_2d.lua

@@ -13,9 +13,11 @@
 -- limitations under the License.
 
 include "map_builder.lua"
+include "trajectory_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
   tracking_frame = "base_link",
   published_frame = "base_link",

cartographer_ros/configuration_files/backpack_3d.lua

@@ -13,9 +13,11 @@
 -- limitations under the License.
 
 include "map_builder.lua"
+include "trajectory_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
   tracking_frame = "base_link",
   published_frame = "base_link",

cartographer_ros/configuration_files/pr2.lua

@@ -13,9 +13,11 @@
 -- limitations under the License.
 
 include "map_builder.lua"
+include "trajectory_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
   tracking_frame = "base_footprint",
   published_frame = "base_footprint",

cartographer_ros/configuration_files/revo_lds.lua

@@ -13,9 +13,11 @@
 -- limitations under the License.
 
 include "map_builder.lua"
+include "trajectory_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
   tracking_frame = "horizontal_laser_link",
   published_frame = "horizontal_laser_link",

cartographer_ros/configuration_files/taurob_tracker.lua

@@ -13,9 +13,11 @@
 -- limitations under the License.
 
 include "map_builder.lua"
+include "trajectory_builder.lua"
 
 options = {
   map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
   map_frame = "map",
   tracking_frame = "base_link",
   published_frame = "odom",