Optional transient map saver (#2215)

v-lopez · Michael-Equi · web-flow · commit 41996f409fcd · 2021-03-03T10:27:51.000-08:00
* Added transient local subscription qos profile parameter to map saver (#1871) * Added transient local subscription qos profile parameter to map saver * Made transient local default true * Fixed linter problem * switched back house world to waffle model * Make transient map subscribe backwards compatible for foxy Co-authored-by: Michael Equi <32988490+Michael-Equi@users.noreply.github.com>
diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml
@@ -243,6 +243,7 @@ map_saver:
     save_map_timeout: 5000
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
+    map_subscribe_transient_local: False
 
 planner_server:
   ros__parameters:
diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp
@@ -103,6 +103,8 @@ class MapSaver : public nav2_util::LifecycleNode
   // Default values for map thresholds
   double free_thresh_default_;
   double occupied_thresh_default_;
+  // param for handling QoS configuration
+  bool map_subscribe_transient_local_;
 
   // The name of the service for saving a map from topic
   const std::string save_map_service_name_{"save_map"};
diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp
@@ -50,6 +50,8 @@ MapSaver::MapSaver()
 
   free_thresh_default_ = declare_parameter("free_thresh_default", 0.25),
   occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65);
+  // false only of foxy for backwards compatibility
+  map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", false);
 }
 
 MapSaver::~MapSaver()
@@ -173,8 +175,15 @@ bool MapSaver::saveMapTopicToFile(
     // Add new subscription for incoming map topic.
     // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode
     // as a map listener.
+    rclcpp::QoS map_qos = rclcpp::SystemDefaultsQoS();  // initialize to default
+    if (map_subscribe_transient_local_) {
+      map_qos = rclcpp::QoS(10);
+      map_qos.transient_local();
+      map_qos.reliable();
+      map_qos.keep_last(1);
+    }
     auto map_sub = rclcpp_node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
-      map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback);
+      map_topic_loc, map_qos, mapCallback);
 
     rclcpp::Time start_time = now();
     while (rclcpp::ok()) {
