From 0d4cda60a64c27b284fb1b9f1858554bf1f2ad2c Mon Sep 17 00:00:00 2001 From: Michael Equi <32988490+Michael-Equi@users.noreply.github.com> Date: Thu, 16 Jul 2020 15:00:50 -0700 Subject: [PATCH 1/2] 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 --- nav2_bringup/bringup/params/nav2_params.yaml | 1 + nav2_map_server/include/nav2_map_server/map_saver.hpp | 2 ++ nav2_map_server/src/map_saver/map_saver.cpp | 9 ++++++++- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 07817d4318..defa6e3238 100644 --- 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: True 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 index 3aa4e2e233..9c9b57d292 100644 --- 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 index 7555ebd632..1bc50c512c 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -50,6 +50,7 @@ MapSaver::MapSaver() free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); + map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", true); } MapSaver::~MapSaver() @@ -173,8 +174,14 @@ 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(10); // initialize to default + if (map_subscribe_transient_local_) { + map_qos.transient_local(); + map_qos.reliable(); + map_qos.keep_last(1); + } auto map_sub = rclcpp_node_->create_subscription( - map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback); + map_topic_loc, map_qos, mapCallback); rclcpp::Time start_time = now(); while (rclcpp::ok()) { From da8ae4de13480fddff659fa31df19f6d9027bb39 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 3 Mar 2021 16:21:47 +0100 Subject: [PATCH 2/2] Make transient map subscribe backwards compatible for foxy --- nav2_bringup/bringup/params/nav2_params.yaml | 2 +- nav2_map_server/src/map_saver/map_saver.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index defa6e3238..bbfc35d969 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -243,7 +243,7 @@ map_saver: save_map_timeout: 5000 free_thresh_default: 0.25 occupied_thresh_default: 0.65 - map_subscribe_transient_local: True + map_subscribe_transient_local: False planner_server: ros__parameters: diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 1bc50c512c..563e0d5290 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -50,7 +50,8 @@ MapSaver::MapSaver() free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); - map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", true); + // false only of foxy for backwards compatibility + map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", false); } MapSaver::~MapSaver() @@ -174,8 +175,9 @@ 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(10); // initialize to default + 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);