From 2816e92f6b41fd4d7780e2cb2f383411edefb4d7 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 19 May 2021 17:24:21 -0700 Subject: [PATCH] Enable QoS overrides (#657) * Enable QoS overrides Signed-off-by: Audrow Nash * Use SubscriptionOptions Signed-off-by: Audrow Nash * Use with_default_policies Signed-off-by: Audrow Nash * Use with_default_policies in ros_filter Signed-off-by: Audrow Nash * Improve formatting Signed-off-by: Audrow Nash --- .../ros_robot_localization_listener.hpp | 17 ++++++++++++++++- src/navsat_transform.cpp | 19 ++++++++++++++----- src/ros_filter.cpp | 7 +++++-- src/ros_robot_localization_listener.cpp | 7 ++++--- 4 files changed, 39 insertions(+), 11 deletions(-) diff --git a/include/robot_localization/ros_robot_localization_listener.hpp b/include/robot_localization/ros_robot_localization_listener.hpp index 4f50d4a32..333c53800 100644 --- 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 index 1fa9b3cf3..eb7ec18d1 100644 --- 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( - "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( - "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( - "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("odometry/gps", rclcpp::QoS(10)); + this->create_publisher( + "odometry/gps", rclcpp::QoS(10), publisher_options); if (publish_gps_) { filtered_gps_pub_ = - this->create_publisher("gps/filtered", rclcpp::QoS(10)); + this->create_publisher( + "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 index 109e77f78..9bedaeb58 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -2002,14 +2002,17 @@ void RosFilter::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("odometry/filtered", rclcpp::QoS(10)); + this->create_publisher( + "odometry/filtered", rclcpp::QoS(10), publisher_options); // Optional acceleration publisher if (publish_acceleration_) { accel_pub_ = this->create_publisher( - "accel/filtered", rclcpp::QoS(10)); + "accel/filtered", rclcpp::QoS(10), publisher_options); } const std::chrono::duration timespan{1.0 / frequency_}; diff --git a/src/ros_robot_localization_listener.cpp b/src/ros_robot_localization_listener.cpp index da6917fcb..a6faeae32 100644 --- 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()),