From 7054e75bf35546c50b34859b70c477ccf6b96489 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Mon, 25 Mar 2019 18:39:27 -0300 Subject: [PATCH 1/4] Added get_actual_qos method to publisher. Signed-off-by: ivanpauno --- rclcpp/include/rclcpp/publisher.hpp | 6 ++++++ rclcpp/src/rclcpp/publisher.cpp | 26 +++++++++++++++++++++++++- 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 4500fedab5..5e05fefad4 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -128,6 +128,12 @@ class PublisherBase size_t get_intra_process_subscription_count() const; + /// Get actual qos settings + /** \return The number of subscriptions. */ + RCLCPP_PUBLIC + std::shared_ptr + 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..fefd529549 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,30 @@ PublisherBase::get_intra_process_subscription_count() const return ipm->get_subscription_count(intra_process_publisher_id_); } +std::shared_ptr +PublisherBase::get_actual_qos() const +{ + auto qos = std::make_shared(); + + rcl_ret_t status = rcl_publisher_get_actual_qos( + &publisher_handle_, + qos.get()); + if (RCL_RET_PUBLISHER_INVALID == status) { + rcl_reset_error(); /* next call will reset error message if not context */ + if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { + rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); + if (nullptr != context && !rcl_context_is_valid(context)) { + /* publisher is invalid due to context being shutdown */ + return 0; + } + } + } + if (RCL_RET_OK != status) { + rclcpp::exceptions::throw_from_rcl_error(status, "failed to get qos settings"); + } + return qos; +} + bool PublisherBase::operator==(const rmw_gid_t & gid) const { From d5f80b7f2b62f5f9b9a09408368c6ef6bbea95cf Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Tue, 26 Mar 2019 17:19:48 -0300 Subject: [PATCH 2/4] Corrected with PR comments. Signed-off-by: ivanpauno --- rclcpp/include/rclcpp/publisher.hpp | 12 +++++++++--- rclcpp/src/rclcpp/publisher.cpp | 16 +++------------- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index 5e05fefad4..d9455d2041 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -128,10 +128,16 @@ class PublisherBase size_t get_intra_process_subscription_count() const; - /// Get actual qos settings - /** \return The number of subscriptions. */ + /// 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. + * \return The actual qos settings. */ RCLCPP_PUBLIC - std::shared_ptr + rmw_qos_profile_t get_actual_qos() const; /// Compare this publisher to a gid. diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index fefd529549..cf0966faee 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -197,24 +197,14 @@ PublisherBase::get_intra_process_subscription_count() const return ipm->get_subscription_count(intra_process_publisher_id_); } -std::shared_ptr +rmw_qos_profile_t PublisherBase::get_actual_qos() const { - auto qos = std::make_shared(); + rmw_qos_profile_t qos; rcl_ret_t status = rcl_publisher_get_actual_qos( &publisher_handle_, - qos.get()); - if (RCL_RET_PUBLISHER_INVALID == status) { - rcl_reset_error(); /* next call will reset error message if not context */ - if (rcl_publisher_is_valid_except_context(&publisher_handle_)) { - rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_); - if (nullptr != context && !rcl_context_is_valid(context)) { - /* publisher is invalid due to context being shutdown */ - return 0; - } - } - } + &qos); if (RCL_RET_OK != status) { rclcpp::exceptions::throw_from_rcl_error(status, "failed to get qos settings"); } From fc870068c0db647d17cd4dd0e6d4f37c0d377db5 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Thu, 28 Mar 2019 18:06:41 -0300 Subject: [PATCH 3/4] Updated get_actual_qos with rcl method change Signed-off-by: ivanpauno --- rclcpp/src/rclcpp/publisher.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index cf0966faee..275e145a52 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -200,15 +200,13 @@ PublisherBase::get_intra_process_subscription_count() const rmw_qos_profile_t PublisherBase::get_actual_qos() const { - rmw_qos_profile_t qos; - - rcl_ret_t status = rcl_publisher_get_actual_qos( - &publisher_handle_, - &qos); - if (RCL_RET_OK != status) { - rclcpp::exceptions::throw_from_rcl_error(status, "failed to get qos settings"); + 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; + return *qos; } bool From 15cc25b1856a82eeed65863514da3eedcd28bc45 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Mon, 1 Apr 2019 17:14:23 -0300 Subject: [PATCH 4/4] Corrected with PR comments Signed-off-by: ivanpauno --- rclcpp/include/rclcpp/publisher.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index d9455d2041..27a417730c 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -135,7 +135,9 @@ class PublisherBase * 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. - * \return The actual qos settings. */ + * May throw runtime_error when an unexpected error occurs. + * \return The actual qos settings. + */ RCLCPP_PUBLIC rmw_qos_profile_t get_actual_qos() const;