Enable QoS overrides (#657)

* Enable QoS overrides Signed-off-by: Audrow Nash <audrow@hey.com> * Use SubscriptionOptions Signed-off-by: Audrow Nash <audrow@hey.com> * Use with_default_policies Signed-off-by: Audrow Nash <audrow@hey.com> * Use with_default_policies in ros_filter Signed-off-by: Audrow Nash <audrow@hey.com> * Improve formatting Signed-off-by: Audrow Nash <audrow@hey.com>
cra-ros-pkg · May 20, 2021 · 2816e92 · 2816e92
1 parent d27e8cb
commit 2816e92
Show file tree

Hide file tree

Showing 4 changed files with 39 additions and 11 deletions.
diff --git a/include/robot_localization/ros_robot_localization_listener.hpp b/include/robot_localization/ros_robot_localization_listener.hpp
@@ -49,6 +49,18 @@
 namespace robot_localization
 {
 
+namespace detail
+{
+rclcpp::SubscriptionOptions
+get_subscription_options_with_default_qos_override_policies()
+{
+  auto subscription_options = rclcpp::SubscriptionOptions();
+  subscription_options.qos_overriding_options =
+    rclcpp::QosOverridingOptions::with_default_policies();
+  return subscription_options;
+}
+}  // namespace detail
+
 //! @brief RosRobotLocalizationListener class
 //!
 //! This class wraps the RobotLocalizationEstimator. It listens to topics over
@@ -70,7 +82,10 @@ class RosRobotLocalizationListener
   //!
   //! @param[in] node - rclcpp node shared pointer
   //!
-  explicit RosRobotLocalizationListener(rclcpp::Node::SharedPtr node);
+  explicit RosRobotLocalizationListener(
+    rclcpp::Node::SharedPtr node,
+    rclcpp::SubscriptionOptions options =
+    detail::get_subscription_options_with_default_qos_override_policies());
 
   //! @brief Destructor
   //!

diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp
@@ -141,23 +141,32 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options)
 
   auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(1));
 
+  auto subscriber_options = rclcpp::SubscriptionOptions();
+  subscriber_options.qos_overriding_options =
+    rclcpp::QosOverridingOptions::with_default_policies();
   odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
-    "odometry/filtered", custom_qos, std::bind(&NavSatTransform::odomCallback, this, _1));
+    "odometry/filtered", custom_qos, std::bind(
+      &NavSatTransform::odomCallback, this, _1), subscriber_options);
 
   gps_sub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
-    "gps/fix", custom_qos, std::bind(&NavSatTransform::gpsFixCallback, this, _1));
+    "gps/fix", custom_qos, std::bind(&NavSatTransform::gpsFixCallback, this, _1),
+    subscriber_options);
 
   if (!use_odometry_yaw_ && !use_manual_datum_) {
     imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
-      "imu", custom_qos, std::bind(&NavSatTransform::imuCallback, this, _1));
+      "imu", custom_qos, std::bind(&NavSatTransform::imuCallback, this, _1), subscriber_options);
   }
 
+  rclcpp::PublisherOptions publisher_options;
+  publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   gps_odom_pub_ =
-    this->create_publisher<nav_msgs::msg::Odometry>("odometry/gps", rclcpp::QoS(10));
+    this->create_publisher<nav_msgs::msg::Odometry>(
+    "odometry/gps", rclcpp::QoS(10), publisher_options);
 
   if (publish_gps_) {
     filtered_gps_pub_ =
-      this->create_publisher<sensor_msgs::msg::NavSatFix>("gps/filtered", rclcpp::QoS(10));
+      this->create_publisher<sensor_msgs::msg::NavSatFix>(
+      "gps/filtered", rclcpp::QoS(10), publisher_options);
   }
 
   // Sleep for the parameterized amount of time, to give

diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp
@@ -2002,14 +2002,17 @@ void RosFilter<T>::initialize()
     tf2::toMsg(tf2::Transform::getIdentity());
 
   // Position publisher
+  rclcpp::PublisherOptions publisher_options;
+  publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
   position_pub_ =
-    this->create_publisher<nav_msgs::msg::Odometry>("odometry/filtered", rclcpp::QoS(10));
+    this->create_publisher<nav_msgs::msg::Odometry>(
+    "odometry/filtered", rclcpp::QoS(10), publisher_options);
 
   // Optional acceleration publisher
   if (publish_acceleration_) {
     accel_pub_ =
       this->create_publisher<geometry_msgs::msg::AccelWithCovarianceStamped>(
-      "accel/filtered", rclcpp::QoS(10));
+      "accel/filtered", rclcpp::QoS(10), publisher_options);
   }
 
   const std::chrono::duration<double> timespan{1.0 / frequency_};

diff --git a/src/ros_robot_localization_listener.cpp b/src/ros_robot_localization_listener.cpp
@@ -75,11 +75,12 @@ FilterTypes::FilterType filterTypeFromString(
 }
 
 RosRobotLocalizationListener::RosRobotLocalizationListener(
-  rclcpp::Node::SharedPtr node)
+  rclcpp::Node::SharedPtr node,
+  rclcpp::SubscriptionOptions options)
 : qos1_(1),
   qos10_(10),
-  odom_sub_(node, "odom/filtered", qos1_.get_rmw_qos_profile()),
-  accel_sub_(node, "acceleration/filtered", qos1_.get_rmw_qos_profile()),
+  odom_sub_(node, "odom/filtered", qos1_.get_rmw_qos_profile(), options),
+  accel_sub_(node, "acceleration/filtered", qos1_.get_rmw_qos_profile(), options),
   sync_(odom_sub_, accel_sub_, 10u),
   node_clock_(node->get_node_clock_interface()->get_clock()),
   node_logger_(node->get_node_logging_interface()),