From e5b63e377af669addc15733b899b630988c43185 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Fri, 25 Nov 2016 09:37:11 +0100 Subject: [PATCH] Fix trajectory start/stop bug and some cleanups. (#184) --- .../cartographer_ros/assets_writer.cc | 6 ++--- .../cartographer_ros/assets_writer_main.cc | 5 ++-- .../cartographer_ros/node_main.cc | 23 ++++++++++++------- .../configuration_files/assets_writer.lua | 1 - .../configuration_files/taurob_tracker.lua | 5 ++-- 5 files changed, 24 insertions(+), 16 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index f9c9756..4fce4e5 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/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); diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index e711f17..a3653b9 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/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); } diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 6959fb0..47a592c 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/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( + &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."; - return true; + LOG(WARNING) << "No data collected and no assets will be written."; + } else { + LOG(INFO) << "Writing assets..."; + WriteAssets(trajectory_nodes, options_, request.stem); } - WriteAssets(trajectory_nodes, options_, request.stem); - // Start a new trajectory. - trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_); - sensor_bridge_ = carto::common::make_unique( - &tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_)); + LOG(INFO) << "New trajectory started."; return true; } diff --git a/cartographer_ros/configuration_files/assets_writer.lua b/cartographer_ros/configuration_files/assets_writer.lua index c992f5f..4ef0fd6 100644 --- a/cartographer_ros/configuration_files/assets_writer.lua +++ b/cartographer_ros/configuration_files/assets_writer.lua @@ -1,6 +1,5 @@ VOXEL_SIZE = 5e-2 - options = { tracking_frame = "base_link", pipeline = { diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index 0ce6826..7f72ddf 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/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