Fix trajectory start/stop bug and some cleanups. (#184)

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -51,15 +51,15 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
   carto::io::XRayPointsProcessor xy_xray_points_processor(
       voxel_size, carto::transform::Rigid3f::Rotation(
                       Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
-      {}, stem + "_xray_xy.png", &null_points_processor);
+      {}, stem + "_xray_xy", &null_points_processor);
   carto::io::XRayPointsProcessor yz_xray_points_processor(
       voxel_size, carto::transform::Rigid3f::Rotation(
                       Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
-      {}, stem + "_xray_yz.png", &xy_xray_points_processor);
+      {}, stem + "_xray_yz", &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())),
-      {}, stem + "_xray_xz.png", &yz_xray_points_processor);
+      {}, stem + "_xray_xz", &yz_xray_points_processor);
   carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
       stem + ".ply", &xz_xray_points_processor);
 

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -44,7 +44,7 @@ DEFINE_string(configuration_basename, "",
               "Basename, i.e. not containing any directory prefix, of the "
               "configuration file.");
 DEFINE_string(
-    urdf, "",
+    urdf_filename, "",
     "URDF file that contains static links for your sensor configuration.");
 DEFINE_string(bag_filename, "", "Bag to process.");
 DEFINE_string(
@@ -182,8 +182,9 @@ int main(int argc, char** argv) {
   CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
   CHECK(!FLAGS_trajectory_filename.empty())
       << "-trajectory_filename is missing.";
+  CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing.";
 
   ::cartographer_ros::Run(FLAGS_trajectory_filename, FLAGS_bag_filename,
                           FLAGS_configuration_directory,
-                          FLAGS_configuration_basename, FLAGS_urdf);
+                          FLAGS_configuration_basename, FLAGS_urdf_filename);
 }

cartographer_ros/cartographer_ros/node_main.cc

@@ -299,22 +299,29 @@ bool Node::HandleSubmapQuery(
 bool Node::HandleFinishTrajectory(
     ::cartographer_ros_msgs::FinishTrajectory::Request& request,
     ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
+  LOG(INFO) << "Finishing trajectory...";
+
   carto::common::MutexLocker lock(&mutex_);
-  map_builder_.FinishTrajectory(trajectory_id_);
+  const int previous_trajectory_id = trajectory_id_;
+
+  trajectory_id_ =
+      map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
+  sensor_bridge_ = carto::common::make_unique<SensorBridge>(
+      &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
+
+  map_builder_.FinishTrajectory(previous_trajectory_id);
   map_builder_.sparse_pose_graph()->RunFinalOptimization();
 
   const auto trajectory_nodes =
       map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
   if (trajectory_nodes.empty()) {
-    LOG(WARNING) << "Map is empty and will not be saved.";
+    LOG(WARNING) << "No data collected and no assets will be written.";
-    return true;
+  } else {
+    LOG(INFO) << "Writing assets...";
+    WriteAssets(trajectory_nodes, options_, request.stem);
   }
-  WriteAssets(trajectory_nodes, options_, request.stem);
 
-  // Start a new trajectory.
+  LOG(INFO) << "New trajectory started.";
-  trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
-  sensor_bridge_ = carto::common::make_unique<SensorBridge>(
-      &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
   return true;
 }
 

cartographer_ros/configuration_files/assets_writer.lua

@@ -1,6 +1,5 @@
 VOXEL_SIZE = 5e-2
 
-
 options = {
   tracking_frame = "base_link",
   pipeline = {

cartographer_ros/configuration_files/taurob_tracker.lua

@@ -30,14 +30,15 @@ options = {
   pose_publish_period_sec = 5e-3,
 }
 
-TRAJECTORY_BUILDER_3D.scans_per_accumulation = 90
+TRAJECTORY_BUILDER_3D.scans_per_accumulation = 180
 TRAJECTORY_BUILDER_3D.laser_min_range = 0.5
 TRAJECTORY_BUILDER_3D.laser_max_range = 20.
+TRAJECTORY_BUILDER_3D.submaps.num_laser_fans = 40.
 
 MAP_BUILDER.use_trajectory_builder_3d = true
 MAP_BUILDER.num_background_threads = 7
 MAP_BUILDER.sparse_pose_graph.optimization_problem.huber_scale = 5e2
-MAP_BUILDER.sparse_pose_graph.optimize_every_n_scans = 320
+MAP_BUILDER.sparse_pose_graph.optimize_every_n_scans = 40
 MAP_BUILDER.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
 MAP_BUILDER.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10
 -- Reuse the coarser 3D voxel filter to speed up the computation of loop closure