Allowing QoS overrides to also be available for nodes using message filters (#5689)

Signed-off-by: SteveMacenski <[email protected]>

Author: SteveMacenski

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -412,6 +412,7 @@ class AmclNode : public nav2::LifecycleNode
   std::string scan_topic_{"scan"};
   std::string map_topic_{"map"};
   bool freespace_downsampling_ = false;
+  bool allow_parameter_qos_overrides_ = true;
 };
 
 }  // namespace nav2_amcl

nav2_amcl/src/amcl_node.cpp

@@ -947,6 +947,8 @@ AmclNode::initParameters()
   scan_topic_ = this->declare_or_get_parameter("scan_topic", std::string{"scan"});
   map_topic_ = this->declare_or_get_parameter("map_topic", std::string{"map"});
   freespace_downsampling_ = this->declare_or_get_parameter("freespace_downsampling", false);
+  allow_parameter_qos_overrides_ = this->declare_or_get_parameter(
+    "allow_parameter_qos_overrides", true);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
   transform_tolerance_ = tf2::durationFromSec(tmp_tol);
@@ -1343,8 +1345,8 @@ AmclNode::initTransforms()
 void
 AmclNode::initMessageFilters()
 {
-  auto sub_opt = rclcpp::SubscriptionOptions();
-  sub_opt.callback_group = callback_group_;
+  auto sub_opt = nav2::interfaces::createSubscriptionOptions(
+    scan_topic_, allow_parameter_qos_overrides_, callback_group_);
 
   #if RCLCPP_VERSION_GTE(29, 6, 0)
   laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -262,6 +262,7 @@ class ObstacleLayer : public CostmapLayer
   bool rolling_window_;
   bool was_reset_;
   nav2_costmap_2d::CombinationMethod combination_method_;
+  bool allow_parameter_qos_overrides_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -48,6 +48,7 @@
 #include "nav2_util/raytrace_line_2d.hpp"
 #include "nav2_costmap_2d/costmap_math.hpp"
 #include "nav2_ros_common/node_utils.hpp"
+#include "nav2_ros_common/interface_factories.hpp"
 #include "rclcpp/version.h"
 
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer)
@@ -95,6 +96,8 @@ void ObstacleLayer::onInitialize()
     throw std::runtime_error{"Failed to lock node"};
   }
 
+  allow_parameter_qos_overrides_ = nav2::declare_or_get_parameter(node,
+    "allow_parameter_qos_overrides", true);
   node->get_parameter(name_ + "." + "enabled", enabled_);
   node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
   node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_);
@@ -133,9 +136,6 @@ void ObstacleLayer::onInitialize()
 
   global_frame_ = layered_costmap_->getGlobalFrameID();
 
-  auto sub_opt = rclcpp::SubscriptionOptions();
-  sub_opt.callback_group = callback_group_;
-
   // now we need to split the topics based on whitespace which we can use a stringstream for
   std::stringstream ss(topics_string);
 
@@ -235,6 +235,9 @@ void ObstacleLayer::onInitialize()
 
     // create a callback for the topic
     if (data_type == "LaserScan") {
+      auto sub_opt = nav2::interfaces::createSubscriptionOptions(
+        topic, allow_parameter_qos_overrides_, callback_group_);
+
       // For Kilted and Older Support from Message Filters API change
       #if RCLCPP_VERSION_GTE(29, 6, 0)
       std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan>> sub;
@@ -289,6 +292,9 @@ void ObstacleLayer::onInitialize()
           tf_filter_tolerance));
 
     } else {
+      auto sub_opt = nav2::interfaces::createSubscriptionOptions(
+        topic, allow_parameter_qos_overrides_, callback_group_);
+
       // For Rolling and Newer Support from PointCloudTransport API change
       #if RCLCPP_VERSION_GTE(30, 0, 0)
       std::shared_ptr<point_cloud_transport::SubscriberFilter> sub;