From 1237ba19eea086646f3953ce69d35a27ca3b5fcc Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Fri, 24 Sep 2021 17:33:23 +0100 Subject: [PATCH] Add client/service QoS getters Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 33 +++++++ rclcpp/include/rclcpp/service.hpp | 33 +++++++ rclcpp/include/rclcpp/subscription_base.hpp | 2 +- rclcpp/src/rclcpp/client.cpp | 38 ++++++++ rclcpp/src/rclcpp/service.cpp | 38 ++++++++ rclcpp/test/rclcpp/test_client.cpp | 103 ++++++++++++++++++++ rclcpp/test/rclcpp/test_service.cpp | 101 +++++++++++++++++++ 7 files changed, 347 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 1fc761f491..84cf1c1cd6 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -35,6 +35,7 @@ #include "rclcpp/function_traits.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" @@ -215,6 +216,38 @@ class ClientBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// 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 + * 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 publsher qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + 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 78fc5e048f..c67f98716c 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -28,6 +28,7 @@ #include "rclcpp/any_service_callback.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" +#include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" @@ -121,6 +122,38 @@ class ServiceBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// 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 + * 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 publisher qos settings. + * \throws std::runtime_error if failed to get qos settings + */ + RCLCPP_PUBLIC + rclcpp::QoS + 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/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 7653292b5e..72ddc23be2 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -107,7 +107,7 @@ class SubscriptionBase : public std::enable_shared_from_this /// 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 + * can only be resolved after the creation of the subscription, 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. diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6a1d3934d4..5da3dc9438 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -198,3 +198,41 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +rclcpp::QoS +ClientBase::get_request_publisher_actual_qos() const +{ + 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's request publisher qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + 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 827062bdd3..60d02ad29f 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -84,3 +84,41 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +rclcpp::QoS +ServiceBase::get_response_publisher_actual_qos() const +{ + 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's response publisher qos settings: ") + + rcl_get_error_string().str; + rcl_reset_error(); + throw std::runtime_error(msg); + } + + 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 5e2adb5e7d..13cc5e399a 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -24,6 +24,7 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/srv/empty.hpp" @@ -340,3 +341,105 @@ TEST_F(TestClientWithServer, take_response) { rclcpp::exceptions::RCLError); } } + +TEST_F(TestClient, rcl_client_request_publisher_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_client_request_publisher_get_actual_qos, nullptr); + auto client = node->create_client("service"); + RCLCPP_EXPECT_THROW_EQ( + client->get_request_publisher_actual_qos(), + std::runtime_error("failed to get client's request publisher qos settings: error not set")); +} + +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 = rmw_qos_profile_services_default; + qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + 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) { + using namespace std::literals::chrono_literals; + + rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default; + client_qos_profile.depth = 2; + + auto client = node->create_client("test_qos_depth", client_qos_profile); + + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default; + + auto server = server_node->create_service( + "test_qos_depth", std::move(server_callback), server_qos_profile); + + auto request = std::make_shared(); + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + + using SharedFuture = rclcpp::Client::SharedFuture; + uint64_t client_cb_count_ = 0; + auto client_callback = [&client_cb_count_, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + client_cb_count_++; + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < client_requests) && + (std::chrono::steady_clock::now() - start) < 2s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(2ms); + } + + EXPECT_GT(server_cb_count_, client_qos_profile.depth); + + start = std::chrono::steady_clock::now(); + while ((client_cb_count_ < client_qos_profile.depth) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(node); + } + + // Spin an extra time to check if client QoS depth has been ignored, + // so more client callbacks might be called than expected. + rclcpp::spin_some(node); + + EXPECT_EQ(client_cb_count_, client_qos_profile.depth); +} diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 16ab31cf1b..7eed9e0161 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -22,6 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" #include "test_msgs/srv/empty.hpp" @@ -235,3 +236,103 @@ TEST_F(TestService, send_response) { rclcpp::exceptions::RCLError); } } + +TEST_F(TestService, rcl_service_response_publisher_get_actual_qos_error) { + auto mock = mocking_utils::patch_and_return( + "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_response_publisher_actual_qos(), + std::runtime_error("failed to get service's response publisher qos settings: error not set")); +} + +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) {}; + 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 = rmw_qos_profile_services_default; + qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + 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); + 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) { + using namespace std::literals::chrono_literals; + + uint64_t server_cb_count_ = 0; + auto server_callback = [&]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_cb_count_++;}; + + auto server_node = std::make_shared("server_node", "/ns"); + + rmw_qos_profile_t server_qos_profile = rmw_qos_profile_default; + server_qos_profile.depth = 2; + + auto server = server_node->create_service( + "test_qos_depth", std::move(server_callback), server_qos_profile); + + rmw_qos_profile_t client_qos_profile = rmw_qos_profile_default; + auto client = node->create_client("test_qos_depth", client_qos_profile); + + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto request = std::make_shared(); + + using SharedFuture = rclcpp::Client::SharedFuture; + auto client_callback = [&request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + }; + + uint64_t client_requests = 5; + for (uint64_t i = 0; i < client_requests; i++) { + client->async_send_request(request, client_callback); + std::this_thread::sleep_for(10ms); + } + + auto start = std::chrono::steady_clock::now(); + while ((server_cb_count_ < server_qos_profile.depth) && + (std::chrono::steady_clock::now() - start) < 1s) + { + rclcpp::spin_some(server_node); + std::this_thread::sleep_for(1ms); + } + + // Spin an extra time to check if server QoS depth has been ignored, + // so more server responses might be processed than expected. + rclcpp::spin_some(server_node); + + EXPECT_EQ(server_cb_count_, server_qos_profile.depth); +}