From b66f07171e5b380bc864ec0d84beddf20c9ac8d1 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Mon, 1 Nov 2021 13:41:38 +0000 Subject: [PATCH] Add Serv/Cli request/response QoS getters Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 23 +++++++-- rclcpp/include/rclcpp/service.hpp | 22 +++++++-- rclcpp/src/rclcpp/client.cpp | 31 ++++++++++-- rclcpp/src/rclcpp/service.cpp | 31 ++++++++++-- rclcpp/test/rclcpp/test_client.cpp | 74 +++++++++++---------------- rclcpp/test/rclcpp/test_service.cpp | 77 ++++++++++++----------------- 6 files changed, 155 insertions(+), 103 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index e14d4d5f5c..3c31998d76 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -216,7 +216,7 @@ class ClientBase bool exchange_in_use_by_wait_set_state(bool in_use_state); - /// Get the actual QoS settings, after the defaults have been determined. + /// Get the actual request publsher 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 client, and it @@ -225,12 +225,29 @@ class ClientBase * it will be set to RMW_QOS_POLICY_*_UNKNOWN. * May throw runtime_error when an unexpected error occurs. * - * \return The actual qos settings. + * \return The actual request publsher qos settings. * \throws std::runtime_error if failed to get qos settings */ RCLCPP_PUBLIC rclcpp::QoS - get_actual_qos() const; + get_request_publisher_actual_qos() const; + + /// Get the actual response subscription 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 client, 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 response subscription qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_response_subscription_actual_qos() const; + protected: RCLCPP_DISABLE_COPY(ClientBase) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index bd2d2b469a..c67f98716c 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -122,7 +122,7 @@ class ServiceBase bool exchange_in_use_by_wait_set_state(bool in_use_state); - /// Get the actual QoS settings, after the defaults have been determined. + /// Get the actual response publisher 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 service, and it @@ -131,12 +131,28 @@ class ServiceBase * it will be set to RMW_QOS_POLICY_*_UNKNOWN. * May throw runtime_error when an unexpected error occurs. * - * \return The actual qos settings. + * \return The actual response publisher qos settings. * \throws std::runtime_error if failed to get qos settings */ RCLCPP_PUBLIC rclcpp::QoS - get_actual_qos() const; + get_response_publisher_actual_qos() const; + + /// Get the actual request subscription 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 service, 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 request subscription qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + get_request_subscription_actual_qos() const; protected: RCLCPP_DISABLE_COPY(ServiceBase) diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index e9d65d8d2b..83430ecd12 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -200,14 +200,37 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) } rclcpp::QoS -ClientBase::get_actual_qos() const +ClientBase::get_request_publisher_actual_qos() const { - const rmw_qos_profile_t * qos = rcl_client_get_actual_qos(client_handle_.get()); + const rmw_qos_profile_t * qos = + rcl_client_request_publisher_get_actual_qos(client_handle_.get()); if (!qos) { - auto msg = std::string("failed to get client qos settings: ") + rcl_get_error_string().str; + auto msg = std::string("failed to get client's request publisher qos settings: ") + + rcl_get_error_string().str; rcl_reset_error(); throw std::runtime_error(msg); } - return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + rclcpp::QoS request_publisher_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return request_publisher_qos; +} + +rclcpp::QoS +ClientBase::get_response_subscription_actual_qos() const +{ + const rmw_qos_profile_t * qos = + rcl_client_response_subscription_get_actual_qos(client_handle_.get()); + if (!qos) { + auto msg = std::string("failed to get client's response subscription qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS response_subscription_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return response_subscription_qos; } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index a483d47888..a6832c9be9 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -86,14 +86,37 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) } rclcpp::QoS -ServiceBase::get_actual_qos() const +ServiceBase::get_response_publisher_actual_qos() const { - const rmw_qos_profile_t * qos = rcl_service_get_actual_qos(service_handle_.get()); + const rmw_qos_profile_t* qos = + rcl_service_response_publisher_get_actual_qos(service_handle_.get()); if (!qos) { - auto msg = std::string("failed to get service qos settings: ") + rcl_get_error_string().str; + auto msg = std::string("failed to get service's response publisher qos settings: ") + + rcl_get_error_string().str; rcl_reset_error(); throw std::runtime_error(msg); } - return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + rclcpp::QoS response_publisher_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return response_publisher_qos; +} + +rclcpp::QoS +ServiceBase::get_request_subscription_actual_qos() const +{ + const rmw_qos_profile_t* qos = + rcl_service_request_subscription_get_actual_qos(service_handle_.get()); + if (!qos) { + auto msg = std::string("failed to get service's request subscription qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + rclcpp::QoS request_subscription_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(*qos), *qos); + + return request_subscription_qos; } diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 1a52ac70b0..c067059dee 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -342,58 +342,44 @@ TEST_F(TestClientWithServer, take_response) { } } -TEST_F(TestClient, rcl_client_get_actual_qos_error) { +TEST_F(TestClient, rcl_client_request_publisher_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_client_get_actual_qos, nullptr); - const std::string service_name = "service"; - auto client = node->create_client(service_name); + "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); + auto client = node->create_client("service"); RCLCPP_EXPECT_THROW_EQ( - client->get_actual_qos(), - std::runtime_error("failed to get client qos settings: error not set")); + client->get_request_publisher_actual_qos(), + std::runtime_error("failed to get client's request publisher qos settings: error not set")); } -TEST_F(TestClient, client_qos_error) { - rmw_qos_profile_t qos_profile = rmw_qos_profile_system_default; - EXPECT_THROW( - node->create_client("client", qos_profile), - rclcpp::exceptions::RCLError); +TEST_F(TestClient, rcl_client_response_subscription_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_response_subscription_get_actual_qos, nullptr); + auto client = node->create_client("service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_response_subscription_actual_qos(), + std::runtime_error("failed to get client's response subscription qos settings: error not set")); } TEST_F(TestClient, client_qos) { - rmw_qos_profile_t qos_profile; - qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default; qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos_profile.depth = 1; - uint64_t duration = 2; - qos_profile.deadline.sec = duration; - qos_profile.lifespan.sec = duration; - qos_profile.liveliness_lease_duration.sec = duration; - qos_profile.deadline.nsec = 0; - qos_profile.lifespan.nsec = 0; - qos_profile.liveliness_lease_duration.nsec = 0; - qos_profile.avoid_ros_namespace_conventions = false; - - auto client = node->create_client("client", qos_profile); - - EXPECT_EQ(rclcpp::ReliabilityPolicy::BestEffort, client->get_actual_qos().reliability()); - EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, client->get_actual_qos().durability()); - EXPECT_EQ(rclcpp::LivelinessPolicy::Automatic, client->get_actual_qos().liveliness()); - EXPECT_EQ(rclcpp::HistoryPolicy::KeepLast, client->get_actual_qos().history()); - EXPECT_EQ(qos_profile.depth, client->get_actual_qos().depth()); - EXPECT_EQ( - duration, - static_cast(client->get_actual_qos().deadline().seconds())); - EXPECT_EQ( - duration, - static_cast(client->get_actual_qos().lifespan().seconds())); - EXPECT_EQ( - duration, - static_cast(client->get_actual_qos().liveliness_lease_duration().seconds())); - EXPECT_EQ( - qos_profile.avoid_ros_namespace_conventions, - client->get_actual_qos().avoid_ros_namespace_conventions()); + uint64_t duration = 1; + qos_profile.deadline = {duration, duration}; + qos_profile.lifespan = {duration, duration}; + qos_profile.liveliness_lease_duration = {duration, duration}; + + auto client = + node->create_client("client", qos_profile); + + auto init_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile); + auto rp_qos = client->get_request_publisher_actual_qos(); + auto rs_qos = client->get_response_subscription_actual_qos(); + + EXPECT_EQ(init_qos, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan); + EXPECT_EQ(init_qos, rs_qos); } TEST_F(TestClient, client_qos_depth) { diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 351092012b..d8745bf184 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -237,66 +237,53 @@ TEST_F(TestService, send_response) { } } -TEST_F(TestService, rcl_service_get_actual_qos_error) { +TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_service_get_actual_qos, nullptr); + "lib:rclcpp", rcl_service_response_publisher_get_actual_qos, nullptr); auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto server = node->create_service("service", callback); RCLCPP_EXPECT_THROW_EQ( - server->get_actual_qos(), - std::runtime_error("failed to get service qos settings: error not set")); + server->get_response_publisher_actual_qos(), + std::runtime_error("failed to get service's response publisher qos settings: error not set")); } -TEST_F(TestService, server_qos_error) { - rmw_qos_profile_t qos_profile = rmw_qos_profile_system_default; - auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, +TEST_F(TestService, rcl_service_request_subscription_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_request_subscription_get_actual_qos, nullptr); + auto callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; - - EXPECT_THROW( - node->create_service("service", callback, qos_profile), - rclcpp::exceptions::RCLError); + auto server = node->create_service("service", callback); + RCLCPP_EXPECT_THROW_EQ( + server->get_request_subscription_actual_qos(), + std::runtime_error("failed to get service's request subscription qos settings: error not set")); } + TEST_F(TestService, server_qos) { - rmw_qos_profile_t qos_profile; - qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + rmw_qos_profile_t qos_profile = rmw_qos_profile_services_default; qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos_profile.depth = 1; - uint64_t duration = 2; - qos_profile.deadline.sec = duration; - qos_profile.lifespan.sec = duration; - qos_profile.liveliness_lease_duration.sec = duration; - qos_profile.deadline.nsec = 0; - qos_profile.lifespan.nsec = 0; - qos_profile.liveliness_lease_duration.nsec = 0; - qos_profile.avoid_ros_namespace_conventions = false; + uint64_t duration = 1; + qos_profile.deadline = {duration, duration}; + qos_profile.lifespan = {duration, duration}; + qos_profile.liveliness_lease_duration = {duration, duration}; auto callback = [](const test_msgs::srv::Empty::Request::SharedPtr, - test_msgs::srv::Empty::Response::SharedPtr) {}; - - auto server = node->create_service("service", callback, qos_profile); - - EXPECT_EQ(rclcpp::ReliabilityPolicy::BestEffort, server->get_actual_qos().reliability()); - EXPECT_EQ(rclcpp::DurabilityPolicy::Volatile, server->get_actual_qos().durability()); - EXPECT_EQ(rclcpp::LivelinessPolicy::Automatic, server->get_actual_qos().liveliness()); - EXPECT_EQ(rclcpp::HistoryPolicy::KeepLast, server->get_actual_qos().history()); - EXPECT_EQ(qos_profile.depth, server->get_actual_qos().depth()); - EXPECT_EQ( - duration, - static_cast(server->get_actual_qos().deadline().seconds())); - EXPECT_EQ( - duration, - static_cast(server->get_actual_qos().lifespan().seconds())); - EXPECT_EQ( - duration, - static_cast(server->get_actual_qos().liveliness_lease_duration().seconds())); - EXPECT_EQ( - qos_profile.avoid_ros_namespace_conventions, - server->get_actual_qos().avoid_ros_namespace_conventions()); + test_msgs::srv::Empty::Response::SharedPtr) {}; + + auto server = node->create_service("service", callback, + qos_profile); + auto init_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos_profile), qos_profile); + auto rs_qos = server->get_request_subscription_actual_qos(); + auto rp_qos = server->get_response_publisher_actual_qos(); + + EXPECT_EQ(init_qos, rp_qos); + // Lifespan has no meaning for subscription/readers + rs_qos.lifespan(qos_profile.lifespan); + EXPECT_EQ(init_qos, rs_qos); } TEST_F(TestService, server_qos_depth) {