From b7d8af834efe2848d785889db8318edb30d77307 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 27 Feb 2018 21:26:52 +0100 Subject: [PATCH] Unfrozen trajectories (#710) Unfrozen trajectories --- .../cartographer_ros/map_builder_bridge.cc | 16 +++++++--- .../cartographer_ros/map_builder_bridge.h | 2 +- cartographer_ros/cartographer_ros/node.cc | 5 ++-- cartographer_ros/cartographer_ros/node.h | 4 +-- .../cartographer_ros/node_main.cc | 16 ++++++---- .../cartographer_ros/offline_node.cc | 30 +++++++++++-------- .../demo_backpack_2d_localization.launch | 2 +- docs/source/demos.rst | 4 +-- docs/source/ros_api.rst | 12 +++++--- 9 files changed, 57 insertions(+), 34 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 121e21a..3052485 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -105,10 +105,18 @@ MapBuilderBridge::MapBuilderBridge( map_builder_(std::move(map_builder)), tf_buffer_(tf_buffer) {} -void MapBuilderBridge::LoadMap(const std::string& map_filename) { - LOG(INFO) << "Loading map '" << map_filename << "'..."; - cartographer::io::ProtoStreamReader stream(map_filename); - map_builder_->LoadMap(&stream); +void MapBuilderBridge::LoadState(const std::string& state_filename, + bool load_frozen_state) { + // Check if suffix of the state file is ".pbstream". + const std::string suffix = ".pbstream"; + CHECK_EQ(state_filename.substr( + std::max(state_filename.size() - suffix.size(), 0)), + suffix) + << "The file containing the state to be loaded must be a " + ".pbstream file."; + LOG(INFO) << "Loading saved state '" << state_filename << "'..."; + cartographer::io::ProtoStreamReader stream(state_filename); + map_builder_->LoadState(&stream, load_frozen_state); } int MapBuilderBridge::AddTrajectory( diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 85b4d89..1108f32 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -63,7 +63,7 @@ class MapBuilderBridge { MapBuilderBridge(const MapBuilderBridge&) = delete; MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; - void LoadMap(const std::string& map_filename); + void LoadState(const std::string& state_filename, bool load_frozen_state); int AddTrajectory( const std::set< ::cartographer::mapping::TrajectoryBuilderInterface::SensorId>& diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 10636e7..6acc3be 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -657,9 +657,10 @@ void Node::SerializeState(const std::string& filename) { << "Could not write state."; } -void Node::LoadMap(const std::string& map_filename) { +void Node::LoadState(const std::string& state_filename, + const bool load_frozen_state) { carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_.LoadMap(map_filename); + map_builder_bridge_.LoadState(state_filename, load_frozen_state); } } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index fa7cd0a..4c61e10 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -108,8 +108,8 @@ class Node { // Serializes the complete Node state. void SerializeState(const std::string& filename); - // Loads a persisted state to use as a map. - void LoadMap(const std::string& map_filename); + // Loads a serialized SLAM state from a .pbstream file. + void LoadState(const std::string& state_filename, bool load_frozen_state); ::ros::NodeHandle* node_handle(); diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 94516ab..b41972c 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -28,12 +28,16 @@ DEFINE_string(configuration_directory, "", DEFINE_string(configuration_basename, "", "Basename, i.e. not containing any directory prefix, of the " "configuration file."); -DEFINE_string(map_filename, "", "If non-empty, filename of a map to load."); +DEFINE_string(load_state_filename, "", + "If non-empty, filename of a .pbstream file to load, containing " + "a saved SLAM state."); +DEFINE_bool(load_frozen_state, true, + "Load the saved state as frozen (non-optimized) trajectories."); DEFINE_bool( start_trajectory_with_default_topics, true, "Enable to immediately start the first trajectory with default topics."); DEFINE_string( - save_map_filename, "", + save_state_filename, "", "If non-empty, serialize state and write it to disk before shutting down."); namespace cartographer_ros { @@ -52,8 +56,8 @@ void Run() { cartographer::common::make_unique( node_options.map_builder_options); Node node(node_options, std::move(map_builder), &tf_buffer); - if (!FLAGS_map_filename.empty()) { - node.LoadMap(FLAGS_map_filename); + if (!FLAGS_load_state_filename.empty()) { + node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } if (FLAGS_start_trajectory_with_default_topics) { @@ -65,8 +69,8 @@ void Run() { node.FinishAllTrajectories(); node.RunFinalOptimization(); - if (!FLAGS_save_map_filename.empty()) { - node.SerializeState(FLAGS_save_map_filename); + if (!FLAGS_save_state_filename.empty()) { + node.SerializeState(FLAGS_save_state_filename); } } diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index 4d21e5d..8b8d08f 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -52,8 +52,11 @@ DEFINE_string(urdf_filenames, "", "static links for the sensor configuration(s)."); DEFINE_bool(use_bag_transforms, true, "Whether to read, use and republish transforms from bags."); -DEFINE_string(pbstream_filename, "", - "If non-empty, filename of a pbstream to load."); +DEFINE_string(load_state_filename, "", + "If non-empty, filename of a .pbstream file to load, containing " + "a saved SLAM state."); +DEFINE_bool(load_frozen_state, true, + "Load the saved state as frozen (non-optimized) trajectories."); DEFINE_bool(keep_running, false, "Keep running the offline node after all messages from the bag " "have been processed."); @@ -75,7 +78,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { << "-configuration_directory is missing."; CHECK(!FLAGS_configuration_basenames.empty()) << "-configuration_basenames is missing."; - CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing."; + CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty())) + << "-bag_filenames and -load_state_filename cannot both be unspecified."; const auto bag_filenames = cartographer_ros::SplitString(FLAGS_bag_filenames, ','); cartographer_ros::NodeOptions node_options; @@ -95,7 +99,9 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { } bag_trajectory_options.push_back(current_trajectory_options); } - CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size()); + if (bag_filenames.size() > 0) { + CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size()); + } // Since we preload the transform buffer, we should never have to wait for a // transform. When we finish processing the bag, we will simply drop any @@ -122,10 +128,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { tf_buffer.setUsingDedicatedThread(true); Node node(node_options, std::move(map_builder), &tf_buffer); - if (!FLAGS_pbstream_filename.empty()) { - // TODO(jihoonl): LoadMap should be replaced by some better deserialization - // of full SLAM state as non-frozen trajectories once possible - node.LoadMap(FLAGS_pbstream_filename); + if (!FLAGS_load_state_filename.empty()) { + node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state); } ::ros::Publisher tf_publisher = @@ -308,10 +312,12 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB"; #endif - if (::ros::ok()) { - const std::string output_filename = bag_filenames.front() + ".pbstream"; - LOG(INFO) << "Writing state to '" << output_filename << "'..."; - node.SerializeState(output_filename); + if (::ros::ok() && bag_filenames.size() > 0) { + const std::string output_filename = bag_filenames.front(); + const std::string suffix = ".pbstream"; + const std::string state_output_filename = output_filename + suffix; + LOG(INFO) << "Writing state to '" << state_output_filename << "'..."; + node.SerializeState(state_output_filename); } if (FLAGS_keep_running) { ::ros::waitForShutdown(); diff --git a/cartographer_ros/launch/demo_backpack_2d_localization.launch b/cartographer_ros/launch/demo_backpack_2d_localization.launch index b1e4658..e91e99f 100644 --- a/cartographer_ros/launch/demo_backpack_2d_localization.launch +++ b/cartographer_ros/launch/demo_backpack_2d_localization.launch @@ -27,7 +27,7 @@ type="cartographer_node" args=" -configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename backpack_2d_localization.lua - -map_filename $(arg map_filename)" + -load_state_filename $(arg load_state_filename)" output="screen"> diff --git a/docs/source/demos.rst b/docs/source/demos.rst index 962c0c4..99b8596 100644 --- a/docs/source/demos.rst +++ b/docs/source/demos.rst @@ -30,7 +30,7 @@ Pure localization roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag # Run pure localization: roslaunch cartographer_ros demo_backpack_2d_localization.launch \ - map_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \ + load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \ bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag # Pure localization demo in 3D: We use 2 different 3D bags from the Deutsche @@ -42,7 +42,7 @@ Pure localization roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag # Run pure localization: roslaunch cartographer_ros demo_backpack_3d_localization.launch \ - map_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \ + load_state_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \ bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag Revo LDS diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index ad183de..547350b 100644 --- a/docs/source/ros_api.rst +++ b/docs/source/ros_api.rst @@ -38,10 +38,14 @@ TODO(hrapp): Should these not be removed? It seems duplicated efforts documentin Basename (i.e. not containing any directory prefix) of the configuration file (e.g. backpack_3d.lua). -\-\-map_filename - A Cartographer state file that will be loaded from disk. This allows to - add new trajectories SLAMing from an earlier state, but the loaded state is - frozen. +\-\-load_state_filename + A Cartographer .pbstream state file that will be loaded from disk. This allows + to add new trajectories SLAMing from an earlier state. + +\-\-load_frozen_state + This boolean parameter controls if the saved state, specified using the option + \-\-load_state_filename, is going to be loaded as a set of frozen (not + optimized) trajectories. Subscribed Topics -----------------