From 6f7e02cbd7827c03ca2527635b285d0d077d68fe Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Thu, 29 Apr 2021 11:45:09 -0700 Subject: [PATCH 1/5] Enable QoS overrides Signed-off-by: Audrow Nash --- .../ros_robot_localization_listener.hpp | 26 +++++++++++- src/navsat_transform.cpp | 41 ++++++++++++++++--- src/ros_filter.cpp | 18 +++++++- src/ros_robot_localization_listener.cpp | 7 ++-- 4 files changed, 81 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..ab4c0a157 100644 --- a/include/robot_localization/ros_robot_localization_listener.hpp +++ b/include/robot_localization/ros_robot_localization_listener.hpp @@ -49,6 +49,27 @@ namespace robot_localization { +namespace detail +{ +rclcpp::SubscriptionOptionsWithAllocator> +get_subscription_options_for_qos_override() +{ + auto subscription_options = rclcpp::SubscriptionOptionsWithAllocator>(); + subscription_options.qos_overriding_options = rclcpp::QosOverridingOptions {{ + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, + rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, + rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability, + }}; + return subscription_options; +} +} // namespace detail + //! @brief RosRobotLocalizationListener class //! //! This class wraps the RobotLocalizationEstimator. It listens to topics over @@ -70,7 +91,10 @@ class RosRobotLocalizationListener //! //! @param[in] node - rclcpp node shared pointer //! - explicit RosRobotLocalizationListener(rclcpp::Node::SharedPtr node); + explicit RosRobotLocalizationListener( + rclcpp::Node::SharedPtr node, + rclcpp::SubscriptionOptionsWithAllocator> options = + detail::get_subscription_options_for_qos_override()); //! @brief Destructor //! diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index 1fa9b3cf3..75b9aa24a 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -141,23 +141,54 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(1)); + auto subscriber_options = rclcpp::SubscriptionOptionsWithAllocator>(); + subscriber_options.qos_overriding_options = rclcpp::QosOverridingOptions {{ + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, + rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, + rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability, + }}; 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 {{ + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, + rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, + rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability, + }}; 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..2f7e50672 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -2002,14 +2002,28 @@ void RosFilter::initialize() tf2::toMsg(tf2::Transform::getIdentity()); // Position publisher + rclcpp::PublisherOptions publisher_options; + publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions {{ + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, + rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, + rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability, + }}; 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..b0457e1e8 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::SubscriptionOptionsWithAllocator> 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()), From d448e67fb0ef784666d56175f79afcb98ea54f00 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 19 May 2021 09:21:04 -0700 Subject: [PATCH 2/5] Use SubscriptionOptions Signed-off-by: Audrow Nash --- .../robot_localization/ros_robot_localization_listener.hpp | 7 +++---- src/navsat_transform.cpp | 2 +- src/ros_robot_localization_listener.cpp | 2 +- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/include/robot_localization/ros_robot_localization_listener.hpp b/include/robot_localization/ros_robot_localization_listener.hpp index ab4c0a157..e53cfcd34 100644 --- a/include/robot_localization/ros_robot_localization_listener.hpp +++ b/include/robot_localization/ros_robot_localization_listener.hpp @@ -51,10 +51,10 @@ namespace robot_localization namespace detail { -rclcpp::SubscriptionOptionsWithAllocator> +rclcpp::SubscriptionOptions get_subscription_options_for_qos_override() { - auto subscription_options = rclcpp::SubscriptionOptionsWithAllocator>(); + auto subscription_options = rclcpp::SubscriptionOptions(); subscription_options.qos_overriding_options = rclcpp::QosOverridingOptions {{ rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, rclcpp::QosPolicyKind::Deadline, @@ -93,8 +93,7 @@ class RosRobotLocalizationListener //! explicit RosRobotLocalizationListener( rclcpp::Node::SharedPtr node, - rclcpp::SubscriptionOptionsWithAllocator> options = - detail::get_subscription_options_for_qos_override()); + rclcpp::SubscriptionOptions options = detail::get_subscription_options_for_qos_override()); //! @brief Destructor //! diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index 75b9aa24a..74cf03fec 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -141,7 +141,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(1)); - auto subscriber_options = rclcpp::SubscriptionOptionsWithAllocator>(); + auto subscriber_options = rclcpp::SubscriptionOptions(); subscriber_options.qos_overriding_options = rclcpp::QosOverridingOptions {{ rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, rclcpp::QosPolicyKind::Deadline, diff --git a/src/ros_robot_localization_listener.cpp b/src/ros_robot_localization_listener.cpp index b0457e1e8..a6faeae32 100644 --- a/src/ros_robot_localization_listener.cpp +++ b/src/ros_robot_localization_listener.cpp @@ -76,7 +76,7 @@ FilterTypes::FilterType filterTypeFromString( RosRobotLocalizationListener::RosRobotLocalizationListener( rclcpp::Node::SharedPtr node, - rclcpp::SubscriptionOptionsWithAllocator> options) + rclcpp::SubscriptionOptions options) : qos1_(1), qos10_(10), odom_sub_(node, "odom/filtered", qos1_.get_rmw_qos_profile(), options), From 0468fd036064c0d000e64a1e3e942b031f91611c Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 19 May 2021 15:30:24 -0700 Subject: [PATCH 3/5] Use with_default_policies Signed-off-by: Audrow Nash --- .../ros_robot_localization_listener.hpp | 18 ++++--------- src/navsat_transform.cpp | 25 +++---------------- 2 files changed, 8 insertions(+), 35 deletions(-) diff --git a/include/robot_localization/ros_robot_localization_listener.hpp b/include/robot_localization/ros_robot_localization_listener.hpp index e53cfcd34..333c53800 100644 --- a/include/robot_localization/ros_robot_localization_listener.hpp +++ b/include/robot_localization/ros_robot_localization_listener.hpp @@ -52,20 +52,11 @@ namespace robot_localization namespace detail { rclcpp::SubscriptionOptions -get_subscription_options_for_qos_override() +get_subscription_options_with_default_qos_override_policies() { auto subscription_options = rclcpp::SubscriptionOptions(); - subscription_options.qos_overriding_options = rclcpp::QosOverridingOptions {{ - rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, - rclcpp::QosPolicyKind::Deadline, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Lifespan, - rclcpp::QosPolicyKind::Liveliness, - rclcpp::QosPolicyKind::LivelinessLeaseDuration, - rclcpp::QosPolicyKind::Reliability, - }}; + subscription_options.qos_overriding_options = + rclcpp::QosOverridingOptions::with_default_policies(); return subscription_options; } } // namespace detail @@ -93,7 +84,8 @@ class RosRobotLocalizationListener //! explicit RosRobotLocalizationListener( rclcpp::Node::SharedPtr node, - rclcpp::SubscriptionOptions options = detail::get_subscription_options_for_qos_override()); + 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 74cf03fec..dd68e8f76 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -142,17 +142,8 @@ 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 {{ - rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, - rclcpp::QosPolicyKind::Deadline, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Lifespan, - rclcpp::QosPolicyKind::Liveliness, - rclcpp::QosPolicyKind::LivelinessLeaseDuration, - rclcpp::QosPolicyKind::Reliability, - }}; + subscriber_options.qos_overriding_options = + rclcpp::QosOverridingOptions::with_default_policies(); odom_sub_ = this->create_subscription( "odometry/filtered", custom_qos, std::bind( &NavSatTransform::odomCallback, this, @@ -168,17 +159,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) } rclcpp::PublisherOptions publisher_options; - publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions {{ - rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, - rclcpp::QosPolicyKind::Deadline, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Lifespan, - rclcpp::QosPolicyKind::Liveliness, - rclcpp::QosPolicyKind::LivelinessLeaseDuration, - rclcpp::QosPolicyKind::Reliability, - }}; + publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); gps_odom_pub_ = this->create_publisher( "odometry/gps", rclcpp::QoS( From 7f17ef98e79ec9fc96cce11bbc8399871a6e2fe1 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 19 May 2021 15:45:40 -0700 Subject: [PATCH 4/5] Use with_default_policies in ros_filter Signed-off-by: Audrow Nash --- src/ros_filter.cpp | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 2f7e50672..9bedaeb58 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -2003,21 +2003,10 @@ void RosFilter::initialize() // Position publisher rclcpp::PublisherOptions publisher_options; - publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions {{ - rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, - rclcpp::QosPolicyKind::Deadline, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Durability, - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Lifespan, - rclcpp::QosPolicyKind::Liveliness, - rclcpp::QosPolicyKind::LivelinessLeaseDuration, - rclcpp::QosPolicyKind::Reliability, - }}; + publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); position_pub_ = this->create_publisher( - "odometry/filtered", rclcpp::QoS( - 10), publisher_options); + "odometry/filtered", rclcpp::QoS(10), publisher_options); // Optional acceleration publisher if (publish_acceleration_) { From dce0fa503605c847d7199ef282b8de50dfde3126 Mon Sep 17 00:00:00 2001 From: Audrow Nash Date: Wed, 19 May 2021 15:46:02 -0700 Subject: [PATCH 5/5] Improve formatting Signed-off-by: Audrow Nash --- src/navsat_transform.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index dd68e8f76..eb7ec18d1 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -146,8 +146,7 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) rclcpp::QosOverridingOptions::with_default_policies(); odom_sub_ = this->create_subscription( "odometry/filtered", custom_qos, std::bind( - &NavSatTransform::odomCallback, this, - _1), subscriber_options); + &NavSatTransform::odomCallback, this, _1), subscriber_options); gps_sub_ = this->create_subscription( "gps/fix", custom_qos, std::bind(&NavSatTransform::gpsFixCallback, this, _1), @@ -162,14 +161,12 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); gps_odom_pub_ = this->create_publisher( - "odometry/gps", rclcpp::QoS( - 10), publisher_options); + "odometry/gps", rclcpp::QoS(10), publisher_options); if (publish_gps_) { filtered_gps_pub_ = this->create_publisher( - "gps/filtered", rclcpp::QoS( - 10), publisher_options); + "gps/filtered", rclcpp::QoS(10), publisher_options); } // Sleep for the parameterized amount of time, to give