Sync Humble Sept 25 (SteveMacenski#637)

* Optional map saver (SteveMacenski#633)

* Add parameter to stop map saver from being initialised

Map saver will subscribe to the map topic and force updates of the map
even when nothing else is subscribed.
This allows it to be turned off to save processing cycles.

* Add details of use_map_saver parameter to README

* Add defaulted-true use_map_saver parameter to launch configs

---------

Co-authored-by: James Ward <j.ward@sydney.edu.au>

* adding namespace support for map saving (SteveMacenski#613)

* fix mismatched service name in rviz plugin (SteveMacenski#634)

* bumping to 2.6.6 for release

---------

Co-authored-by: James Ward <james@robomo.co>
Co-authored-by: James Ward <j.ward@sydney.edu.au>
Co-authored-by: gene.su <47352191+gene-su@users.noreply.github.com>

Author: 4 people

README.md

@@ -224,6 +224,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

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

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

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

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

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_;

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>slam_toolbox</name>
-  <version>2.6.5</version>
+  <version>2.6.6</version>
   <description>
      This package provides a sped up improved slam karto with updated SDK and visualization and modification toolsets
   </description>

rviz_plugin/slam_toolbox_rviz_plugin.cpp

@@ -78,9 +78,9 @@ SlamToolboxPlugin::SlamToolboxPlugin(QWidget * parent)
     "/slam_toolbox/pause_new_measurements");
   _load_submap_for_merging =
     ros_node_->create_client<slam_toolbox::srv::AddSubmap>(
-    "/map_merging/add_submap");
+    "/slam_toolbox/add_submap");
   _merge = ros_node_->create_client<slam_toolbox::srv::MergeMaps>(
-    "/map_merging/merge_submaps");
+    "/slam_toolbox/merge_submaps");
 
   _vbox = new QVBoxLayout();
   _hbox1 = new QHBoxLayout();

src/map_saver.cpp

@@ -58,10 +58,16 @@ bool MapSaver::saveMapCallback(
   }
 
   const std::string name = req->name.data;
+  std::string set_namespace;
+  const std::string namespace_str = std::string(node_->get_namespace());
+  if (!namespace_str.empty()) {
+    set_namespace = " -r __ns:=" + namespace_str;
+  }
+
   if (name != "") {
     RCLCPP_INFO(node_->get_logger(),
       "SlamToolbox: Saving map as %s.", name.c_str());
-    int rc = system(("ros2 run nav2_map_server map_saver_cli -f " + name  + " --ros-args -p map_subscribe_transient_local:=true").c_str());
+    int rc = system(("ros2 run nav2_map_server map_saver_cli -f " + name  + " --ros-args -p map_subscribe_transient_local:=true" + set_namespace).c_str());
     if (rc == 0) {
       response->result = response->RESULT_SUCCESS;
     } else {
@@ -70,7 +76,7 @@ bool MapSaver::saveMapCallback(
   } else {
     RCLCPP_INFO(node_->get_logger(),
       "SlamToolbox: Saving map in current directory.");
-    int rc = system("ros2 run nav2_map_server map_saver_cli --ros-args -p map_subscribe_transient_local:=true");
+    int rc = system(("ros2 run nav2_map_server map_saver_cli --ros-args -p map_subscribe_transient_local:=true" + set_namespace).c_str());
     if (rc == 0) {
       response->result = response->RESULT_SUCCESS;
     } else {

src/slam_toolbox_common.cpp

@@ -61,8 +61,10 @@ void SlamToolbox::configure()
   pose_helper_ = std::make_unique<pose_utils::GetPoseHelper>(
     tf_.get(), base_frame_, odom_frame_);
   scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
-  map_saver_ = std::make_unique<map_saver::MapSaver>(shared_from_this(),
-      map_name_);
+  if (use_map_saver_) {
+    map_saver_ = std::make_unique<map_saver::MapSaver>(shared_from_this(),
+        map_name_);
+  }
   closure_assistant_ =
     std::make_unique<loop_closure_assistant::LoopClosureAssistant>(
     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_);