diff --git a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc b/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc index b8dc03f..b59573b 100644 --- a/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc +++ b/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc @@ -78,7 +78,8 @@ void Run() { node.RunFinalOptimization(); if (!FLAGS_save_map_filename.empty()) { - node.SerializeState(FLAGS_save_map_filename); + node.SerializeState(FLAGS_save_map_filename, + false /* include_unfinished_submaps */); } } diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index a13a387..4ac5c6d 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -160,9 +160,10 @@ void MapBuilderBridge::RunFinalOptimization() { map_builder_->pose_graph()->RunFinalOptimization(); } -bool MapBuilderBridge::SerializeState(const std::string& filename) { +bool MapBuilderBridge::SerializeState(const std::string& filename, + const bool include_unfinished_submaps) { cartographer::io::ProtoStreamWriter writer(filename); - map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer); + map_builder_->SerializeState(include_unfinished_submaps, &writer); return writer.Close(); } diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 67e5751..46da02f 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -72,7 +72,8 @@ class MapBuilderBridge { const TrajectoryOptions& trajectory_options); void FinishTrajectory(int trajectory_id); void RunFinalOptimization(); - bool SerializeState(const std::string& filename); + bool SerializeState(const std::string& filename, + const bool include_unfinished_submaps); void HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 7a0e321..9c19769 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -639,7 +639,8 @@ bool Node::HandleWriteState( ::cartographer_ros_msgs::WriteState::Request& request, ::cartographer_ros_msgs::WriteState::Response& response) { carto::common::MutexLocker lock(&mutex_); - if (map_builder_bridge_.SerializeState(request.filename)) { + if (map_builder_bridge_.SerializeState(request.filename, + request.include_unfinished_submaps)) { response.status.code = cartographer_ros_msgs::StatusCode::OK; response.status.message = "State written to '" + request.filename + "'."; } else { @@ -787,9 +788,11 @@ void Node::HandlePointCloud2Message( ->HandlePointCloud2Message(sensor_id, msg); } -void Node::SerializeState(const std::string& filename) { +void Node::SerializeState(const std::string& filename, + const bool include_unfinished_submaps) { carto::common::MutexLocker lock(&mutex_); - CHECK(map_builder_bridge_.SerializeState(filename)) + CHECK( + map_builder_bridge_.SerializeState(filename, include_unfinished_submaps)) << "Could not write state."; } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 05a4c96..9d5f604 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -112,7 +112,8 @@ class Node { const sensor_msgs::PointCloud2::ConstPtr& msg); // Serializes the complete Node state. - void SerializeState(const std::string& filename); + void SerializeState(const std::string& filename, + const bool include_unfinished_submaps); // Loads a serialized SLAM state from a .pbstream file. void LoadState(const std::string& state_filename, bool load_frozen_state); diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 9d1cbc9..e430af2 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -74,7 +74,8 @@ void Run() { node.RunFinalOptimization(); if (!FLAGS_save_state_filename.empty()) { - node.SerializeState(FLAGS_save_state_filename); + node.SerializeState(FLAGS_save_state_filename, + true /* include_unfinished_submaps */); } } diff --git a/cartographer_ros/cartographer_ros/offline_node.cc b/cartographer_ros/cartographer_ros/offline_node.cc index bc3e1b1..4268083 100644 --- a/cartographer_ros/cartographer_ros/offline_node.cc +++ b/cartographer_ros/cartographer_ros/offline_node.cc @@ -372,7 +372,8 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) { 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); + node.SerializeState(state_output_filename, + true /* include_unfinished_submaps */); } if (FLAGS_keep_running) { ::ros::waitForShutdown(); diff --git a/cartographer_ros_msgs/srv/WriteState.srv b/cartographer_ros_msgs/srv/WriteState.srv index cb7c1c8..6b40196 100644 --- a/cartographer_ros_msgs/srv/WriteState.srv +++ b/cartographer_ros_msgs/srv/WriteState.srv @@ -13,5 +13,6 @@ # limitations under the License. string filename +bool include_unfinished_submaps --- cartographer_ros_msgs/StatusResponse status