From b2a94f91f111771d6c9fb386e609584f7e133275 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Mon, 1 Apr 2019 17:55:08 -0300 Subject: [PATCH] Add function to get publisher actual qos settings (#667) * Added get_actual_qos method to publisher. Signed-off-by: ivanpauno --- rclcpp/include/rclcpp/publisher.hpp | 14 ++++++++++++++ rclcpp/src/rclcpp/publisher.cpp | 14 +++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 4500fedab5..27a417730c 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -128,6 +128,20 @@ class PublisherBase size_t get_intra_process_subscription_count() const; + /// Get the actual QoS settings, after the defaults have been determined. + /** + * The actual configuration applied when using RMW_QOS_POLICY_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the publisher, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_QOS_POLICY_*_UNKNOWN. + * May throw runtime_error when an unexpected error occurs. + * \return The actual qos settings. + */ + RCLCPP_PUBLIC + rmw_qos_profile_t + get_actual_qos() const; + /// Compare this publisher to a gid. /** * Note that this function calls the next function. diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index ce6ebadaee..275e145a52 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -159,7 +159,7 @@ PublisherBase::get_subscription_count() const { size_t inter_process_subscription_count = 0; - rmw_ret_t status = rcl_publisher_get_subscription_count( + rcl_ret_t status = rcl_publisher_get_subscription_count( &publisher_handle_, &inter_process_subscription_count); @@ -197,6 +197,18 @@ PublisherBase::get_intra_process_subscription_count() const return ipm->get_subscription_count(intra_process_publisher_id_); } +rmw_qos_profile_t +PublisherBase::get_actual_qos() const +{ + const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_); + if (!qos) { + auto msg = std::string("failed to get qos settings: ") + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + return *qos; +} + bool PublisherBase::operator==(const rmw_gid_t & gid) const {