diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 6debdf5..6d5f730 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -33,6 +33,12 @@ MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options, map_builder_(node_options.map_builder_options), 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); +} + int MapBuilderBridge::AddTrajectory( const std::unordered_set& expected_sensor_ids, const TrajectoryOptions& trajectory_options) { @@ -153,7 +159,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { std::unique_ptr MapBuilderBridge::BuildOccupancyGrid() { CHECK(node_options_.map_builder_options.use_trajectory_builder_2d()) - << "Publishing OccupancyGrids for 3D data is not yet supported"; + << "Publishing OccupancyGrids for 3D data is not yet supported."; const auto all_trajectory_nodes = map_builder_.sparse_pose_graph()->GetTrajectoryNodes(); std::unique_ptr occupancy_grid; diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 46fae8d..f99f890 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_ #include +#include #include #include @@ -49,6 +50,7 @@ class MapBuilderBridge { MapBuilderBridge(const MapBuilderBridge&) = delete; MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; + void LoadMap(const std::string& map_filename); int AddTrajectory(const std::unordered_set& expected_sensor_ids, const TrajectoryOptions& trajectory_options); void FinishTrajectory(int trajectory_id); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 296adb9..d540d8b 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -43,8 +43,8 @@ namespace { // properly. constexpr float kPointCloudComponentFourMagic = 1.; -using ::cartographer::transform::Rigid3d; using ::cartographer::sensor::PointCloudWithIntensities; +using ::cartographer::transform::Rigid3d; sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp, const string& frame_id, diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 29a8923..681a18d 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -494,4 +494,9 @@ void Node::FinishAllTrajectories() { } } } + +void Node::LoadMap(const std::string& map_filename) { + map_builder_bridge_.LoadMap(map_filename); +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index ce4ed55..31b6570 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -53,6 +53,9 @@ class Node { // Starts the first trajectory with the default topics. void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options); + // Loads a persisted state to use as a map. + void LoadMap(const std::string& map_filename); + ::ros::NodeHandle* node_handle(); MapBuilderBridge* map_builder_bridge(); diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 5b8f6f0..5821cc1 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -34,6 +34,7 @@ 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."); namespace cartographer_ros { namespace { @@ -60,6 +61,9 @@ void Run() { std::tie(node_options, trajectory_options) = LoadOptions(); Node node(node_options, &tf_buffer); + if (!FLAGS_map_filename.empty()) { + node.LoadMap(FLAGS_map_filename); + } node.StartTrajectoryWithDefaultTopics(trajectory_options); ::ros::spin();