diff --git a/README.md b/README.md index 37683af3f..a8cf95cf0 100644 --- a/README.md +++ b/README.md @@ -227,6 +227,8 @@ The following settings and options are exposed to you. My default configuration `scan_queue_size` - The number of scan messages to queue up before throwing away old ones. Should always be set to 1 in async mode +`use_map_saver` - Instantiate the map saver service and self-subscribe to the map topic + `map_file_name` - Name of the pose-graph file to load on startup if available `map_start_pose` - Pose to start pose-graph mapping/localization in, if available diff --git a/config/mapper_params_lifelong.yaml b/config/mapper_params_lifelong.yaml index b8d76d38b..995eed666 100644 --- a/config/mapper_params_lifelong.yaml +++ b/config/mapper_params_lifelong.yaml @@ -15,6 +15,7 @@ slam_toolbox: map_frame: map base_frame: base_footprint scan_topic: /scan + use_map_saver: true mode: mapping # lifelong params diff --git a/config/mapper_params_offline.yaml b/config/mapper_params_offline.yaml index c700dbb1c..c06591c31 100644 --- a/config/mapper_params_offline.yaml +++ b/config/mapper_params_offline.yaml @@ -14,6 +14,7 @@ slam_toolbox: map_frame: map base_frame: base_footprint scan_topic: /scan + use_map_saver: true mode: mapping #localization debug_logging: false throttle_scans: 1 diff --git a/config/mapper_params_online_async.yaml b/config/mapper_params_online_async.yaml index d29a1b514..d90d62cf5 100644 --- a/config/mapper_params_online_async.yaml +++ b/config/mapper_params_online_async.yaml @@ -14,6 +14,7 @@ slam_toolbox: map_frame: map base_frame: base_footprint scan_topic: /scan + use_map_saver: true mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose diff --git a/config/mapper_params_online_sync.yaml b/config/mapper_params_online_sync.yaml index a48c04860..5303edec3 100644 --- a/config/mapper_params_online_sync.yaml +++ b/config/mapper_params_online_sync.yaml @@ -14,6 +14,7 @@ slam_toolbox: map_frame: map base_frame: base_footprint scan_topic: /scan + use_map_saver: true mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose diff --git a/include/slam_toolbox/slam_toolbox_common.hpp b/include/slam_toolbox/slam_toolbox_common.hpp index 9cca586ab..4e3a5b85f 100644 --- a/include/slam_toolbox/slam_toolbox_common.hpp +++ b/include/slam_toolbox/slam_toolbox_common.hpp @@ -142,6 +142,7 @@ class SlamToolbox : public rclcpp::Node // Storage for ROS parameters std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; + bool use_map_saver_; rclcpp::Duration transform_timeout_, minimum_time_interval_; std_msgs::msg::Header scan_header; int throttle_scans_, scan_queue_size_; diff --git a/src/slam_toolbox_common.cpp b/src/slam_toolbox_common.cpp index 2f7fa3580..77ac6b113 100644 --- a/src/slam_toolbox_common.cpp +++ b/src/slam_toolbox_common.cpp @@ -61,8 +61,10 @@ void SlamToolbox::configure() pose_helper_ = std::make_unique( tf_.get(), base_frame_, odom_frame_); scan_holder_ = std::make_unique(lasers_); - map_saver_ = std::make_unique(shared_from_this(), - map_name_); + if (use_map_saver_) { + map_saver_ = std::make_unique(shared_from_this(), + map_name_); + } closure_assistant_ = std::make_unique( shared_from_this(), smapper_->getMapper(), scan_holder_.get(), @@ -144,6 +146,9 @@ void SlamToolbox::setParams() map_name_ = std::string("/map"); map_name_ = this->declare_parameter("map_name", map_name_); + use_map_saver_ = true; + use_map_saver_ = this->declare_parameter("use_map_saver", use_map_saver_); + scan_topic_ = std::string("/scan"); scan_topic_ = this->declare_parameter("scan_topic", scan_topic_);