Latched map subscriber QoS depth 1 (#5665)

* Latched map subscriber QoS depth 1

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* 3

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

* typo

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>

---------

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

Author: doisyg

nav2_amcl/src/amcl_node.cpp

@@ -1223,7 +1223,7 @@ AmclNode::updateParametersCallback(
     map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
       map_topic_,
       std::bind(&AmclNode::mapReceived, this, std::placeholders::_1),
-      nav2::qos::LatchedSubscriptionQoS());
+      nav2::qos::LatchedSubscriptionQoS(3));
   }
 }
 
@@ -1395,7 +1395,7 @@ AmclNode::initPubSub()
   map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
     map_topic_,
     std::bind(&AmclNode::mapReceived, this, std::placeholders::_1),
-    nav2::qos::LatchedSubscriptionQoS());
+    nav2::qos::LatchedSubscriptionQoS(3));
 
   RCLCPP_INFO(get_logger(), "Subscribed to map topic.");
 }

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

@@ -53,7 +53,7 @@ class CostmapSubscriber
     costmap_sub_ = nav2::interfaces::create_subscription<nav2_msgs::msg::Costmap>(
       parent, topic_name_,
       std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1),
-      nav2::qos::LatchedSubscriptionQoS());
+      nav2::qos::LatchedSubscriptionQoS(3));
 
     costmap_update_sub_ = nav2::interfaces::create_subscription<nav2_msgs::msg::CostmapUpdate>(
       parent, topic_name_ + "_updates",

nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp

@@ -144,7 +144,7 @@ void BinaryFilter::filterInfoCallback(
   mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
     mask_topic_,
     std::bind(&BinaryFilter::maskCallback, this, std::placeholders::_1),
-    nav2::qos::LatchedSubscriptionQoS());
+    nav2::qos::LatchedSubscriptionQoS(3));
 }
 
 void BinaryFilter::maskCallback(

nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp

@@ -133,7 +133,7 @@ void KeepoutFilter::filterInfoCallback(
   mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
     mask_topic_,
     std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1),
-    nav2::qos::LatchedSubscriptionQoS());
+    nav2::qos::LatchedSubscriptionQoS(3));
 }
 
 void KeepoutFilter::maskCallback(

nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

@@ -152,7 +152,7 @@ void SpeedFilter::filterInfoCallback(
   mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
     mask_topic_,
     std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1),
-    nav2::qos::LatchedSubscriptionQoS());
+    nav2::qos::LatchedSubscriptionQoS(3));
 }
 
 void SpeedFilter::maskCallback(

nav2_costmap_2d/plugins/static_layer.cpp

@@ -77,7 +77,7 @@ StaticLayer::onInitialize()
 
   rclcpp::QoS map_qos = nav2::qos::StandardTopicQoS();  // initialize to default
   if (map_subscribe_transient_local_) {
-    map_qos = nav2::qos::LatchedSubscriptionQoS();
+    map_qos = nav2::qos::LatchedSubscriptionQoS(3);
   }
 
   RCLCPP_INFO(

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -96,7 +96,7 @@ void VoxelLayer::onInitialize()
 
   if (publish_voxel_) {
     voxel_pub_ = node->create_publisher<nav2_msgs::msg::VoxelGrid>(
-      "voxel_grid", nav2::qos::LatchedPublisherQoS());
+      "voxel_grid", nav2::qos::LatchedPublisherQoS(1));
     voxel_pub_->on_activate();
   }
 

nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp

@@ -87,7 +87,7 @@ class LayerSubscriber
     layer_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
       topic_name,
       std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1),
-      nav2::qos::LatchedSubscriptionQoS(),
+      nav2::qos::LatchedSubscriptionQoS(3),
       callback_group_);
 
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();

nav2_map_server/src/map_saver/map_saver.cpp

@@ -188,7 +188,7 @@ bool MapSaver::saveMapTopicToFile(
 
     rclcpp::QoS map_qos = nav2::qos::StandardTopicQoS();  // initialize to default
     if (map_subscribe_transient_local_) {
-      map_qos = nav2::qos::LatchedSubscriptionQoS();
+      map_qos = nav2::qos::LatchedSubscriptionQoS(3);
     }
 
     // Create new CallbackGroup for map_sub

nav2_map_server/test/component/test_map_saver_publisher.cpp

@@ -16,6 +16,7 @@
 #include <string>
 #include <memory>
 
+#include "nav2_ros_common/qos_profiles.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_map_server/map_io.hpp"
 #include "test_constants/test_constants.h"
@@ -41,7 +42,7 @@ class TestPublisher : public rclcpp::Node
 
     map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
       "map",
-      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+      nav2::qos::LatchedPublisherQoS());
     map_pub_->publish(msg);
   }