From 27a97e868cde2aaebf6c3d04e5b29b0e539bee4f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 8 Oct 2019 15:15:08 -0700 Subject: [PATCH] make get_actual_qos return a rclcpp::QoS (#883) * make get_actual_qos return a rclcpp::QoS Signed-off-by: William Woodall * make simpler following suggestion from review Signed-off-by: William Woodall --- rclcpp/include/rclcpp/publisher_base.hpp | 4 +++- rclcpp/include/rclcpp/subscription_base.hpp | 4 +++- rclcpp/src/rclcpp/publisher_base.cpp | 8 +++++--- rclcpp/src/rclcpp/subscription_base.cpp | 5 +++-- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 1901a27..a3ac1d7 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -29,6 +29,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/mapped_ring_buffer.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -153,10 +154,11 @@ public: * 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 + rclcpp::QoS get_actual_qos() const; /// Compare this publisher to a gid. diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 25b99b8..8cad2b7 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -27,6 +27,7 @@ #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -106,10 +107,11 @@ public: * 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 + rclcpp::QoS get_actual_qos() const; /// Borrow a new message. diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index a2c6cea..a5d142f 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -205,7 +205,7 @@ PublisherBase::get_intra_process_subscription_count() const return ipm->get_subscription_count(intra_process_publisher_id_); } -rmw_qos_profile_t +rclcpp::QoS PublisherBase::get_actual_qos() const { const rmw_qos_profile_t * qos = rcl_publisher_get_actual_qos(&publisher_handle_); @@ -214,7 +214,8 @@ PublisherBase::get_actual_qos() const rcl_reset_error(); throw std::runtime_error(msg); } - return *qos; + + return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); } bool @@ -264,7 +265,8 @@ PublisherBase::setup_intra_process( const rcl_publisher_options_t & intra_process_options) { // Intraprocess configuration is not allowed with "durability" qos policy non "volatile". - if (this->get_actual_qos().durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { + auto actual_durability = this->get_actual_qos().get_rmw_qos_profile().durability; + if (actual_durability != RMW_QOS_POLICY_DURABILITY_VOLATILE) { throw std::invalid_argument( "intraprocess communication is not allowed with durability qos policy non-volatile"); } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 8d9b205..0cdd1f5 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -129,7 +129,7 @@ SubscriptionBase::get_event_handlers() const return event_handlers_; } -rmw_qos_profile_t +rclcpp::QoS SubscriptionBase::get_actual_qos() const { const rmw_qos_profile_t * qos = rcl_subscription_get_actual_qos(subscription_handle_.get()); @@ -138,7 +138,8 @@ SubscriptionBase::get_actual_qos() const rcl_reset_error(); throw std::runtime_error(msg); } - return *qos; + + return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); } const rosidl_message_type_support_t &