From a94f6b32bdd55207c40819fd15420de5b30f6b91 Mon Sep 17 00:00:00 2001 From: brawner Date: Mon, 22 Jun 2020 14:25:51 -0700 Subject: [PATCH 01/61] Throw exception if rcl_timer_init fails (#1179) * Throw exception if rcl_timer_init fails Signed-off-by: Stephen Brawner * Add bad-argument tests for GenericTimer Signed-off-by: Stephen Brawner * Add comments Signed-off-by: Stephen Brawner * Address feedback Signed-off-by: Stephen Brawner * Address feedback Signed-off-by: Stephen Brawner --- rclcpp/src/rclcpp/timer.cpp | 14 ++++------- rclcpp/test/rclcpp/test_timer.cpp | 40 +++++++++++++++++++++++++++++++ 2 files changed, 45 insertions(+), 9 deletions(-) diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 9ece178c28..50df28baaa 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -61,15 +61,11 @@ TimerBase::TimerBase( rcl_clock_t * clock_handle = clock_->get_clock_handle(); { std::lock_guard clock_guard(clock_->get_clock_mutex()); - if ( - rcl_timer_init( - timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, - rcl_get_default_allocator()) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR_NAMED( - "rclcpp", - "Couldn't initialize rcl timer handle: %s\n", rcl_get_error_string().str); - rcl_reset_error(); + rcl_ret_t ret = rcl_timer_init( + timer_handle_.get(), clock_handle, rcl_context.get(), period.count(), nullptr, + rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't initialize rcl timer handle"); } } } diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 2d9022e045..7f85a1c4c5 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rcl/timer.h" @@ -151,3 +152,42 @@ TEST_F(TestTimer, test_run_cancel_timer) EXPECT_TRUE(has_timer_run.load()); EXPECT_TRUE(timer->is_canceled()); } + +TEST_F(TestTimer, test_bad_arguments) { + auto node_base = rclcpp::node_interfaces::get_node_base_interface(test_node); + auto context = node_base->get_context(); + + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + + // Negative period + EXPECT_THROW( + rclcpp::GenericTimer(steady_clock, -1ms, []() {}, context), + rclcpp::exceptions::RCLInvalidArgument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::GenericTimer( + steady_clock, nanoseconds_min, []() {}, context), + rclcpp::exceptions::RCLInvalidArgument); + + // nanoseconds max, should be ok + constexpr auto nanoseconds_max = std::chrono::nanoseconds::max(); + EXPECT_NO_THROW( + rclcpp::GenericTimer( + steady_clock, nanoseconds_max, []() {}, context)); + + // 0 duration period, should be ok + EXPECT_NO_THROW( + rclcpp::GenericTimer(steady_clock, 0ms, []() {}, context)); + + // context is null, which resorts to default + EXPECT_NO_THROW( + rclcpp::GenericTimer(steady_clock, 1ms, []() {}, nullptr)); + + // Clock is unitialized + auto unitialized_clock = std::make_shared(RCL_CLOCK_UNINITIALIZED); + EXPECT_THROW( + rclcpp::GenericTimer(unitialized_clock, 1us, []() {}, context), + rclcpp::exceptions::RCLError); +} From d9320761609aeb268e79404b9363c58bb6f5aada Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 18 Jun 2020 14:00:31 -0700 Subject: [PATCH 02/61] Check period duration in create_wall_timer (#1178) * Check period duration in create_wall_timer Signed-off-by: Stephen Brawner * Adding comments Signed-off-by: Stephen Brawner --- rclcpp/include/rclcpp/create_timer.hpp | 38 +++++++++++++--- rclcpp/test/rclcpp/test_create_timer.cpp | 57 +++++++++++++++++++++++- 2 files changed, 89 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/create_timer.hpp b/rclcpp/include/rclcpp/create_timer.hpp index f7b810dc4d..345f43fcbb 100644 --- a/rclcpp/include/rclcpp/create_timer.hpp +++ b/rclcpp/include/rclcpp/create_timer.hpp @@ -76,14 +76,14 @@ create_timer( * \tparam DurationRepT * \tparam DurationT * \tparam CallbackT - * \param period period to exectute callback + * \param period period to execute callback. This duration must be 0 <= period < nanoseconds::max() * \param callback callback to execute via the timer period * \param group * \param node_base * \param node_timers * \return * \throws std::invalid argument if either node_base or node_timers - * are null + * are null, or period is negative or too large */ template typename rclcpp::WallTimer::SharedPtr @@ -102,10 +102,38 @@ create_wall_timer( throw std::invalid_argument{"input node_timers cannot be null"}; } + if (period < std::chrono::duration::zero()) { + throw std::invalid_argument{"timer period cannot be negative"}; + } + + // Casting to a double representation might lose precision and allow the check below to succeed + // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max. + constexpr auto maximum_safe_cast_ns = + std::chrono::nanoseconds::max() - std::chrono::duration(1); + + // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow + // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is + // greater than nanoseconds::max() is a difficult general problem. This is a more conservative + // version of Howard Hinnant's (the guy>) response here: + // https://stackoverflow.com/a/44637334/2089061 + // However, this doesn't solve the issue for all possible duration types of period. + // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177 + constexpr auto ns_max_as_double = + std::chrono::duration_cast>( + maximum_safe_cast_ns); + if (period > ns_max_as_double) { + throw std::invalid_argument{ + "timer period must be less than std::chrono::nanoseconds::max()"}; + } + + const auto period_ns = std::chrono::duration_cast(period); + if (period_ns < std::chrono::nanoseconds::zero()) { + throw std::runtime_error{ + "Casting timer period to nanoseconds resulted in integer overflow."}; + } + auto timer = rclcpp::WallTimer::make_shared( - std::chrono::duration_cast(period), - std::move(callback), - node_base->get_context()); + period_ns, std::move(callback), node_base->get_context()); node_timers->add_timer(timer, group); return timer; } diff --git a/rclcpp/test/rclcpp/test_create_timer.cpp b/rclcpp/test/rclcpp/test_create_timer.cpp index cff870a8a4..f6acd79b24 100644 --- a/rclcpp/test/rclcpp/test_create_timer.cpp +++ b/rclcpp/test/rclcpp/test_create_timer.cpp @@ -18,10 +18,10 @@ #include #include +#include "node_interfaces/node_wrapper.hpp" #include "rclcpp/create_timer.hpp" #include "rclcpp/executors.hpp" #include "rclcpp/node.hpp" -#include "node_interfaces/node_wrapper.hpp" using namespace std::chrono_literals; @@ -61,3 +61,58 @@ TEST(TestCreateTimer, call_with_node_wrapper_compiles) []() {}); rclcpp::shutdown(); } + +TEST(TestCreateTimer, call_wall_timer_with_bad_arguments) +{ + rclcpp::init(0, nullptr); + NodeWrapper node("test_create_wall_timers_with_bad_arguments"); + auto callback = []() {}; + rclcpp::CallbackGroup::SharedPtr group = nullptr; + auto node_interface = + rclcpp::node_interfaces::get_node_base_interface(node).get(); + auto timers_interface = + rclcpp::node_interfaces::get_node_timers_interface(node).get(); + + // Negative period + EXPECT_THROW( + rclcpp::create_wall_timer(-1ms, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Very negative period + constexpr auto nanoseconds_min = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_wall_timer( + nanoseconds_min, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // Period must be less than nanoseconds::max() + constexpr auto nanoseconds_max = std::chrono::nanoseconds::min(); + EXPECT_THROW( + rclcpp::create_wall_timer( + nanoseconds_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + EXPECT_NO_THROW( + rclcpp::create_wall_timer( + nanoseconds_max - 1us, callback, group, node_interface, timers_interface)); + + EXPECT_NO_THROW( + rclcpp::create_wall_timer(0ms, callback, group, node_interface, timers_interface)); + + // Period must be less than nanoseconds::max() + constexpr auto hours_max = std::chrono::hours::max(); + EXPECT_THROW( + rclcpp::create_wall_timer(hours_max, callback, group, node_interface, timers_interface), + std::invalid_argument); + + // node_interface is null + EXPECT_THROW( + rclcpp::create_wall_timer(1ms, callback, group, nullptr, timers_interface), + std::invalid_argument); + + // timers_interface is null + EXPECT_THROW( + rclcpp::create_wall_timer(1ms, callback, group, node_interface, nullptr), + std::invalid_argument); + rclcpp::shutdown(); +} From 0ee47efbaa192887e0b2644e8e66353c11248137 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 2 Jul 2020 16:08:40 -0300 Subject: [PATCH 03/61] Fix rclcpp::NodeOptions::operator= (#1211) Signed-off-by: Michel Hidalgo --- rclcpp/src/rclcpp/node_options.cpp | 1 + rclcpp/test/rclcpp/test_node_options.cpp | 30 ++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index 3a2dcd7f5c..0f4f274f01 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -79,6 +79,7 @@ NodeOptions::operator=(const NodeOptions & other) this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_; this->automatically_declare_parameters_from_overrides_ = other.automatically_declare_parameters_from_overrides_; + this->node_options_.reset(); } return *this; } diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 17d108f92e..8543d9b975 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -168,3 +168,33 @@ TEST(TestNodeOptions, enable_rosout) { EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout); } } + +TEST(TestNodeOptions, copy) { + std::vector expected_args{"--unknown-flag", "arg"}; + auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false); + const rcl_node_options_t * rcl_options = options.get_rcl_node_options(); + + { + rclcpp::NodeOptions copied_options = options; + EXPECT_FALSE(copied_options.use_global_arguments()); + EXPECT_EQ(expected_args, copied_options.arguments()); + const rcl_node_options_t * copied_rcl_options = copied_options.get_rcl_node_options(); + EXPECT_EQ(copied_rcl_options->use_global_arguments, rcl_options->use_global_arguments); + EXPECT_EQ( + rcl_arguments_get_count_unparsed(&copied_rcl_options->arguments), + rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + } + + { + auto other_options = rclcpp::NodeOptions().use_global_arguments(true); + (void)other_options.get_rcl_node_options(); // force C structure initialization + other_options = options; + EXPECT_FALSE(other_options.use_global_arguments()); + EXPECT_EQ(expected_args, other_options.arguments()); + const rcl_node_options_t * other_rcl_options = other_options.get_rcl_node_options(); + EXPECT_EQ(other_rcl_options->use_global_arguments, rcl_options->use_global_arguments); + EXPECT_EQ( + rcl_arguments_get_count_unparsed(&other_rcl_options->arguments), + rcl_arguments_get_count_unparsed(&rcl_options->arguments)); + } +} From 7cb96ede466ce2c98713f02905274817f3bb7653 Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 19 Jun 2020 13:22:06 -0700 Subject: [PATCH 04/61] Add unit tests for logging functionality (#1184) Signed-off-by: Stephen Brawner --- rclcpp/test/rclcpp/test_logger.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/rclcpp/test/rclcpp/test_logger.cpp b/rclcpp/test/rclcpp/test_logger.cpp index 258cdd0959..3eba40f20e 100644 --- a/rclcpp/test/rclcpp/test_logger.cpp +++ b/rclcpp/test/rclcpp/test_logger.cpp @@ -14,10 +14,12 @@ #include +#include #include #include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "rclcpp/node.hpp" TEST(TestLogger, factory_functions) { rclcpp::Logger logger = rclcpp::get_logger("test_logger"); @@ -33,3 +35,15 @@ TEST(TestLogger, hierarchy) { rclcpp::Logger subsublogger = sublogger.get_child("grandchild"); EXPECT_STREQ("test_logger.child.grandchild", subsublogger.get_name()); } + +TEST(TestLogger, get_node_logger) { + rclcpp::init(0, nullptr); + auto node = std::make_shared("my_node", "/ns"); + auto node_base = rclcpp::node_interfaces::get_node_base_interface(node); + auto logger = rclcpp::get_node_logger(node_base->get_rcl_node_handle()); + EXPECT_STREQ(logger.get_name(), "ns.my_node"); + + logger = rclcpp::get_node_logger(nullptr); + EXPECT_STREQ(logger.get_name(), "rclcpp"); + rclcpp::shutdown(); +} From ba38c4f54c3cb5a29e5eff8d259da75492331a3e Mon Sep 17 00:00:00 2001 From: brawner Date: Mon, 22 Jun 2020 13:37:54 -0700 Subject: [PATCH 05/61] Unit tests for some header-only functions/classes (#1181) * Unit tests for header-only functions/classes Adds coverage for: * any_service_callback.hpp * any_subscription_callback.hpp * create_subscription.hpp * create_timer.hpp Signed-off-by: Stephen Brawner * Address PR feedback Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 21 ++ .../test/rclcpp/test_any_service_callback.cpp | 78 +++++++ .../rclcpp/test_any_subscription_callback.cpp | 205 ++++++++++++++++++ .../test/rclcpp/test_create_subscription.cpp | 67 ++++++ 4 files changed, 371 insertions(+) create mode 100644 rclcpp/test/rclcpp/test_any_service_callback.cpp create mode 100644 rclcpp/test/rclcpp/test_any_subscription_callback.cpp create mode 100644 rclcpp/test/rclcpp/test_create_subscription.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index d500f3e58a..46fe7c15d6 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -17,6 +17,20 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs SKIP_INSTALL ) +ament_add_gtest(test_any_service_callback rclcpp/test_any_service_callback.cpp) +if(TARGET test_any_service_callback) + ament_target_dependencies(test_any_service_callback + "test_msgs" + ) + target_link_libraries(test_any_service_callback ${PROJECT_NAME}) +endif() +ament_add_gtest(test_any_subscription_callback rclcpp/test_any_subscription_callback.cpp) +if(TARGET test_any_subscription_callback) + ament_target_dependencies(test_any_subscription_callback + "test_msgs" + ) + target_link_libraries(test_any_subscription_callback ${PROJECT_NAME}) +endif() ament_add_gtest(test_client rclcpp/test_client.cpp) if(TARGET test_client) ament_target_dependencies(test_client @@ -39,6 +53,13 @@ if(TARGET test_create_timer) target_link_libraries(test_create_timer ${PROJECT_NAME}) target_include_directories(test_create_timer PRIVATE rclcpp/) endif() +ament_add_gtest(test_create_subscription rclcpp/test_create_subscription.cpp) +if(TARGET test_create_subscription) + target_link_libraries(test_create_subscription ${PROJECT_NAME}) + ament_target_dependencies(test_create_subscription + "test_msgs" + ) +endif() ament_add_gtest(test_expand_topic_or_service_name rclcpp/test_expand_topic_or_service_name.cpp) if(TARGET test_expand_topic_or_service_name) ament_target_dependencies(test_expand_topic_or_service_name diff --git a/rclcpp/test/rclcpp/test_any_service_callback.cpp b/rclcpp/test/rclcpp/test_any_service_callback.cpp new file mode 100644 index 0000000000..d16c11921e --- /dev/null +++ b/rclcpp/test/rclcpp/test_any_service_callback.cpp @@ -0,0 +1,78 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// This file includes basic API tests for the AnyServiceCallback class. +// It is also tested in test_externally_defined_services.cpp + +#include + +#include +#include +#include + +#include "rclcpp/any_service_callback.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/empty.h" + +class TestAnyServiceCallback : public ::testing::Test +{ +public: + void SetUp() + { + request_header_ = std::make_shared(); + request_ = std::make_shared(); + response_ = std::make_shared(); + } + +protected: + rclcpp::AnyServiceCallback any_service_callback_; + std::shared_ptr request_header_; + std::shared_ptr request_; + std::shared_ptr response_; +}; + +TEST_F(TestAnyServiceCallback, no_set_and_dispatch_throw) { + EXPECT_THROW( + any_service_callback_.dispatch(request_header_, request_, response_), + std::runtime_error); +} + +TEST_F(TestAnyServiceCallback, set_and_dispatch_no_header) { + int callback_calls = 0; + auto callback = [&callback_calls](const std::shared_ptr, + std::shared_ptr) { + callback_calls++; + }; + + any_service_callback_.set(callback); + EXPECT_NO_THROW( + any_service_callback_.dispatch(request_header_, request_, response_)); + EXPECT_EQ(callback_calls, 1); +} + + +TEST_F(TestAnyServiceCallback, set_and_dispatch_header) { + int callback_with_header_calls = 0; + auto callback_with_header = + [&callback_with_header_calls](const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) { + callback_with_header_calls++; + }; + + any_service_callback_.set(callback_with_header); + EXPECT_NO_THROW( + any_service_callback_.dispatch(request_header_, request_, response_)); + EXPECT_EQ(callback_with_header_calls, 1); +} diff --git a/rclcpp/test/rclcpp/test_any_subscription_callback.cpp b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp new file mode 100644 index 0000000000..81316818fc --- /dev/null +++ b/rclcpp/test/rclcpp/test_any_subscription_callback.cpp @@ -0,0 +1,205 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/any_subscription_callback.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/empty.h" + +class TestAnySubscriptionCallback : public ::testing::Test +{ +public: + TestAnySubscriptionCallback() + : any_subscription_callback_(allocator_) {} + void SetUp() + { + msg_shared_ptr_ = std::make_shared(); + msg_const_shared_ptr_ = std::make_shared(); + msg_unique_ptr_ = std::make_unique(); + } + +protected: + std::shared_ptr> allocator_; + rclcpp::AnySubscriptionCallback> + any_subscription_callback_; + + std::shared_ptr msg_shared_ptr_; + std::shared_ptr msg_const_shared_ptr_; + std::unique_ptr msg_unique_ptr_; + rclcpp::MessageInfo message_info_; +}; + +TEST_F(TestAnySubscriptionCallback, construct_destruct) { +} + +TEST_F(TestAnySubscriptionCallback, unset_dispatch_throw) { + EXPECT_THROW( + any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), + std::runtime_error); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr) { + int callback_count = 0; + auto shared_ptr_callback = [&callback_count]( + const std::shared_ptr) { + callback_count++; + }; + + any_subscription_callback_.set(shared_ptr_callback); + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + // Can't convert ConstSharedPtr to SharedPtr + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 1); + + // Promotes Unique into SharedPtr + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_shared_ptr_w_info) { + int callback_count = 0; + auto shared_ptr_w_info_callback = [&callback_count]( + const std::shared_ptr, const rclcpp::MessageInfo &) { + callback_count++; + }; + + any_subscription_callback_.set(shared_ptr_w_info_callback); + + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + // Can't convert ConstSharedPtr to SharedPtr + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 1); + + // Promotes Unique into SharedPtr + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr) { + int callback_count = 0; + auto const_shared_ptr_callback = [&callback_count]( + std::shared_ptr) { + callback_count++; + }; + + any_subscription_callback_.set(const_shared_ptr_callback); + + // Ok to promote shared_ptr to ConstSharedPtr + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 2); + + // Not allowed to convert unique_ptr to const shared_ptr + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_const_shared_ptr_w_info) { + int callback_count = 0; + auto const_shared_ptr_callback = [&callback_count]( + std::shared_ptr, const rclcpp::MessageInfo &) { + callback_count++; + }; + + any_subscription_callback_.set( + std::move(const_shared_ptr_callback)); + + // Ok to promote shared_ptr to ConstSharedPtr + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 2); + + // Not allowed to convert unique_ptr to const shared_ptr + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr) { + int callback_count = 0; + auto unique_ptr_callback = [&callback_count]( + std::unique_ptr) { + callback_count++; + }; + + any_subscription_callback_.set(unique_ptr_callback); + + // Message is copied into unique_ptr + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 1); + + // Unique_ptr is_moved + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); + EXPECT_EQ(callback_count, 2); +} + +TEST_F(TestAnySubscriptionCallback, set_dispatch_unique_ptr_w_info) { + int callback_count = 0; + auto unique_ptr_callback = [&callback_count]( + std::unique_ptr, const rclcpp::MessageInfo &) { + callback_count++; + }; + + any_subscription_callback_.set(unique_ptr_callback); + + // Message is copied into unique_ptr + EXPECT_NO_THROW(any_subscription_callback_.dispatch(msg_shared_ptr_, message_info_)); + EXPECT_EQ(callback_count, 1); + + EXPECT_THROW( + any_subscription_callback_.dispatch_intra_process(msg_const_shared_ptr_, message_info_), + std::runtime_error); + EXPECT_EQ(callback_count, 1); + + // Unique_ptr is_moved + EXPECT_NO_THROW( + any_subscription_callback_.dispatch_intra_process(std::move(msg_unique_ptr_), message_info_)); + EXPECT_EQ(callback_count, 2); +} diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp new file mode 100644 index 0000000000..184904b8e7 --- /dev/null +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -0,0 +1,67 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/create_subscription.hpp" +#include "rclcpp/node.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/msg/empty.h" + +using namespace std::chrono_literals; + +class TestCreateSubscription : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + } + + void TearDown() override + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestCreateSubscription, create) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription = + rclcpp::create_subscription(node, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} + +TEST_F(TestCreateSubscription, create_with_statistics) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; + options.topic_stats_options.publish_topic = "topic_statistics"; + options.topic_stats_options.publish_period = 5min; + + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription = + rclcpp::create_subscription(node, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} From 0580a3a5432fcbe0d08ae3afc6ec84287dce6281 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 1 Jul 2020 12:48:41 -0700 Subject: [PATCH 06/61] Unit tests for node interfaces (#1202) * Unit tests for node interfaces Signed-off-by: Stephen Brawner * Address PR Feedback Signed-off-by: Stephen Brawner * Address PR feedback Signed-off-by: Stephen Brawner * Adjusting comment Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 46 +++++ .../rclcpp/node_interfaces/test_node_base.cpp | 60 ++++++ .../node_interfaces/test_node_clock.cpp | 49 +++++ .../node_interfaces/test_node_graph.cpp | 186 ++++++++++++++++++ .../node_interfaces/test_node_parameters.cpp | 89 +++++++++ .../node_interfaces/test_node_services.cpp | 111 +++++++++++ .../node_interfaces/test_node_timers.cpp | 71 +++++++ .../node_interfaces/test_node_topics.cpp | 129 ++++++++++++ .../node_interfaces/test_node_waitables.cpp | 68 +++++++ 9 files changed, 809 insertions(+) create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp create mode 100644 rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 46fe7c15d6..974b3c7c35 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -136,6 +136,52 @@ ament_add_gtest(test_node_interfaces__get_node_interfaces if(TARGET test_node_interfaces__get_node_interfaces) target_link_libraries(test_node_interfaces__get_node_interfaces ${PROJECT_NAME}) endif() +ament_add_gtest(test_node_interfaces__node_base + rclcpp/node_interfaces/test_node_base.cpp) +if(TARGET test_node_interfaces__node_base) + target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_clock + rclcpp/node_interfaces/test_node_clock.cpp) +if(TARGET test_node_interfaces__node_clock) + target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_graph + rclcpp/node_interfaces/test_node_graph.cpp) +if(TARGET test_node_interfaces__node_graph) + ament_target_dependencies( + test_node_interfaces__node_graph + "test_msgs") + target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_parameters + rclcpp/node_interfaces/test_node_parameters.cpp) +if(TARGET test_node_interfaces__node_parameters) + target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_services + rclcpp/node_interfaces/test_node_services.cpp) +if(TARGET test_node_interfaces__node_services) + target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_timers + rclcpp/node_interfaces/test_node_timers.cpp) +if(TARGET test_node_interfaces__node_timers) + target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_topics + rclcpp/node_interfaces/test_node_topics.cpp) +if(TARGET test_node_interfaces__node_topics) + ament_target_dependencies( + test_node_interfaces__node_topics + "test_msgs") + target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME}) +endif() +ament_add_gtest(test_node_interfaces__node_waitables + rclcpp/node_interfaces/test_node_waitables.cpp) +if(TARGET test_node_interfaces__node_waitables) + target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME}) +endif() # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output # rclcpp_add_build_failure_test(build_failure__get_node_topics_interface_const_ref_rclcpp_node diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp new file mode 100644 index 0000000000..cfe42d7819 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp @@ -0,0 +1,60 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestNodeBase : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeBase, construct_from_node) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_base = + dynamic_cast(node->get_node_base_interface().get()); + ASSERT_NE(nullptr, node_base); + + EXPECT_STREQ("node", node_base->get_name()); + EXPECT_STREQ("/ns", node_base->get_namespace()); + + EXPECT_STREQ("/ns/node", node_base->get_fully_qualified_name()); + EXPECT_NE(nullptr, node_base->get_context()); + EXPECT_NE(nullptr, node_base->get_rcl_node_handle()); + EXPECT_NE(nullptr, node_base->get_shared_rcl_node_handle()); + + const auto * const_node_base = node_base; + EXPECT_NE(nullptr, const_node_base->get_rcl_node_handle()); + EXPECT_NE(nullptr, const_node_base->get_shared_rcl_node_handle()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp new file mode 100644 index 0000000000..dec171b655 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_clock.cpp @@ -0,0 +1,49 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/node_interfaces/node_clock.hpp" +#include "rclcpp/node.hpp" + +class TestNodeClock : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeClock, construct_from_node) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_clock = + dynamic_cast(node->get_node_clock_interface().get()); + ASSERT_NE(nullptr, node_clock); + EXPECT_NE(nullptr, node_clock->get_clock()); + + const auto * const_node_clock = node_clock; + EXPECT_NE(nullptr, const_node_clock->get_clock()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp new file mode 100644 index 0000000000..db69207f3b --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -0,0 +1,186 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_base.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +class TestNodeGraph : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeGraph, construct_from_node) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + auto topic_names_and_types = node_graph->get_topic_names_and_types(false); + EXPECT_LT(0u, topic_names_and_types.size()); + + auto service_names_and_types = node_graph->get_service_names_and_types(); + EXPECT_LT(0u, service_names_and_types.size()); + + auto names = node_graph->get_node_names(); + EXPECT_EQ(1u, names.size()); + + auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); + EXPECT_EQ(1u, names_and_namespaces.size()); + + EXPECT_EQ(0u, node_graph->count_publishers("not_a_topic")); + EXPECT_EQ(0u, node_graph->count_subscribers("not_a_topic")); +} + +TEST_F(TestNodeGraph, get_topic_names_and_types) +{ + auto node = std::make_shared("node2", "ns"); + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + auto topic_names_and_types = node_graph->get_topic_names_and_types(); + EXPECT_LT(0u, topic_names_and_types.size()); +} + +TEST_F(TestNodeGraph, get_service_names_and_types) +{ + auto node = std::make_shared("node2", "ns"); + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + auto service_names_and_types = node_graph->get_service_names_and_types(); + EXPECT_LT(0u, service_names_and_types.size()); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_by_node) +{ + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + const auto * node_graph = + dynamic_cast(node1->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + EXPECT_THROW( + node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + std::runtime_error); + auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("node1", "/ns"); + auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("node2", "/ns"); + EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size()); +} + +TEST_F(TestNodeGraph, get_node_names_and_namespaces) +{ + auto node = std::make_shared("node", "ns"); + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); + EXPECT_EQ(1u, names_and_namespaces.size()); +} + +TEST_F(TestNodeGraph, notify_shutdown) +{ + auto node = std::make_shared("node", "ns"); + auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + EXPECT_NO_THROW(node_graph->notify_shutdown()); +} + +TEST_F(TestNodeGraph, wait_for_graph_change) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + EXPECT_NO_THROW(node_graph->notify_graph_change()); + EXPECT_THROW( + node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), + rclcpp::exceptions::InvalidEventError); + + auto event = std::make_shared(); + EXPECT_THROW( + node_graph->wait_for_graph_change(event, std::chrono::milliseconds(0)), + rclcpp::exceptions::EventNotRegisteredError); +} + +TEST_F(TestNodeGraph, get_info_by_topic) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + const rclcpp::QoS publisher_qos(1); + auto publisher = node->create_publisher("topic", publisher_qos); + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + + const rclcpp::QoS subscriber_qos(10); + auto subscription = + node->create_subscription( + "topic", subscriber_qos, std::move(callback)); + + const auto * node_graph = + dynamic_cast(node->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph); + + auto publishers = node_graph->get_publishers_info_by_topic("topic", false); + ASSERT_EQ(1u, publishers.size()); + + auto publisher_endpoint_info = publishers[0]; + const auto const_publisher_endpoint_info = publisher_endpoint_info; + EXPECT_STREQ("node", publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ("node", const_publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ("/ns", publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ("/ns", const_publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ("test_msgs/msg/Empty", publisher_endpoint_info.topic_type().c_str()); + EXPECT_STREQ("test_msgs/msg/Empty", const_publisher_endpoint_info.topic_type().c_str()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_endpoint_info.endpoint_type()); + EXPECT_EQ(rclcpp::EndpointType::Publisher, const_publisher_endpoint_info.endpoint_type()); + + rclcpp::QoS actual_qos = publisher_endpoint_info.qos_profile(); + EXPECT_EQ(0u, actual_qos.get_rmw_qos_profile().depth); + + rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile(); + EXPECT_EQ(0u, const_actual_qos.get_rmw_qos_profile().depth); + + auto endpoint_gid = publisher_endpoint_info.endpoint_gid(); + auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid(); + bool endpoint_gid_is_all_zeros = true; + for (size_t i = 0; i < RMW_GID_STORAGE_SIZE; ++i) { + endpoint_gid_is_all_zeros &= (endpoint_gid[i] == 0); + EXPECT_EQ(endpoint_gid[i], const_endpoint_gid[i]); + } + EXPECT_FALSE(endpoint_gid_is_all_zeros); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp new file mode 100644 index 0000000000..68a7546c83 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -0,0 +1,89 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * NodeParameters is a complicated interface with lots of code, but it is tested elsewhere + * very thoroughly. This currently just includes unittests for the currently uncovered + * functionality. + */ + +#include + +#include +#include +#include +#include + +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_parameters.hpp" + +class TestNodeParameters : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeParameters, list_parameters) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + std::vector prefixes; + const auto list_result = node_parameters->list_parameters(prefixes, 1u); + + // Currently the only default parameter is 'use_sim_time', but that may change. + size_t number_of_parameters = list_result.names.size(); + EXPECT_GE(1u, number_of_parameters); + + const std::string parameter_name = "new_parameter"; + const rclcpp::ParameterValue value(true); + const rcl_interfaces::msg::ParameterDescriptor descriptor; + const auto added_parameter_value = + node_parameters->declare_parameter(parameter_name, value, descriptor, false); + EXPECT_EQ(value.get(), added_parameter_value.get()); + + auto list_result2 = node_parameters->list_parameters(prefixes, 1u); + EXPECT_EQ(number_of_parameters + 1u, list_result2.names.size()); + + EXPECT_NE( + std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name), + list_result2.names.end()); +} + +TEST_F(TestNodeParameters, parameter_overrides) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + auto * node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); + + const auto & parameter_overrides = node_parameters->get_parameter_overrides(); + EXPECT_EQ(0u, parameter_overrides.size()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp new file mode 100644 index 0000000000..893340d33a --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -0,0 +1,111 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_services.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestService : public rclcpp::ServiceBase +{ +public: + explicit TestService(rclcpp::Node * node) + : rclcpp::ServiceBase(node->get_node_base_interface()->get_shared_rcl_node_handle()) {} + + std::shared_ptr create_request() override {return nullptr;} + std::shared_ptr create_request_header() override {return nullptr;} + void handle_request(std::shared_ptr, std::shared_ptr) override {} +}; + +class TestClient : public rclcpp::ClientBase +{ +public: + explicit TestClient(rclcpp::Node * node) + : rclcpp::ClientBase(node->get_node_base_interface().get(), node->get_node_graph_interface()) {} + + std::shared_ptr create_response() override {return nullptr;} + std::shared_ptr create_request_header() override {return nullptr;} + void handle_response( + std::shared_ptr, std::shared_ptr) override {} +}; + +class TestNodeService : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeService, add_service) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_services = + dynamic_cast( + node->get_node_services_interface().get()); + ASSERT_NE(nullptr, node_services); + + auto service = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW( + node_services->add_service(service, callback_group)); + + // Check that adding a service from node to a callback group of different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_services->add_service(service, callback_group_in_different_node), + std::runtime_error); +} + +TEST_F(TestNodeService, add_client) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_services = + dynamic_cast( + node->get_node_services_interface().get()); + ASSERT_NE(nullptr, node_services); + + auto client = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_services->add_client(client, callback_group)); + + // Check that adding a client from node to a callback group of different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_services->add_client(client, callback_group_in_different_node), + std::runtime_error); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp new file mode 100644 index 0000000000..af340d292e --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -0,0 +1,71 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_timers.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestTimer : public rclcpp::TimerBase +{ +public: + explicit TestTimer(rclcpp::Node * node) + : TimerBase(node->get_clock(), std::chrono::nanoseconds(1), + node->get_node_base_interface()->get_context()) {} + + void execute_callback() override {} + bool is_steady() override {return false;} +}; + +class TestNodeTimers : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeTimers, add_timer) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto node_timers = + dynamic_cast(node->get_node_timers_interface().get()); + ASSERT_NE(nullptr, node_timers); + auto timer = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_timers->add_timer(timer, callback_group)); + + // Check that adding timer from node to callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_timers->add_timer(timer, callback_group_in_different_node), + std::runtime_error); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp new file mode 100644 index 0000000000..86b4e72e36 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -0,0 +1,129 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_topics.hpp" +#include "rclcpp/rclcpp.hpp" +#include "test_msgs/msg/empty.hpp" + +namespace +{ + +const rosidl_message_type_support_t EmptyTypeSupport() +{ + return *rosidl_typesupport_cpp::get_message_type_support_handle(); +} + +const rcl_publisher_options_t PublisherOptions() +{ + return rclcpp::PublisherOptionsWithAllocator>().template + to_rcl_publisher_options(rclcpp::QoS(10)); +} + +const rcl_subscription_options_t SubscriptionOptions() +{ + return rclcpp::SubscriptionOptionsWithAllocator>().template + to_rcl_subscription_options(rclcpp::QoS(10)); +} + +} // namespace + +class TestPublisher : public rclcpp::PublisherBase +{ +public: + explicit TestPublisher(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} +}; + +class TestSubscription : public rclcpp::SubscriptionBase +{ +public: + explicit TestSubscription(rclcpp::Node * node) + : rclcpp::SubscriptionBase( + node->get_node_base_interface().get(), EmptyTypeSupport(), "topic", SubscriptionOptions()) {} + std::shared_ptr create_message() override {return nullptr;} + + std::shared_ptr + create_serialized_message() override {return nullptr;} + + void handle_message(std::shared_ptr &, const rclcpp::MessageInfo &) override {} + void handle_loaned_message(void *, const rclcpp::MessageInfo &) override {} + void return_message(std::shared_ptr &) override {} + void return_serialized_message(std::shared_ptr &) override {} +}; + +class TestNodeTopics : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeTopics, add_publisher) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + auto * node_topics = + dynamic_cast(node->get_node_topics_interface().get()); + ASSERT_NE(nullptr, node_topics); + auto publisher = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_topics->add_publisher(publisher, callback_group)); + + // Check that adding publisher from node to a callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_topics->add_publisher(publisher, callback_group_in_different_node), + std::runtime_error); +} + +TEST_F(TestNodeTopics, add_subscription) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + auto * node_topics = + dynamic_cast(node->get_node_topics_interface().get()); + ASSERT_NE(nullptr, node_topics); + auto subscription = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_NO_THROW(node_topics->add_subscription(subscription, callback_group)); + + // Check that adding subscription from node to callback group in different_node throws exception. + std::shared_ptr different_node = std::make_shared("node2", "ns"); + + auto callback_group_in_different_node = + different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_THROW( + node_topics->add_subscription(subscription, callback_group_in_different_node), + std::runtime_error); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp new file mode 100644 index 0000000000..19cc63fe07 --- /dev/null +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -0,0 +1,68 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/node_options.h" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/node_waitables.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestWaitable : public rclcpp::Waitable +{ +public: + bool add_to_wait_set(rcl_wait_set_t *) override {return false;} + bool is_ready(rcl_wait_set_t *) override {return false;} + void execute() override {} +}; + +class TestNodeWaitables : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestNodeWaitables, add_remove_waitable) +{ + std::shared_ptr node = std::make_shared("node", "ns"); + + auto * node_waitables = + dynamic_cast( + node->get_node_waitables_interface().get()); + ASSERT_NE(nullptr, node_waitables); + + std::shared_ptr node2 = std::make_shared("node2", "ns"); + + auto callback_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto callback_group2 = node2->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto waitable = std::make_shared(); + EXPECT_NO_THROW( + node_waitables->add_waitable(waitable, callback_group1)); + EXPECT_THROW( + node_waitables->add_waitable(waitable, callback_group2), + std::runtime_error); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1)); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2)); +} From 81641b6c05fa9676a2c790f1ac9a5cea639f6aa7 Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 14 Jul 2020 10:50:37 -0700 Subject: [PATCH 07/61] Unit tests for allocator_memory_strategy.hpp (#1197) * Unit tests for allocator_memory_strategy.hpp Part 1 of 2 for this file, but part 2 of 3 for memory strategies overall Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * Remove find_package Signed-off-by: Stephen Brawner * Remove ref to osrf_testing_tools Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 8 + .../test_allocator_memory_strategy.cpp | 532 ++++++++++++++++++ 2 files changed, 540 insertions(+) create mode 100644 rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 974b3c7c35..cfd878cf32 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -17,6 +17,14 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs SKIP_INSTALL ) +ament_add_gtest(test_allocator_memory_strategy rclcpp/strategies/test_allocator_memory_strategy.cpp) +if(TARGET test_allocator_memory_strategy) + ament_target_dependencies(test_allocator_memory_strategy + "rcl" + "test_msgs" + ) + target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME}) +endif() ament_add_gtest(test_any_service_callback rclcpp/test_any_service_callback.cpp) if(TARGET test_any_service_callback) ament_target_dependencies(test_any_service_callback diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp new file mode 100644 index 0000000000..d208377dcb --- /dev/null +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -0,0 +1,532 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/scope_exit.hpp" +#include "rclcpp/strategies/allocator_memory_strategy.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +using rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy; +using WeakNodeList = std::list; + +static bool test_waitable_result = false; + +/** + * Mock Waitable class, with a globally setable boolean result. + */ +class TestWaitable : public rclcpp::Waitable +{ +public: + bool add_to_wait_set(rcl_wait_set_t *) override + { + return test_waitable_result; + } + + bool is_ready(rcl_wait_set_t *) override + { + return test_waitable_result; + } + + void execute() override {} +}; + +struct RclWaitSetSizes +{ + size_t size_of_subscriptions = 0; + size_t size_of_guard_conditions = 0; + size_t size_of_timers = 0; + size_t size_of_clients = 0; + size_t size_of_services = 0; + size_t size_of_events = 0; + size_t size_of_waitables = 0; +}; + +// For a standard rclcpp node, this should be more than enough capacity for each type. +RclWaitSetSizes SufficientWaitSetCapacities() +{ + return {100, 100, 100, 100, 100, 100, 100}; +} + +class TestAllocatorMemoryStrategy : public ::testing::Test +{ +public: + TestAllocatorMemoryStrategy() + : allocator_memory_strategy_(nullptr), + num_ready_subscriptions_of_basic_node_(0), + num_ready_services_of_basic_node_(0), + num_ready_events_of_basic_node_(0), + num_ready_clients_of_basic_node_(0), + num_guard_conditions_of_basic_node_(0), + num_ready_timers_of_basic_node_(0), + num_waitables_of_basic_node_(0) {} + + void SetUp() override + { + rclcpp::init(0, nullptr); + allocator_ = std::make_shared>(); + // Even though this is just using a basic allocator, the custom allocator constructor was + // not covered in general testing. + allocator_memory_strategy_ = std::make_shared>(allocator_); + + auto basic_node = std::make_shared("basic_node", "ns"); + auto dummy_memory_strategy = std::make_shared>(); + WeakNodeList nodes; + nodes.push_back(basic_node->get_node_base_interface()); + dummy_memory_strategy->collect_entities(nodes); + + num_ready_subscriptions_of_basic_node_ = + dummy_memory_strategy->number_of_ready_subscriptions(); + num_ready_services_of_basic_node_ = dummy_memory_strategy->number_of_ready_services(); + num_ready_events_of_basic_node_ = dummy_memory_strategy->number_of_ready_events(); + num_ready_clients_of_basic_node_ = dummy_memory_strategy->number_of_ready_clients(); + num_guard_conditions_of_basic_node_ = dummy_memory_strategy->number_of_guard_conditions(); + num_ready_timers_of_basic_node_ = dummy_memory_strategy->number_of_ready_timers(); + num_waitables_of_basic_node_ = dummy_memory_strategy->number_of_waitables(); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr> allocator_memory_strategy() + { + return allocator_memory_strategy_; + } + + // + // Convience methods, but it adds entities to vectors so the weak pointers kept by the node + // interfaces remain alive and valid + // + + std::shared_ptr create_node_with_subscription( + const std::string & name, bool with_exclusive_callback_group) + { + auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + const rclcpp::QoS qos(10); + auto node_with_subscription = std::make_shared(name, "ns"); + + rclcpp::SubscriptionOptions subscription_options; + + if (with_exclusive_callback_group) { + auto callback_group = + node_with_subscription->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + subscription_options.callback_group = callback_group; + callback_groups_.push_back(callback_group); + } + + auto subscription = node_with_subscription->create_subscription< + test_msgs::msg::Empty, decltype(subscription_callback)>( + "topic", qos, std::move(subscription_callback), subscription_options); + subscriptions_.push_back(subscription); + + return node_with_subscription; + } + + std::shared_ptr create_node_with_service( + const std::string & name, bool with_exclusive_callback_group = false) + { + auto service_callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto node_with_service = std::make_shared(name, "ns"); + + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + + if (with_exclusive_callback_group) { + callback_group = + node_with_service->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + callback_groups_.push_back(callback_group); + } + + services_.push_back( + node_with_service->create_service( + "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group)); + return node_with_service; + } + + std::shared_ptr create_node_with_client( + const std::string & name, bool with_exclusive_callback_group = false) + { + auto node_with_client = std::make_shared(name, "ns"); + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + if (with_exclusive_callback_group) { + callback_group = + node_with_client->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + callback_groups_.push_back(callback_group); + } + + clients_.push_back( + node_with_client->create_client( + "service", rmw_qos_profile_services_default, callback_group)); + return node_with_client; + } + + std::shared_ptr create_node_with_timer( + const std::string & name, bool with_exclusive_callback_group = false) + { + auto timer_callback = []() {}; + auto node_with_timer = std::make_shared(name, "ns"); + + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + if (with_exclusive_callback_group) { + callback_group = + node_with_timer->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + callback_groups_.push_back(callback_group); + } + + timers_.push_back( + node_with_timer->create_wall_timer( + std::chrono::milliseconds(1), timer_callback, callback_group)); + return node_with_timer; + } + + size_t number_of_ready_subscriptions_in_addition_to_basic_node() const + { + return + allocator_memory_strategy_->number_of_ready_subscriptions() - + num_ready_subscriptions_of_basic_node_; + } + + size_t number_of_ready_services_in_addition_to_basic_node() const + { + return + allocator_memory_strategy_->number_of_ready_services() - + num_ready_services_of_basic_node_; + } + + size_t number_of_ready_events_in_addition_to_basic_node() const + { + return + allocator_memory_strategy_->number_of_ready_events() - + num_ready_events_of_basic_node_; + } + + size_t number_of_ready_clients_in_addition_to_basic_node() const + { + return + allocator_memory_strategy_->number_of_ready_clients() - + num_ready_clients_of_basic_node_; + } + + size_t number_of_guard_conditions_in_addition_to_basic_node() const + { + return + allocator_memory_strategy_->number_of_guard_conditions() - + num_guard_conditions_of_basic_node_; + } + + size_t number_of_ready_timers_in_addition_to_basic_node() const + { + return + allocator_memory_strategy_->number_of_ready_timers() - + num_ready_timers_of_basic_node_; + } + + size_t number_of_waitables_in_addition_to_basic_node() const + { + return + allocator_memory_strategy_->number_of_waitables() - + num_waitables_of_basic_node_; + } + + ::testing::AssertionResult TestNumberOfEntitiesAfterCollection( + std::shared_ptr node, + const RclWaitSetSizes & expected) + { + WeakNodeList nodes; + nodes.push_back(node->get_node_base_interface()); + + allocator_memory_strategy()->collect_entities(nodes); + EXPECT_EQ( + expected.size_of_subscriptions, number_of_ready_subscriptions_in_addition_to_basic_node()); + EXPECT_EQ( + expected.size_of_guard_conditions, number_of_guard_conditions_in_addition_to_basic_node()); + EXPECT_EQ(expected.size_of_timers, number_of_ready_timers_in_addition_to_basic_node()); + EXPECT_EQ(expected.size_of_clients, number_of_ready_clients_in_addition_to_basic_node()); + EXPECT_EQ(expected.size_of_services, number_of_ready_services_in_addition_to_basic_node()); + EXPECT_EQ(expected.size_of_events, number_of_ready_events_in_addition_to_basic_node()); + EXPECT_EQ(expected.size_of_waitables, number_of_waitables_in_addition_to_basic_node()); + if (::testing::Test::HasFailure()) { + return ::testing::AssertionFailure() << + "Expected number of entities did not match actual counts"; + } + return ::testing::AssertionSuccess(); + } + + ::testing::AssertionResult TestAddHandlesToWaitSet( + std::shared_ptr node, + const RclWaitSetSizes & insufficient_capacities) + { + WeakNodeList nodes; + nodes.push_back(node->get_node_base_interface()); + allocator_memory_strategy()->collect_entities(nodes); + + auto context = node->get_node_base_interface()->get_context(); + rcl_context_t * rcl_context = context->get_rcl_context().get(); + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + RclWaitSetSizes sufficient_capacities = SufficientWaitSetCapacities(); + + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, + sufficient_capacities.size_of_subscriptions, + sufficient_capacities.size_of_guard_conditions, + sufficient_capacities.size_of_timers, + sufficient_capacities.size_of_clients, + sufficient_capacities.size_of_services, + sufficient_capacities.size_of_events, + rcl_context, + allocator); + if (ret != RCL_RET_OK) { + return ::testing::AssertionFailure() << + "Calling rcl_wait_set_init() with expected sufficient capacities failed"; + } + + RCLCPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); + }); + + if (!allocator_memory_strategy()->add_handles_to_wait_set(&wait_set)) { + return ::testing::AssertionFailure() << + "Calling add_handles_to_wait_set() with a wait_set with expected sufficient capacities" + " failed"; + } + + rcl_wait_set_t wait_set_no_capacity = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + &wait_set_no_capacity, + insufficient_capacities.size_of_subscriptions, + insufficient_capacities.size_of_guard_conditions, + insufficient_capacities.size_of_timers, + insufficient_capacities.size_of_clients, + insufficient_capacities.size_of_services, + insufficient_capacities.size_of_events, + rcl_context, + allocator); + + if (ret != RCL_RET_OK) { + return ::testing::AssertionFailure() << + "Calling rcl_wait_set_init() with expected insufficient capacities failed"; + } + + RCLCPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set_no_capacity)); + }); + + if (allocator_memory_strategy()->add_handles_to_wait_set(&wait_set_no_capacity)) { + return ::testing::AssertionFailure() << + "Calling add_handles_to_wait_set() with a wait_set with insufficient capacities" + " unexpectedly succeeded"; + } + return ::testing::AssertionSuccess(); + } + + std::vector> callback_groups_; + +private: + std::shared_ptr> allocator_; + std::shared_ptr> allocator_memory_strategy_; + + size_t num_ready_subscriptions_of_basic_node_; + size_t num_ready_services_of_basic_node_; + size_t num_ready_events_of_basic_node_; + size_t num_ready_clients_of_basic_node_; + size_t num_guard_conditions_of_basic_node_; + size_t num_ready_timers_of_basic_node_; + size_t num_waitables_of_basic_node_; + + // These are generally kept as weak pointers in the rclcpp::Node interfaces, so they need to be + // owned by this class. + std::vector subscriptions_; + std::vector services_; + std::vector clients_; + std::vector timers_; + std::vector waitables_; +}; + +TEST_F(TestAllocatorMemoryStrategy, construct_destruct) { + WeakNodeList nodes; + auto basic_node = std::make_shared("node", "ns"); + nodes.push_back(basic_node->get_node_base_interface()); + allocator_memory_strategy()->collect_entities(nodes); + EXPECT_EQ(0u, number_of_ready_subscriptions_in_addition_to_basic_node()); + EXPECT_EQ(0u, number_of_ready_services_in_addition_to_basic_node()); + EXPECT_EQ(0u, number_of_ready_events_in_addition_to_basic_node()); + EXPECT_EQ(0u, number_of_ready_clients_in_addition_to_basic_node()); + EXPECT_EQ(0u, number_of_guard_conditions_in_addition_to_basic_node()); + EXPECT_EQ(0u, number_of_ready_timers_in_addition_to_basic_node()); + EXPECT_EQ(0u, number_of_waitables_in_addition_to_basic_node()); +} + +TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { + rcl_guard_condition_t guard_condition1 = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_t guard_condition2 = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_t guard_condition3 = rcl_get_zero_initialized_guard_condition(); + + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3)); + EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); + + // Adding a second time should not add to vector + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->add_guard_condition(&guard_condition3)); + EXPECT_EQ(3u, allocator_memory_strategy()->number_of_guard_conditions()); + + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3)); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); + + // Removing second time should have no effect + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition1)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition2)); + EXPECT_NO_THROW(allocator_memory_strategy()->remove_guard_condition(&guard_condition3)); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); +} + +TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) { + EXPECT_THROW(allocator_memory_strategy()->add_waitable_handle(nullptr), std::runtime_error); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); + + rclcpp::Waitable::SharedPtr waitable = std::make_shared(); + EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable)); + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); + + EXPECT_NO_THROW(allocator_memory_strategy()->clear_handles()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); +} + +TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { + RclWaitSetSizes expected_sizes = {}; + expected_sizes.size_of_subscriptions = 1; + auto node_with_subscription = create_node_with_subscription("subscription_node", false); + EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes)); +} + +TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_service) { + RclWaitSetSizes expected_sizes = {}; + expected_sizes.size_of_services = 1; + auto node_with_service = create_node_with_service("service_node"); + EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_service, expected_sizes)); +} + +TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_client) { + RclWaitSetSizes expected_sizes = {}; + expected_sizes.size_of_clients = 1; + auto node_with_client = create_node_with_client("client_node"); + EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_client, expected_sizes)); +} + +TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) { + RclWaitSetSizes expected_sizes = {}; + expected_sizes.size_of_timers = 1; + auto node_with_timer = create_node_with_timer("timer_node"); + EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_timer, expected_sizes)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) { + auto node = create_node_with_subscription("subscription_node", false); + WeakNodeList nodes; + nodes.push_back(node->get_node_base_interface()); + allocator_memory_strategy()->collect_entities(nodes); + EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_subscription) { + auto node_with_subscription = create_node_with_subscription("subscription_node", false); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_subscriptions = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_subscription, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_service) { + auto node_with_service = create_node_with_service("service_node"); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_services = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_service, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) { + auto node_with_client = create_node_with_client("client_node"); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_clients = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_client, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { + auto node = std::make_shared("node", "ns"); + rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); + auto context = node->get_node_base_interface()->get_context(); + rcl_context_t * rcl_context = context->get_rcl_context().get(); + rcl_guard_condition_options_t guard_condition_options = { + rcl_get_default_allocator()}; + + EXPECT_EQ( + RCL_RET_OK, + rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options)); + RCLCPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition)); + }); + + allocator_memory_strategy()->add_guard_condition(&guard_condition); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_guard_conditions = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) { + auto node_with_timer = create_node_with_timer("timer_node"); + RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); + insufficient_capacities.size_of_timers = 0; + EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_timer, insufficient_capacities)); +} + +TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { + rclcpp::Waitable::SharedPtr waitable = std::make_shared(); + EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable)); + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); + + test_waitable_result = true; + EXPECT_TRUE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); + + test_waitable_result = false; + EXPECT_FALSE(allocator_memory_strategy()->add_handles_to_wait_set(nullptr)); + + // This calls TestWaitable's functions, so rcl errors are not set + EXPECT_FALSE(rcl_error_is_set()); +} From b1c41660b2b8582669895594832ef96361779cc1 Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 14 Jul 2020 19:00:19 -0700 Subject: [PATCH 08/61] Unit tests for allocator_memory_strategy.cpp part 2 (#1198) Signed-off-by: Stephen Brawner --- .../test_allocator_memory_strategy.cpp | 301 ++++++++++++++++++ 1 file changed, 301 insertions(+) diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index d208377dcb..5055d63eff 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -351,6 +351,70 @@ class TestAllocatorMemoryStrategy : public ::testing::Test return ::testing::AssertionSuccess(); } + ::testing::AssertionResult TestGetNextEntity( + std::shared_ptr node_with_entity1, + std::shared_ptr node_with_entity2, + std::function get_next_entity_func) + { + WeakNodeList nodes; + auto basic_node = std::make_shared("basic_node", "ns"); + nodes.push_back(node_with_entity1->get_node_base_interface()); + nodes.push_back(basic_node->get_node_base_interface()); + allocator_memory_strategy()->collect_entities(nodes); + + rclcpp::AnyExecutable result = get_next_entity_func(nodes); + if (result.node_base != node_with_entity1->get_node_base_interface()) { + return ::testing::AssertionFailure() << + "Failed to get expected entity with specified get_next_*() function"; + } + + WeakNodeList uncollected_nodes; + auto basic_node2 = std::make_shared("basic_node2", "ns"); + uncollected_nodes.push_back(node_with_entity2->get_node_base_interface()); + uncollected_nodes.push_back(basic_node2->get_node_base_interface()); + + rclcpp::AnyExecutable failed_result = get_next_entity_func(uncollected_nodes); + if (nullptr != failed_result.node_base) { + return ::testing::AssertionFailure() << + "A node was retrieved with the specified get_next_*() function despite" + " none of the nodes that were passed to it were added to the" + " allocator_memory_strategy"; + } + return ::testing::AssertionSuccess(); + } + + ::testing::AssertionResult TestGetNextEntityMutuallyExclusive( + std::shared_ptr node_with_entity, + std::function get_next_entity_func) + { + WeakNodeList nodes; + auto basic_node = std::make_shared("basic_node", "ns"); + + auto node_base = node_with_entity->get_node_base_interface(); + auto basic_node_base = basic_node->get_node_base_interface(); + nodes.push_back(node_base); + nodes.push_back(basic_node_base); + allocator_memory_strategy()->collect_entities(nodes); + + // It's important that these be set after collect_entities() otherwise collect_entities() will + // not do anything + node_base->get_default_callback_group()->can_be_taken_from() = false; + basic_node_base->get_default_callback_group()->can_be_taken_from() = false; + for (auto & callback_group : callback_groups_) { + callback_group->can_be_taken_from() = false; + } + + rclcpp::AnyExecutable result = get_next_entity_func(nodes); + + if (nullptr != result.node_base) { + return ::testing::AssertionFailure() << + "A node was retrieved with the specified get_next_*() function despite" + " setting can_be_taken_from() to false for all nodes and callback_groups"; + } + + return ::testing::AssertionSuccess(); + } + std::vector> callback_groups_; private: @@ -530,3 +594,240 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { // This calls TestWaitable's functions, so rcl errors are not set EXPECT_FALSE(rcl_error_is_set()); } + +TEST_F(TestAllocatorMemoryStrategy, get_next_subscription) { + auto node1 = create_node_with_subscription("node1", false); + auto node2 = create_node_with_subscription("node2", false); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_subscription(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_service) { + auto node1 = create_node_with_service("node1", false); + auto node2 = create_node_with_service("node2", false); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_service(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_client) { + auto node1 = create_node_with_client("node1", false); + auto node2 = create_node_with_client("node2", false); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_client(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_timer) { + auto node1 = create_node_with_timer("node1", false); + auto node2 = create_node_with_timer("node2", false); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_timer(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) { + auto node1 = std::make_shared("waitable_node", "ns"); + auto node2 = std::make_shared("waitable_node2", "ns"); + rclcpp::Waitable::SharedPtr waitable = std::make_shared(); + node1->get_node_waitables_interface()->add_waitable(waitable, nullptr); + node2->get_node_waitables_interface()->add_waitable(waitable, nullptr); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_waitable(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntity(node1, node2, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) { + auto node = create_node_with_subscription("node", true); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_subscription(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) { + auto node = create_node_with_service("node", true); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_service(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) { + auto node = create_node_with_client("node", true); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_client(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_timer_mutually_exclusive) { + auto node = create_node_with_timer("node", true); + + auto get_next_entity = [this](const WeakNodeList & nodes) { + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_timer(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_mutually_exclusive) { + auto node = std::make_shared("waitable_node", "ns"); + rclcpp::Waitable::SharedPtr waitable = std::make_shared(); + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + node->get_node_waitables_interface()->add_waitable(waitable, callback_group); + + auto get_next_entity = [this, callback_group](const WeakNodeList & nodes) { + // This callback group isn't in the base class' callback_group list, so this needs to be done + // before get_next_waitable() is called. + callback_group->can_be_taken_from() = false; + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_waitable(result, nodes); + return result; + }; + + EXPECT_TRUE(TestGetNextEntityMutuallyExclusive(node, get_next_entity)); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) { + WeakNodeList nodes; + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + // Force subscription to go out of scope and cleanup after collecting entities. + { + auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + const rclcpp::QoS qos(10); + + auto subscription = node->create_subscription< + test_msgs::msg::Empty, decltype(subscription_callback)>( + "topic", qos, std::move(subscription_callback)); + + allocator_memory_strategy()->collect_entities(nodes); + } + EXPECT_EQ(1u, number_of_ready_subscriptions_in_addition_to_basic_node()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_subscription(result, nodes); + EXPECT_EQ(node->get_node_base_interface(), result.node_base); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { + WeakNodeList nodes; + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + // Force service to go out of scope and cleanup after collecting entities. + { + auto service_callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = node->create_service( + "service", std::move(service_callback), rmw_qos_profile_services_default, nullptr); + + allocator_memory_strategy()->collect_entities(nodes); + } + EXPECT_EQ(1u, number_of_ready_services_in_addition_to_basic_node()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_service(result, nodes); + EXPECT_EQ(node->get_node_base_interface(), result.node_base); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { + WeakNodeList nodes; + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + // Force client to go out of scope and cleanup after collecting entities. + { + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto client = node->create_client( + "service", rmw_qos_profile_services_default, callback_group); + + allocator_memory_strategy()->collect_entities(nodes); + } + EXPECT_EQ(1u, number_of_ready_clients_in_addition_to_basic_node()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_client(result, nodes); + EXPECT_EQ(nullptr, result.node_base); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) { + WeakNodeList nodes; + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + + // Force timer to go out of scope and cleanup after collecting entities. + { + auto timer = node->create_wall_timer( + std::chrono::seconds(10), []() {}); + + allocator_memory_strategy()->collect_entities(nodes); + } + EXPECT_EQ(1u, number_of_ready_timers_in_addition_to_basic_node()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_timer(result, nodes); + EXPECT_EQ(nullptr, result.node_base); +} + +TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) { + WeakNodeList nodes; + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + + // Force waitable to go out of scope and cleanup after collecting entities. + { + allocator_memory_strategy()->collect_entities(nodes); + auto waitable = std::make_shared(); + allocator_memory_strategy()->add_waitable_handle(waitable); + } + EXPECT_EQ(1u, number_of_waitables_in_addition_to_basic_node()); + + rclcpp::AnyExecutable result; + allocator_memory_strategy()->get_next_waitable(result, nodes); + EXPECT_EQ(nullptr, result.node_base); +} From 9cf088b07111665687b03ff4d80073abe28c2703 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 15 Jul 2020 12:18:43 -0700 Subject: [PATCH 09/61] Add unit test for static_executor_entities_collector (#1221) * Add unit test for static_executor_entities_collector Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 9 + ...est_static_executor_entities_collector.cpp | 291 ++++++++++++++++++ 2 files changed, 300 insertions(+) create mode 100644 rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index cfd878cf32..0f5d5b9a3c 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -466,6 +466,15 @@ if(TARGET test_multi_threaded_executor) target_link_libraries(test_multi_threaded_executor ${PROJECT_NAME}) endif() +ament_add_gtest(test_static_executor_entities_collector rclcpp/executors/test_static_executor_entities_collector.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_static_executor_entities_collector) + ament_target_dependencies(test_static_executor_entities_collector + "rcl" + "test_msgs") + target_link_libraries(test_static_executor_entities_collector ${PROJECT_NAME}) +endif() + ament_add_gtest(test_guard_condition rclcpp/test_guard_condition.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_guard_condition) diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp new file mode 100644 index 0000000000..673df6ce7c --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -0,0 +1,291 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/rclcpp.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +namespace +{ + +struct NumberOfEntities +{ + size_t subscriptions = 0; + size_t timers = 0; + size_t services = 0; + size_t clients = 0; + size_t waitables = 0; +}; + +std::unique_ptr get_number_of_default_entities(rclcpp::Node::SharedPtr node) +{ + auto number_of_entities = std::make_unique(); + for (auto & weak_group : node->get_callback_groups()) { + auto group = weak_group.lock(); + EXPECT_NE(nullptr, group); + if (!group || !group->can_be_taken_from().load()) { + return nullptr; + } + group->find_subscription_ptrs_if( + [&number_of_entities](rclcpp::SubscriptionBase::SharedPtr &) + { + number_of_entities->subscriptions++; return false; + }); + group->find_timer_ptrs_if( + [&number_of_entities](rclcpp::TimerBase::SharedPtr &) + { + number_of_entities->timers++; return false; + }); + group->find_service_ptrs_if( + [&number_of_entities](rclcpp::ServiceBase::SharedPtr &) + { + number_of_entities->services++; return false; + }); + group->find_client_ptrs_if( + [&number_of_entities](rclcpp::ClientBase::SharedPtr &) + { + number_of_entities->clients++; return false; + }); + group->find_waitable_ptrs_if( + [&number_of_entities](rclcpp::Waitable::SharedPtr &) + { + number_of_entities->waitables++; return false; + }); + } + + return number_of_entities; +} + +} // namespace + +class TestStaticExecutorEntitiesCollector : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + entities_collector_ = + std::make_shared(); + } + + void TearDown() + { + rclcpp::shutdown(); + } + + rclcpp::executors::StaticExecutorEntitiesCollector::SharedPtr entities_collector_; +}; + +TEST_F(TestStaticExecutorEntitiesCollector, construct_destruct) { + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + EXPECT_EQ(0u, entities_collector_->get_number_of_waitables()); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node) { + auto node1 = std::make_shared("node1", "ns"); + EXPECT_NO_THROW(entities_collector_->add_node(node1->get_node_base_interface())); + + // Check adding second time + EXPECT_THROW(entities_collector_->add_node(node1->get_node_base_interface()), std::runtime_error); + + auto node2 = std::make_shared("node2", "ns"); + EXPECT_FALSE(entities_collector_->remove_node(node2->get_node_base_interface())); + EXPECT_NO_THROW(entities_collector_->add_node(node2->get_node_base_interface())); + + EXPECT_TRUE(entities_collector_->remove_node(node1->get_node_base_interface())); + EXPECT_FALSE(entities_collector_->remove_node(node1->get_node_base_interface())); + EXPECT_TRUE(entities_collector_->remove_node(node2->get_node_base_interface())); +} + +TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) { + auto node = std::make_shared("node", "ns"); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + rclcpp::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + // Check memory strategy is nullptr + rclcpp::memory_strategy::MemoryStrategy::SharedPtr memory_strategy = nullptr; + EXPECT_THROW( + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition), + std::runtime_error); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { + auto node = std::make_shared("node", "ns"); + const auto expected_number_of_entities = get_number_of_default_entities(node); + EXPECT_NE(nullptr, expected_number_of_entities); + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + EXPECT_EQ( + expected_number_of_entities->subscriptions, + entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(expected_number_of_entities->timers, entities_collector_->get_number_of_timers()); + EXPECT_EQ(expected_number_of_entities->services, entities_collector_->get_number_of_services()); + EXPECT_EQ(expected_number_of_entities->clients, entities_collector_->get_number_of_clients()); + // One extra for the executor + EXPECT_EQ( + 1u + expected_number_of_entities->waitables, + entities_collector_->get_number_of_waitables()); + + EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +} + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { + rclcpp::Context::SharedPtr shared_context = nullptr; + { + auto node1 = std::make_shared("node1", "ns"); + auto node2 = std::make_shared("node2", "ns"); + auto node3 = std::make_shared("node3", "ns"); + entities_collector_->add_node(node1->get_node_base_interface()); + entities_collector_->add_node(node2->get_node_base_interface()); + entities_collector_->add_node(node3->get_node_base_interface()); + shared_context = node1->get_node_base_interface()->get_context(); + } + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + rclcpp::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + // Expect weak_node pointers to be cleaned up and used + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +} + +class TestWaitable : public rclcpp::Waitable +{ +public: + bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + + bool is_ready(rcl_wait_set_t *) override {return true;} + + void execute() override {} +}; + +TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { + auto node = std::make_shared("node", "ns"); + const auto expected_number_of_entities = get_number_of_default_entities(node); + EXPECT_NE(nullptr, expected_number_of_entities); + + // Create 1 of each entity type + auto subscription = + node->create_subscription( + "topic", rclcpp::QoS(10), [](test_msgs::msg::Empty::SharedPtr) {}); + auto timer = + node->create_wall_timer(std::chrono::seconds(60), []() {}); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + node->get_node_waitables_interface()->add_waitable(waitable, nullptr); + + entities_collector_->add_node(node->get_node_base_interface()); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto shared_context = node->get_node_base_interface()->get_context(); + rcl_context_t * context = shared_context->get_rcl_context().get(); + EXPECT_EQ( + RCL_RET_OK, + rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); + RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + + auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); + + rclcpp::GuardCondition guard_condition(shared_context); + rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); + + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + + EXPECT_EQ( + 1u + expected_number_of_entities->subscriptions, + entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(1u + expected_number_of_entities->timers, entities_collector_->get_number_of_timers()); + EXPECT_EQ( + 1u + expected_number_of_entities->services, + entities_collector_->get_number_of_services()); + EXPECT_EQ( + 1u + expected_number_of_entities->clients, + entities_collector_->get_number_of_clients()); + + // One extra for the executor + EXPECT_EQ( + 2u + expected_number_of_entities->waitables, + entities_collector_->get_number_of_waitables()); + + entities_collector_->remove_node(node->get_node_base_interface()); + entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); + EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); + EXPECT_EQ(0u, entities_collector_->get_number_of_services()); + EXPECT_EQ(0u, entities_collector_->get_number_of_clients()); + // Still one for the executor + EXPECT_EQ(1u, entities_collector_->get_number_of_waitables()); +} From 621d3bd442bb6b3861fa620ab522bc3d3e36f6e8 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 7 Oct 2020 11:18:24 -0700 Subject: [PATCH 10/61] [foxy backport] Derive and throw exception in spin_some spin_all for StaticSingleThreadedExecutor (#1385) * Derive and throw exception in spin_some spin_all for StaticSingleThreadedExecutor (#1220) * Derive and throw exception in spin_some spin_all Signed-off-by: Stephen Brawner * Fix style and add unit test Signed-off-by: Stephen Brawner * Remove header changes and throw exceptions in .cpp Signed-off-by: Stephen Brawner --- .../include/rclcpp/exceptions/exceptions.hpp | 9 ++++ rclcpp/src/rclcpp/executor.cpp | 12 +++++ rclcpp/test/CMakeLists.txt | 6 +++ .../test_static_single_threaded_executor.cpp | 48 +++++++++++++++++++ 4 files changed, 75 insertions(+) create mode 100644 rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index d501acd71c..233b4b67ba 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -100,6 +100,15 @@ class InvalidServiceNameError : public NameValidationError {} }; +class UnimplementedError : public std::runtime_error +{ +public: + UnimplementedError() + : std::runtime_error("This code is unimplemented.") {} + explicit UnimplementedError(const std::string & msg) + : std::runtime_error(msg) {} +}; + /// Throw a C++ std::exception which was created based on an rcl error. /** * Passing nullptr for reset_error is safe and will avoid calling any function diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 0c83e0deba..5c295f320f 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -22,6 +22,7 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" #include "rclcpp/node.hpp" #include "rclcpp/scope_exit.hpp" #include "rclcpp/utilities.hpp" @@ -215,6 +216,12 @@ Executor::spin_node_some(std::shared_ptr node) void Executor::spin_some(std::chrono::nanoseconds max_duration) { + if (nullptr != dynamic_cast(this)) { + throw rclcpp::exceptions::UnimplementedError( + "spin_some is not implemented for StaticSingleThreadedExecutor, use spin or " + "spin_until_future_complete"); + } + auto start = std::chrono::steady_clock::now(); auto max_duration_not_elapsed = [max_duration, start]() { if (std::chrono::nanoseconds(0) == max_duration) { @@ -256,6 +263,11 @@ Executor::spin_once_impl(std::chrono::nanoseconds timeout) void Executor::spin_once(std::chrono::nanoseconds timeout) { + if (nullptr != dynamic_cast(this)) { + throw rclcpp::exceptions::UnimplementedError( + "spin_once is not implemented for StaticSingleThreadedExecutor, use spin or " + "spin_until_future_complete"); + } if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 0f5d5b9a3c..21221e3596 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -458,6 +458,12 @@ if(TARGET test_interface_traits) target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() +ament_add_gtest(test_static_single_threaded_executor rclcpp/executors/test_static_single_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_static_single_threaded_executor) + target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME}) +endif() + ament_add_gtest(test_multi_threaded_executor rclcpp/executors/test_multi_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_multi_threaded_executor) diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp new file mode 100644 index 0000000000..7ff72426f9 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -0,0 +1,48 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +using namespace std::chrono_literals; + +class TestStaticSingleThreadedExecutor : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestStaticSingleThreadedExecutor, check_unimplemented) { + rclcpp::executors::StaticSingleThreadedExecutor executor; + auto node = std::make_shared("node", "ns"); + executor.add_node(node); + + EXPECT_THROW(executor.spin_some(), rclcpp::exceptions::UnimplementedError); + EXPECT_THROW(executor.spin_once(0ns), rclcpp::exceptions::UnimplementedError); +} From 666013915e20767cea704c8cb329e3c5c9cea517 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 7 Oct 2020 11:29:27 -0700 Subject: [PATCH 11/61] [foxy backport] Parameterize test executors for all executor types (#1222) (#1386) * Parameterize test executors for all executor types (#1222) * Relocate test_executor to executors directory Signed-off-by: Stephen Brawner * Parametrize test_executors for all executor types Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * More fixup Signed-off-by: Stephen Brawner * Fixup Signed-off-by: Stephen Brawner * Adding issue for tracking Signed-off-by: Stephen Brawner * Remove tests for non-foxy API Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 16 +- .../test/rclcpp/executors/test_executors.cpp | 325 ++++++++++++++++++ rclcpp/test/rclcpp/test_executor.cpp | 69 ---- 3 files changed, 333 insertions(+), 77 deletions(-) create mode 100644 rclcpp/test/rclcpp/executors/test_executors.cpp delete mode 100644 rclcpp/test/rclcpp/test_executor.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 21221e3596..d150cad49b 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -396,14 +396,6 @@ if(TARGET test_duration) target_link_libraries(test_duration ${PROJECT_NAME}) endif() -ament_add_gtest(test_executor rclcpp/test_executor.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") -if(TARGET test_executor) - ament_target_dependencies(test_executor - "rcl") - target_link_libraries(test_executor ${PROJECT_NAME}) -endif() - ament_add_gtest(test_logger rclcpp/test_logger.cpp) target_link_libraries(test_logger ${PROJECT_NAME}) @@ -458,6 +450,14 @@ if(TARGET test_interface_traits) target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() +ament_add_gtest(test_executors rclcpp/executors/test_executors.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_executors) + ament_target_dependencies(test_executors + "rcl") + target_link_libraries(test_executors ${PROJECT_NAME}) +endif() + ament_add_gtest(test_static_single_threaded_executor rclcpp/executors/test_static_single_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_static_single_threaded_executor) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp new file mode 100644 index 0000000000..035c154586 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -0,0 +1,325 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * This test checks all implementations of rclcpp::executor to check they pass they basic API + * tests. Anything specific to any executor in particular should go in a separate test file. + * + */ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/empty.hpp" + +using namespace std::chrono_literals; + +template +class TestExecutors : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + callback_count = 0; + std::stringstream topic_name; + const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); + topic_name << "topic_" << test_info->test_case_name() << "_" << test_info->name(); + + publisher = node->create_publisher(topic_name.str(), rclcpp::QoS(10)); + auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; + subscription = + node->create_subscription( + topic_name.str(), rclcpp::QoS(10), std::move(callback)); + } + + void TearDown() + { + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + } + + rclcpp::Node::SharedPtr node; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + int callback_count; +}; + +// spin_all and spin_some are not implemented correctly in StaticSingleThreadedExecutor, see: +// https://github.com/ros2/rclcpp/issues/1219 for tracking +template +class TestExecutorsStable : public TestExecutors {}; + +using ExecutorTypes = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor, + rclcpp::executors::StaticSingleThreadedExecutor>; + +class ExecutorTypeNames +{ +public: + template + static std::string GetName(int idx) + { + (void)idx; + if (std::is_same()) { + return "SingleThreadedExecutor"; + } + + if (std::is_same()) { + return "MultiThreadedExecutor"; + } + + if (std::is_same()) { + return "StaticSingleThreadedExecutor"; + } + + return ""; + } +}; + +// TYPED_TEST_CASE is deprecated as of gtest 1.9, use TYPED_TEST_SUITE when gtest dependency +// is updated. +TYPED_TEST_CASE(TestExecutors, ExecutorTypes, ExecutorTypeNames); + +// StaticSingleThreadedExecutor is not included in these tests for now, due to: +// https://github.com/ros2/rclcpp/issues/1219 +using StandardExecutors = + ::testing::Types< + rclcpp::executors::SingleThreadedExecutor, + rclcpp::executors::MultiThreadedExecutor>; +TYPED_TEST_CASE(TestExecutorsStable, StandardExecutors, ExecutorTypeNames); + +// Make sure that executors detach from nodes when destructing +TYPED_TEST(TestExecutors, detachOnDestruction) { + using ExecutorType = TypeParam; + { + ExecutorType executor; + executor.add_node(this->node); + } + { + ExecutorType executor; + EXPECT_NO_THROW(executor.add_node(this->node)); + } +} + +// Make sure that the executor can automatically remove expired nodes correctly +// Currently fails for StaticSingleThreadedExecutor so it is being skipped, see: +// https://github.com/ros2/rclcpp/issues/1231 +TYPED_TEST(TestExecutorsStable, addTemporaryNode) { + using ExecutorType = TypeParam; + ExecutorType executor; + + { + // Let node go out of scope before executor.spin() + auto node = std::make_shared("temporary_node"); + executor.add_node(node); + } + + // Sleep for a short time to verify executor.spin() is going, and didn't throw. + std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); + + std::this_thread::sleep_for(50ms); + rclcpp::shutdown(); + spinner.join(); +} + +// Check executor throws properly if the same node is added a second time +TYPED_TEST(TestExecutors, addNodeTwoExecutors) { + using ExecutorType = TypeParam; + ExecutorType executor1; + ExecutorType executor2; + EXPECT_NO_THROW(executor1.add_node(this->node)); + EXPECT_THROW(executor2.add_node(this->node), std::runtime_error); +} + +// Check simple spin example +TYPED_TEST(TestExecutors, spinWithTimer) { + using ExecutorType = TypeParam; + ExecutorType executor; + + bool timer_completed = false; + auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); + executor.add_node(this->node); + + std::thread spinner([&]() {executor.spin();}); + + auto start = std::chrono::steady_clock::now(); + while (!timer_completed && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_TRUE(timer_completed); + + // Shutdown needs to be called before join, so that executor.spin() returns. + rclcpp::shutdown(); + spinner.join(); +} + +TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + bool timer_completed = false; + auto timer = this->node->create_wall_timer(1ms, [&]() {timer_completed = true;}); + + std::thread spinner([&]() {executor.spin();}); + // Sleep for a short time to verify executor.spin() is going, and didn't throw. + + auto start = std::chrono::steady_clock::now(); + while (!timer_completed && (std::chrono::steady_clock::now() - start) < 1s) { + std::this_thread::sleep_for(1ms); + } + + EXPECT_TRUE(timer_completed); + EXPECT_THROW(executor.spin(), std::runtime_error); + + // Shutdown needs to be called before join, so that executor.spin() returns. + rclcpp::shutdown(); + spinner.join(); +} + +// Check executor exits immediately if future is complete. +TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { + using ExecutorType = TypeParam; + ExecutorType executor; + executor.add_node(this->node); + + // test success of an immediately finishing future + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + // spin_until_future_complete is expected to exit immediately, but would block up until its + // timeout if the future is not checked before spin_once_impl. + auto start = std::chrono::steady_clock::now(); + auto shared_future = future.share(); + auto ret = executor.spin_until_future_complete(shared_future, 1s); + + // Check it didn't reach timeout + EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + { + rcl_guard_condition_options_t guard_condition_options = + rcl_guard_condition_get_default_options(); + + gc_ = rcl_get_zero_initialized_guard_condition(); + rcl_ret_t ret = rcl_guard_condition_init( + &gc_, + rclcpp::contexts::get_global_default_context()->get_rcl_context().get(), + guard_condition_options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret); + } + } + + bool + add_to_wait_set(rcl_wait_set_t * wait_set) override + { + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL); + if (RCL_RET_OK != ret) { + return false; + } + ret = rcl_trigger_guard_condition(&gc_); + return RCL_RET_OK == ret; + } + + bool + is_ready(rcl_wait_set_t * wait_set) override + { + (void)wait_set; + return true; + } + + void + execute() override + { + count_++; + std::this_thread::sleep_for(1ms); + } + + size_t + get_number_of_ready_guard_conditions() override {return 1;} + + size_t + get_count() + { + return count_; + } + +private: + size_t count_ = 0; + rcl_guard_condition_t gc_; +}; + +TYPED_TEST(TestExecutorsStable, spinSome) { + using ExecutorType = TypeParam; + ExecutorType executor; + auto waitable_interfaces = this->node->get_node_waitables_interface(); + auto my_waitable = std::make_shared(); + waitable_interfaces->add_waitable(my_waitable, nullptr); + executor.add_node(this->node); + + // Long timeout, doesn't block test from finishing because spin_some should exit after the + // first one completes. + bool spin_exited = false; + std::thread spinner([&spin_exited, &executor, this]() { + executor.spin_some(1s); + executor.remove_node(this->node); + spin_exited = true; + }); + + // Do some work until sufficient calls to the waitable occur, but keep going until either + // count becomes too large, spin exits, or the 1 second timeout completes. + auto start = std::chrono::steady_clock::now(); + while ( + my_waitable->get_count() <= 1 && + !spin_exited && + (std::chrono::steady_clock::now() - start < 1s)) + { + this->publisher->publish(std_msgs::msg::Empty()); + std::this_thread::sleep_for(1ms); + } + + EXPECT_EQ(1u, my_waitable->get_count()); + waitable_interfaces->remove_waitable(my_waitable, nullptr); + EXPECT_TRUE(spin_exited); + // Cancel if it hasn't exited already. + executor.cancel(); + + spinner.join(); +} diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp deleted file mode 100644 index 95371415fc..0000000000 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include -#include - -#include "rcl/error_handling.h" -#include "rcl/time.h" -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/rclcpp.hpp" - -using namespace std::chrono_literals; - -class TestExecutors : public ::testing::Test -{ -protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - void SetUp() - { - node = std::make_shared("my_node"); - } - - void TearDown() - { - node.reset(); - } - - rclcpp::Node::SharedPtr node; -}; - -// Make sure that executors detach from nodes when destructing -TEST_F(TestExecutors, detachOnDestruction) { - { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - } - { - rclcpp::executors::SingleThreadedExecutor executor; - EXPECT_NO_THROW(executor.add_node(node)); - } -} - -// Make sure that the executor can automatically remove expired nodes correctly -TEST_F(TestExecutors, addTemporaryNode) { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(std::make_shared("temporary_node")); - EXPECT_NO_THROW(executor.spin_some()); -} From dbf9a19edf9c7e577e19badd5194daef7d11f712 Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 17 Jul 2020 12:03:18 -0700 Subject: [PATCH 12/61] EXPECT_THROW_EQ and ASSERT_THROW_EQ macros for unittests (#1232) * EXPECT_THROW_EQ and ASSERT_THROW_EQ macros for unittests Signed-off-by: Stephen Brawner * Address PR Feedback Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 5 + rclcpp/test/utils/rclcpp_gtest_macros.hpp | 195 +++++++++++++++++ .../test/utils/test_rclcpp_gtest_macros.cpp | 204 ++++++++++++++++++ 3 files changed, 404 insertions(+) create mode 100644 rclcpp/test/utils/rclcpp_gtest_macros.hpp create mode 100644 rclcpp/test/utils/test_rclcpp_gtest_macros.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index d150cad49b..f9dbf0c4b5 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -518,6 +518,11 @@ if(TARGET test_subscription_options) target_link_libraries(test_subscription_options ${PROJECT_NAME}) endif() +ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp) +if(TARGET test_rclcpp_gtest_macros) + target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME}) +endif() + # Install test resources install( DIRECTORY resources diff --git a/rclcpp/test/utils/rclcpp_gtest_macros.hpp b/rclcpp/test/utils/rclcpp_gtest_macros.hpp new file mode 100644 index 0000000000..a7b1b97d77 --- /dev/null +++ b/rclcpp/test/utils/rclcpp_gtest_macros.hpp @@ -0,0 +1,195 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS__RCLCPP_GTEST_MACROS_HPP_ +#define UTILS__RCLCPP_GTEST_MACROS_HPP_ + +#include + +#include +#include + +#include "rclcpp/exceptions/exceptions.hpp" + +namespace rclcpp +{ +namespace testing +{ +namespace details +{ + +/** + * \brief Check if two thrown objects are equals. + * + * For generic thrown objects, probably is unlikely to be used. This type must + * overload the == and << operators. + */ +template::value>> +::testing::AssertionResult AreThrowableContentsEqual( + const T & expected, const T & actual, const char * expected_exception_expression, + const char * throwing_expression) +{ + if (expected == actual) { + return ::testing::AssertionSuccess() << + "'\nThe value of the non-standard throwable thrown by the expression\n'" << + throwing_expression << "'\n\nmatches the value of the expected thrown object\n'" << + expected_exception_expression << "'\n\t(" << expected << " == " << actual << ")\n"; + } + + return ::testing::AssertionFailure() << + "\nThe value of the non-standard throwable thrown by the expression\n'" << + throwing_expression << "'\n\ndoes not match the value of the expected thrown object\n'" << + expected_exception_expression << "'\n\t(" << expected << " != " << actual << ")\n"; +} + +/** + * \brief Check if two std::exceptions are equal according to their message. + * + * If the exception type also derives from rclcpp::Exception, then the next overload is called + * instead + */ +template::value>> +::testing::AssertionResult AreThrowableContentsEqual( + const std::exception & expected, const std::exception & actual, + const char * expected_exception_expression, + const char * throwing_expression) +{ + if (std::strcmp(expected.what(), actual.what()) == 0) { + return ::testing::AssertionSuccess() << + "'\nThe contents of the std::exception thrown by the expression\n'" << + throwing_expression << "':\n\te.what(): '" << actual.what() << + "'\n\nmatch the contents of the expected std::exception\n'" << + expected_exception_expression << "'\n\te.what(): '" << expected.what() << "'\n"; + } + + return ::testing::AssertionFailure() << + "\nThe contents of the std::exception thrown by the expression\n'" << + throwing_expression << "':\n\te.what(): '" << actual.what() << + "'\n\ndo not match the contents of the expected std::exception\n'" << + expected_exception_expression << "'\n\te.what(): '" << expected.what() << "'\n"; +} + +/** + * \brief Check if two exceptions that derive from rclcpp::RCLErrorBase are equal. + * + * This checks equality based on their return and message. It does not check the formatted + * message, which is what is reported by std::exception::what() for RCLErrors. + */ +template::value>> +::testing::AssertionResult AreThrowableContentsEqual( + const rclcpp::exceptions::RCLErrorBase & expected, + const rclcpp::exceptions::RCLErrorBase & actual, + const char * expected_exception_expression, + const char * throwing_expression) +{ + if ((expected.ret == actual.ret) && (expected.message == actual.message)) { + return ::testing::AssertionSuccess() << + "'\nThe contents of the RCLError thrown by the expression\n'" << throwing_expression << + "':\n\trcl_ret_t: " << actual.ret << "\n\tmessage: '" << actual.message << + "'\n\nmatch the contents of the expected RCLError\n'" << + expected_exception_expression << "'\n\trcl_ret_t: " << expected.ret << + "\n\tmessage: '" << expected.message << "'\n"; + } + + return ::testing::AssertionFailure() << + "'\nThe contents of the RCLError thrown by the expression\n'" << throwing_expression << + "':\n\trcl_ret_t: " << actual.ret << "\n\tmessage: '" << actual.message << + "'\n\ndo not match the contents of the expected RCLError\n'" << + expected_exception_expression << "'\n\trcl_ret_t: " << expected.ret << "\n\tmessage: '" << + expected.message << "'\n"; +} + +} // namespace details +} // namespace testing +} // namespace rclcpp + +/** + * \def CHECK_THROW_EQ_IMPL + * \brief Implemented check if statement throws expected exception. don't use directly, use + * RCLCPP_EXPECT_THROW_EQ or RCLCPP_ASSERT_THROW_EQ instead. + */ +#define CHECK_THROW_EQ_IMPL(throwing_statement, expected_exception, assertion_result) \ + do { \ + using ExceptionT = decltype(expected_exception); \ + try { \ + throwing_statement; \ + assertion_result = ::testing::AssertionFailure() << \ + "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \ + #expected_exception "'\nbut it did not throw.\n"; \ + } catch (const ExceptionT & e) { \ + assertion_result = \ + rclcpp::testing::details::AreThrowableContentsEqual( \ + expected_exception, e, #expected_exception, #throwing_statement); \ + } catch (const std::exception & e) { \ + assertion_result = ::testing::AssertionFailure() << \ + "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \ + #expected_exception "'\nbut it threw:\n\tType: " << typeid(e).name() << \ + "\n\te.what(): '" << e.what() << "'\n"; \ + } catch (...) { \ + assertion_result = ::testing::AssertionFailure() << \ + "\nExpected the expression:\n\t'" #throwing_statement "'\nto throw: \n\t'" << \ + #expected_exception "'\nbut it threw an unrecognized throwable type.\n"; \ + } \ + } while (0) + +/** + * \def RCLCPP_EXPECT_THROW_EQ + * \brief Check if a statement throws the expected exception type and that the exceptions matches + * the expected exception. + * + * Like other gtest EXPECT_ macros, this doesn't halt a test and return on failure. Instead it + * just adds a failure to the current test. + * + * See test_gtest_macros.cpp for examples + */ +#define RCLCPP_EXPECT_THROW_EQ(throwing_statement, expected_exception) \ + do { \ + ::testing::AssertionResult \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ + ::testing::AssertionSuccess(); \ + CHECK_THROW_EQ_IMPL( \ + throwing_statement, \ + expected_exception, \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \ + EXPECT_TRUE(is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \ + } while (0) + +/** + * \def RCLCPP_ASSERT_THROW_EQ + * \brief Assert that a statement throws the expected exception type and that the exceptions + * matches the expected exception. + * + * See test_gtest_macros.cpp for examples + * + * Like other gtest ASSERT_ macros, this will halt the test on failure and return. + */ +#define RCLCPP_ASSERT_THROW_EQ(throwing_statement, expected_exception) \ + do { \ + ::testing::AssertionResult \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable = \ + ::testing::AssertionSuccess(); \ + CHECK_THROW_EQ_IMPL( \ + throwing_statement, \ + expected_exception, \ + is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \ + ASSERT_TRUE(is_the_result_of_the_throwing_expression_equal_to_the_expected_throwable); \ + } while (0) + +#endif // UTILS__RCLCPP_GTEST_MACROS_HPP_ diff --git a/rclcpp/test/utils/test_rclcpp_gtest_macros.cpp b/rclcpp/test/utils/test_rclcpp_gtest_macros.cpp new file mode 100644 index 0000000000..2c3b3ff20b --- /dev/null +++ b/rclcpp/test/utils/test_rclcpp_gtest_macros.cpp @@ -0,0 +1,204 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include + +#include "./rclcpp_gtest_macros.hpp" + +#include "rcl/rcl.h" +#include "rclcpp/rclcpp.hpp" + +struct NonStandardThrowable +{ + bool operator==(const NonStandardThrowable &) const + { + return true; + } +}; + +std::ostream & operator<<(std::ostream & os, const NonStandardThrowable &) +{ + os << "NonStandardThrowable"; + return os; +} + +TEST(TestGtestMacros, standard_exceptions) { + RCLCPP_EXPECT_THROW_EQ( + throw std::runtime_error("some runtime error"), + std::runtime_error("some runtime error")); + + RCLCPP_EXPECT_THROW_EQ( + throw std::invalid_argument("some invalid argument error"), + std::invalid_argument("some invalid argument error")); + + RCLCPP_ASSERT_THROW_EQ( + throw std::runtime_error("some runtime error"), + std::runtime_error("some runtime error")); + + RCLCPP_ASSERT_THROW_EQ( + throw std::invalid_argument("some invalid argument error"), + std::invalid_argument("some invalid argument error")); +} + +TEST(TestGtestMacros, standard_exceptions_not_equals) { + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + CHECK_THROW_EQ_IMPL( + throw std::runtime_error("some runtime error"), + std::range_error("some runtime error"), + result); + EXPECT_FALSE(result); + + CHECK_THROW_EQ_IMPL( + throw std::invalid_argument("some invalid argument error"), + std::invalid_argument("some different invalid argument error"), + result); + EXPECT_FALSE(result); +} + +TEST(TestGTestMacros, non_standard_types) { + RCLCPP_EXPECT_THROW_EQ(throw 0, 0); + + RCLCPP_EXPECT_THROW_EQ(throw 42, 42); + + RCLCPP_EXPECT_THROW_EQ(throw std::string("some string"), std::string("some string")); + + RCLCPP_EXPECT_THROW_EQ(throw NonStandardThrowable(), NonStandardThrowable()); + + RCLCPP_ASSERT_THROW_EQ(throw 0, 0); + + RCLCPP_ASSERT_THROW_EQ(throw 42, 42); + + RCLCPP_ASSERT_THROW_EQ(throw std::string("some string"), std::string("some string")); + + RCLCPP_ASSERT_THROW_EQ(throw NonStandardThrowable(), NonStandardThrowable()); +} + +TEST(TestGTestMacros, non_standard_types_not_equals) { + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + + CHECK_THROW_EQ_IMPL(throw 0, 1, result); + EXPECT_FALSE(result); + result = ::testing::AssertionSuccess(); + + CHECK_THROW_EQ_IMPL(throw -42, 42, result); + EXPECT_FALSE(result); + result = ::testing::AssertionSuccess(); + + CHECK_THROW_EQ_IMPL(throw std::string("some string"), std::string("some other string"), result); + EXPECT_FALSE(result); +} + +TEST(TestGTestMacros, rclcpp_exceptions) { + rcutils_error_state_t rcl_error_state = {"this is some error message", __FILE__, __LINE__}; + { + auto expected = + rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + RCLCPP_ASSERT_THROW_EQ(throw actual, expected); + } + { + auto expected = + rclcpp::exceptions::RCLBadAlloc(RCL_RET_BAD_ALLOC, &rcl_error_state); + auto actual = + rclcpp::exceptions::RCLBadAlloc(RCL_RET_BAD_ALLOC, &rcl_error_state); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + } + { + // Prefixes are not checked + auto expected = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "different_prefix"); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + RCLCPP_ASSERT_THROW_EQ(throw actual, expected); + } + { + // File names are not checked + rcutils_error_state_t different_error_state = rcl_error_state; + std::snprintf( + different_error_state.file, RCUTILS_ERROR_STATE_FILE_MAX_LENGTH, "different_file.cpp"); + auto expected = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &different_error_state, "exception_prefix"); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + RCLCPP_ASSERT_THROW_EQ(throw actual, expected); + } + { + // Line numbers are not checked + rcutils_error_state_t different_error_state = rcl_error_state; + different_error_state.line_number += 42; + auto expected = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &different_error_state, "exception_prefix"); + RCLCPP_EXPECT_THROW_EQ(throw actual, expected); + RCLCPP_ASSERT_THROW_EQ(throw actual, expected); + } +} + +TEST(TestGTestMacros, rclcpp_exceptions_not_equal) { + rcutils_error_state_t rcl_error_state = {"this is some error message", __FILE__, __LINE__}; + { + // Check different return errors + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + auto expected = + rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + + auto actual = + rclcpp::exceptions::RCLError(RCL_RET_BAD_ALLOC, &rcl_error_state, "exception_prefix"); + CHECK_THROW_EQ_IMPL(throw actual, expected, result); + EXPECT_FALSE(result); + } + { + // Check different error messages + rcutils_error_state_t different_error_state = rcl_error_state; + std::snprintf( + different_error_state.message, + RCUTILS_ERROR_STATE_MESSAGE_MAX_LENGTH, + "this is a different error message"); + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + auto expected = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, &different_error_state, "exception_prefix"); + CHECK_THROW_EQ_IMPL(throw actual, expected, result); + EXPECT_FALSE(result); + } + { + // Check different exception types + ::testing::AssertionResult result = ::testing::AssertionSuccess(); + auto expected = + rclcpp::exceptions::RCLError(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + auto actual = + rclcpp::exceptions::RCLInvalidArgument(RCL_RET_ERROR, &rcl_error_state, "exception_prefix"); + CHECK_THROW_EQ_IMPL(throw actual, expected, result); + EXPECT_FALSE(result); + } +} From d5720da26702333c3a0cf413ea38b1eb87997df9 Mon Sep 17 00:00:00 2001 From: brawner Date: Mon, 20 Jul 2020 13:03:24 -0700 Subject: [PATCH 13/61] Unittests for memory strategy files, except allocator_memory_strategy (#1189) * Unit tests for memory_strategy classes (part 1) Adds unit tests for: * strategies/message_pool_memory_strategy.hpp * memory_strategy.cpp * message_memory_strategy.cpp Signed-off-by: Stephen Brawner * Address PR Feedback Signed-off-by: Stephen Brawner * Update with new macros Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 20 + .../test_message_pool_memory_strategy.cpp | 73 ++++ rclcpp/test/rclcpp/test_memory_strategy.cpp | 400 ++++++++++++++++++ .../rclcpp/test_message_memory_strategy.cpp | 58 +++ 4 files changed, 551 insertions(+) create mode 100644 rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp create mode 100644 rclcpp/test/rclcpp/test_memory_strategy.cpp create mode 100644 rclcpp/test/rclcpp/test_message_memory_strategy.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index f9dbf0c4b5..bf2fa94af8 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -25,6 +25,14 @@ if(TARGET test_allocator_memory_strategy) ) target_link_libraries(test_allocator_memory_strategy ${PROJECT_NAME}) endif() +ament_add_gtest(test_message_pool_memory_strategy rclcpp/strategies/test_message_pool_memory_strategy.cpp) +if(TARGET test_message_pool_memory_strategy) + ament_target_dependencies(test_message_pool_memory_strategy + "rcl" + "test_msgs" + ) + target_link_libraries(test_message_pool_memory_strategy ${PROJECT_NAME}) +endif() ament_add_gtest(test_any_service_callback rclcpp/test_any_service_callback.cpp) if(TARGET test_any_service_callback) ament_target_dependencies(test_any_service_callback @@ -126,6 +134,18 @@ ament_target_dependencies(test_loaned_message ) target_link_libraries(test_loaned_message ${PROJECT_NAME}) +ament_add_gtest(test_memory_strategy rclcpp/test_memory_strategy.cpp) +ament_target_dependencies(test_memory_strategy + "test_msgs" +) +target_link_libraries(test_memory_strategy ${PROJECT_NAME}) + +ament_add_gtest(test_message_memory_strategy rclcpp/test_message_memory_strategy.cpp) +ament_target_dependencies(test_message_memory_strategy + "test_msgs" +) +target_link_libraries(test_message_memory_strategy ${PROJECT_NAME}) + ament_add_gtest(test_node rclcpp/test_node.cpp TIMEOUT 240) if(TARGET test_node) ament_target_dependencies(test_node diff --git a/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp new file mode 100644 index 0000000000..b68bc96ab6 --- /dev/null +++ b/rclcpp/test/rclcpp/strategies/test_message_pool_memory_strategy.cpp @@ -0,0 +1,73 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "gtest/gtest.h" + +#include "rclcpp/strategies/message_pool_memory_strategy.hpp" +#include "test_msgs/msg/empty.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +using rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy; + +class TestMessagePoolMemoryStrategy : public ::testing::Test +{ +public: + void SetUp() + { + message_memory_strategy_ = + std::make_shared>(); + } + +protected: + std::shared_ptr> message_memory_strategy_; +}; + +TEST_F(TestMessagePoolMemoryStrategy, construct_destruct) { + ASSERT_NE(nullptr, message_memory_strategy_); + EXPECT_NE(nullptr, message_memory_strategy_->message_allocator_); + EXPECT_NE(nullptr, message_memory_strategy_->serialized_message_allocator_); + EXPECT_NE(nullptr, message_memory_strategy_->buffer_allocator_); +} + +TEST_F(TestMessagePoolMemoryStrategy, borrow_return) { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); + + EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); +} + +TEST_F(TestMessagePoolMemoryStrategy, borrow_too_many) { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); + + // Size is 1, borrowing second time should fail + RCLCPP_EXPECT_THROW_EQ( + message_memory_strategy_->borrow_message(), + std::runtime_error("Tried to access message that was still in use! Abort.")); + EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); +} + +TEST_F(TestMessagePoolMemoryStrategy, return_unrecognized) { + auto message = message_memory_strategy_->borrow_message(); + ASSERT_NE(nullptr, message); + + auto unrecognized = std::make_shared(); + // Unrecognized does not belong to pool + RCLCPP_EXPECT_THROW_EQ( + message_memory_strategy_->return_message(unrecognized), + std::runtime_error("Unrecognized message ptr in return_message.")); + EXPECT_NO_THROW(message_memory_strategy_->return_message(message)); +} diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp new file mode 100644 index 0000000000..f076094643 --- /dev/null +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -0,0 +1,400 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rclcpp/strategies/allocator_memory_strategy.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +using rclcpp::memory_strategy::MemoryStrategy; +using WeakNodeList = std::list; + +/** + * Mock Waitable class + */ +class TestWaitable : public rclcpp::Waitable +{ +public: + bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + bool is_ready(rcl_wait_set_t *) override {return true;} + void execute() override {} +}; + +class TestMemoryStrategy : public ::testing::Test +{ +public: + TestMemoryStrategy() + : memory_strategy_(nullptr) {} + + void SetUp() override + { + rclcpp::init(0, nullptr); + + // This doesn't test AllocatorMemoryStrategy directly, so we cast to the base class. + // AllocatorMemoryStrategy is more commonly used than MessagePoolMemoryStrategy + // so we use this derived class for these tests. + memory_strategy_ = + std::make_shared< + rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr memory_strategy() + { + return memory_strategy_; + } + +private: + std::shared_ptr memory_strategy_; +}; + +TEST_F(TestMemoryStrategy, construct_destruct) { + EXPECT_NE(nullptr, memory_strategy()); +} + +TEST_F(TestMemoryStrategy, get_subscription_by_handle) { + WeakNodeList nodes; + std::shared_ptr subscription_handle; + rclcpp::SubscriptionBase::SharedPtr found_subscription = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + const rclcpp::QoS qos(10); + + { + auto subscription = node->create_subscription< + test_msgs::msg::Empty, decltype(subscription_callback)>( + "topic", qos, std::move(subscription_callback)); + + subscription_handle = subscription->get_subscription_handle(); + + EXPECT_EQ( + subscription, + memory_strategy()->get_subscription_by_handle(subscription_handle, nodes)); + } // subscription goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_subscription_by_handle(subscription_handle, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_subscription_by_handle(subscription_handle, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_subscription_by_handle(subscription_handle, nodes)); +} + +TEST_F(TestMemoryStrategy, get_service_by_handle) { + WeakNodeList nodes; + std::shared_ptr service_handle; + rclcpp::ServiceBase::SharedPtr found_service = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto service_callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + const rclcpp::QoS qos(10); + + { + auto service = node->create_service( + "service", std::move(service_callback), + rmw_qos_profile_services_default, callback_group); + + service_handle = service->get_service_handle(); + + EXPECT_EQ( + service, + memory_strategy()->get_service_by_handle(service_handle, nodes)); + } // service goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_service_by_handle(service_handle, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_service_by_handle(service_handle, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_service_by_handle(service_handle, nodes)); +} + +TEST_F(TestMemoryStrategy, get_client_by_handle) { + WeakNodeList nodes; + std::shared_ptr client_handle; + rclcpp::ClientBase::SharedPtr found_client = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + { + auto client = node->create_client( + "service", rmw_qos_profile_services_default, callback_group); + + client_handle = client->get_client_handle(); + + EXPECT_EQ( + client, + memory_strategy()->get_client_by_handle(client_handle, nodes)); + } // client goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_client_by_handle(client_handle, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_client_by_handle(client_handle, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_client_by_handle(client_handle, nodes)); +} + +TEST_F(TestMemoryStrategy, get_timer_by_handle) { + WeakNodeList nodes; + std::shared_ptr timer_handle; + rclcpp::TimerBase::SharedPtr found_timer = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + { + auto timer_callback = []() {}; + auto timer = node->create_wall_timer( + std::chrono::milliseconds(1), timer_callback, callback_group); + + timer_handle = timer->get_timer_handle(); + + EXPECT_EQ( + timer, + memory_strategy()->get_timer_by_handle(timer_handle, nodes)); + } // timer goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_timer_by_handle(timer_handle, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_timer_by_handle(timer_handle, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_timer_by_handle(timer_handle, nodes)); +} + +TEST_F(TestMemoryStrategy, get_node_by_group) { + WeakNodeList nodes; + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + { + auto node = std::make_shared("node", "ns"); + auto node_handle = node->get_node_base_interface(); + nodes.push_back(node_handle); + memory_strategy()->collect_entities(nodes); + EXPECT_EQ( + nullptr, + memory_strategy()->get_node_by_group(nullptr, nodes)); + + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + EXPECT_EQ( + node_handle, + memory_strategy()->get_node_by_group(callback_group, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_node_by_group(callback_group, nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_subscription) { + WeakNodeList nodes; + rclcpp::SubscriptionBase::SharedPtr subscription = nullptr; + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + // This group is just used to test that a callback group that is held as a weak pointer + // by node, doesn't confuse get_group_by_subscription() when it goes out of scope + auto non_persistant_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + const rclcpp::QoS qos(10); + + rclcpp::SubscriptionOptions subscription_options; + + // This callback group is held as a shared_ptr in subscription_options, which means it + // stays alive as long as subscription does. + subscription_options.callback_group = callback_group; + + subscription = node->create_subscription< + test_msgs::msg::Empty, decltype(subscription_callback)>( + "topic", qos, std::move(subscription_callback), subscription_options); + + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_subscription(subscription, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_subscription(subscription, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_subscription(subscription, nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_service) { + WeakNodeList nodes; + rclcpp::ServiceBase::SharedPtr service = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto service_callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + const rclcpp::QoS qos(10); + + service = node->create_service( + "service", std::move(service_callback), + rmw_qos_profile_services_default, callback_group); + + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_service(service, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_service(service, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_service(service, nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_client) { + WeakNodeList nodes; + rclcpp::ClientBase::SharedPtr client = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + client = node->create_client( + "service", rmw_qos_profile_services_default, callback_group); + + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_client(client, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_client(client, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_client(client, nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_timer) { + WeakNodeList nodes; + rclcpp::TimerBase::SharedPtr timer = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto timer_callback = []() {}; + timer = node->create_wall_timer( + std::chrono::milliseconds(1), timer_callback, callback_group); + + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_timer(timer, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_timer(timer, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_timer(timer, nodes)); +} + +TEST_F(TestMemoryStrategy, get_group_by_waitable) { + WeakNodeList nodes; + rclcpp::Waitable::SharedPtr waitable = nullptr; + { + auto node = std::make_shared("node", "ns"); + nodes.push_back(node->get_node_base_interface()); + memory_strategy()->collect_entities(nodes); + { + waitable = std::make_shared(); + auto callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + node->get_node_waitables_interface()->add_waitable(waitable, callback_group); + + EXPECT_EQ( + callback_group, + memory_strategy()->get_group_by_waitable(waitable, nodes)); + } // callback_group goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_waitable(waitable, nodes)); + } // Node goes out of scope + EXPECT_EQ( + nullptr, + memory_strategy()->get_group_by_waitable(waitable, nodes)); +} diff --git a/rclcpp/test/rclcpp/test_message_memory_strategy.cpp b/rclcpp/test/rclcpp/test_message_memory_strategy.cpp new file mode 100644 index 0000000000..26ebd5ec37 --- /dev/null +++ b/rclcpp/test/rclcpp/test_message_memory_strategy.cpp @@ -0,0 +1,58 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/message_memory_strategy.hpp" +#include "test_msgs/msg/empty.hpp" + +TEST(TestMemoryStrategies, construct_destruct) { + rclcpp::message_memory_strategy::MessageMemoryStrategy memory_strategy1; + + EXPECT_NE(nullptr, memory_strategy1.message_allocator_); + EXPECT_NE(nullptr, memory_strategy1.serialized_message_allocator_); + EXPECT_NE(nullptr, memory_strategy1.buffer_allocator_); + + auto allocator = std::make_shared>(); + rclcpp::message_memory_strategy::MessageMemoryStrategy memory_strategy2( + allocator); + + EXPECT_NE(nullptr, memory_strategy2.message_allocator_); + EXPECT_NE(nullptr, memory_strategy2.serialized_message_allocator_); + EXPECT_NE(nullptr, memory_strategy2.buffer_allocator_); +} + +TEST(TestMemoryStrategies, standard_allocation) { + auto memory_strategy = + rclcpp::message_memory_strategy::MessageMemoryStrategy< + test_msgs::msg::Empty>::create_default(); + ASSERT_NE(nullptr, memory_strategy); + + auto borrowed_message = memory_strategy->borrow_message(); + ASSERT_NE(nullptr, borrowed_message); + EXPECT_NO_THROW(memory_strategy->return_message(borrowed_message)); + + auto serialized_message = memory_strategy->borrow_serialized_message(); + ASSERT_NE(nullptr, serialized_message); + EXPECT_EQ(0u, serialized_message->capacity()); + EXPECT_NO_THROW(memory_strategy->return_serialized_message(serialized_message)); + + memory_strategy->set_default_buffer_capacity(42); + serialized_message = memory_strategy->borrow_serialized_message(); + ASSERT_NE(nullptr, serialized_message); + EXPECT_EQ(42u, serialized_message->capacity()); + EXPECT_NO_THROW(memory_strategy->return_serialized_message(serialized_message)); +} From 9c4387984362b3d91c8ad1b454d59c98ef192404 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 28 Jul 2020 14:58:37 -0700 Subject: [PATCH 14/61] fix node graph test with Connext and CycloneDDS returning actual data (#1245) * fix node graph test with Connext and CycloneDDS returning actual data Signed-off-by: Dirk Thomas * use ADD_FAILURE() Signed-off-by: Dirk Thomas --- .../node_interfaces/test_node_graph.cpp | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index db69207f3b..c29b24549a 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -170,10 +170,28 @@ TEST_F(TestNodeGraph, get_info_by_topic) EXPECT_EQ(rclcpp::EndpointType::Publisher, const_publisher_endpoint_info.endpoint_type()); rclcpp::QoS actual_qos = publisher_endpoint_info.qos_profile(); - EXPECT_EQ(0u, actual_qos.get_rmw_qos_profile().depth); + switch (actual_qos.get_rmw_qos_profile().history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + EXPECT_EQ(1u, actual_qos.get_rmw_qos_profile().depth); + break; + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + EXPECT_EQ(0u, actual_qos.get_rmw_qos_profile().depth); + break; + default: + ADD_FAILURE() << "unexpected history"; + } rclcpp::QoS const_actual_qos = const_publisher_endpoint_info.qos_profile(); - EXPECT_EQ(0u, const_actual_qos.get_rmw_qos_profile().depth); + switch (const_actual_qos.get_rmw_qos_profile().history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + EXPECT_EQ(1u, const_actual_qos.get_rmw_qos_profile().depth); + break; + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + EXPECT_EQ(0u, const_actual_qos.get_rmw_qos_profile().depth); + break; + default: + ADD_FAILURE() << "unexpected history"; + } auto endpoint_gid = publisher_endpoint_info.endpoint_gid(); auto const_endpoint_gid = const_publisher_endpoint_info.endpoint_gid(); From fea538f8c8961c0fa712bbfc93cc74af037933c4 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Thu, 30 Jul 2020 12:20:15 -0700 Subject: [PATCH 15/61] fix failing test with Connext since it doesn't wait for discovery (#1246) * fix failing test with Connext since it doesn't wait for discovery Signed-off-by: Dirk Thomas * Check for added service in the node graph Signed-off-by: Stephen Brawner Co-authored-by: Stephen Brawner --- .../node_interfaces/test_node_graph.cpp | 29 +++++++++++++++++-- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index c29b24549a..a1922d56af 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -23,6 +23,7 @@ #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/rclcpp.hpp" #include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" class TestNodeGraph : public ::testing::Test { @@ -87,17 +88,39 @@ TEST_F(TestNodeGraph, get_service_names_and_types) TEST_F(TestNodeGraph, get_service_names_and_types_by_node) { auto node1 = std::make_shared("node1", "ns"); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node1->create_service("node1_service", std::move(callback)); auto node2 = std::make_shared("node2", "ns"); const auto * node_graph = dynamic_cast(node1->get_node_graph_interface().get()); ASSERT_NE(nullptr, node_graph); + // rcl_get_service_names_and_types_by_node() expects the node to exist, otherwise it fails EXPECT_THROW( node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), std::runtime_error); - auto service_names_and_types1 = node_graph->get_service_names_and_types_by_node("node1", "/ns"); - auto service_names_and_types2 = node_graph->get_service_names_and_types_by_node("node2", "/ns"); - EXPECT_EQ(service_names_and_types1.size(), service_names_and_types2.size()); + + // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph + // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. + auto services_of_node1 = + node_graph->get_service_names_and_types_by_node("node1", "/ns"); + auto services_of_node2 = + node_graph->get_service_names_and_types_by_node("node2", "/ns"); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { + services_of_node1 = node_graph->get_service_names_and_types_by_node("node1", "/ns"); + services_of_node2 = node_graph->get_service_names_and_types_by_node("node2", "/ns"); + if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) { + break; + } + } + + EXPECT_TRUE(services_of_node1.find("/ns/node1_service") != services_of_node1.end()); + EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end()); } TEST_F(TestNodeGraph, get_node_names_and_namespaces) From a5ffa3a513f38a0b42e5f469d61c002f547e6e0b Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 31 Jul 2020 13:33:51 -0700 Subject: [PATCH 16/61] Adjust test_static_executor_entities_collector for rmw_connext_cpp (#1251) It turns out rmw_connext_cpp adds a default waitable that other rmw implementations do not. Adjusting the unit test to take this into account in a non-rmw specific manner. Signed-off-by: Stephen Brawner --- .../executors/test_static_executor_entities_collector.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index 673df6ce7c..92ded7fe5a 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -227,7 +227,7 @@ class TestWaitable : public rclcpp::Waitable TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { auto node = std::make_shared("node", "ns"); - const auto expected_number_of_entities = get_number_of_default_entities(node); + auto expected_number_of_entities = get_number_of_default_entities(node); EXPECT_NE(nullptr, expected_number_of_entities); // Create 1 of each entity type @@ -244,6 +244,10 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { test_msgs::srv::Empty::Response::SharedPtr) {}); auto client = node->create_client("service"); auto waitable = std::make_shared(); + + // Adding a subscription with rmw_connext_cpp adds another waitable, so we need to get the + // current number of waitables just before adding the new waitable. + expected_number_of_entities->waitables = get_number_of_default_entities(node)->waitables; node->get_node_waitables_interface()->add_waitable(waitable, nullptr); entities_collector_->add_node(node->get_node_base_interface()); From 12aeba2cffa48b31abd14dd7f034e4532f3668bf Mon Sep 17 00:00:00 2001 From: brawner Date: Mon, 3 Aug 2020 13:02:58 -0700 Subject: [PATCH 17/61] Increase timeouts for connext for long tests (#1253) * Increase timeouts for connext for long tests Signed-off-by: Stephen Brawner * Fix cmakelists Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index bf2fa94af8..840c5de0e3 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -17,7 +17,13 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs SKIP_INSTALL ) -ament_add_gtest(test_allocator_memory_strategy rclcpp/strategies/test_allocator_memory_strategy.cpp) +# Increasing timeout because connext can take a long time to destroy nodes +# TODO(brawner) remove when destroying Node for Connext is resolved. See: +# https://github.com/ros2/rclcpp/issues/1250 +ament_add_gtest( + test_allocator_memory_strategy + rclcpp/strategies/test_allocator_memory_strategy.cpp + TIMEOUT 360) if(TARGET test_allocator_memory_strategy) ament_target_dependencies(test_allocator_memory_strategy "rcl" @@ -470,8 +476,13 @@ if(TARGET test_interface_traits) target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() -ament_add_gtest(test_executors rclcpp/executors/test_executors.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") +# TODO(brawner) remove when destroying Node for Connext is resolved. See: +# https://github.com/ros2/rclcpp/issues/1250 +ament_add_gtest( + test_executors + rclcpp/executors/test_executors.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" + TIMEOUT 180) if(TARGET test_executors) ament_target_dependencies(test_executors "rcl") From 93839e1eba0724a16ec5b33840ccd9209bea9a5e Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 4 Aug 2020 14:57:54 -0700 Subject: [PATCH 18/61] Simplify and fix allocator memory strategy unit test for connext (#1252) * Fix allocator memory strategy for connext Signed-off-by: Stephen Brawner * PR Feedback Signed-off-by: Stephen Brawner --- .../test_allocator_memory_strategy.cpp | 293 +++++++----------- 1 file changed, 120 insertions(+), 173 deletions(-) diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 5055d63eff..4c2a9c3527 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -70,14 +70,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test { public: TestAllocatorMemoryStrategy() - : allocator_memory_strategy_(nullptr), - num_ready_subscriptions_of_basic_node_(0), - num_ready_services_of_basic_node_(0), - num_ready_events_of_basic_node_(0), - num_ready_clients_of_basic_node_(0), - num_guard_conditions_of_basic_node_(0), - num_ready_timers_of_basic_node_(0), - num_waitables_of_basic_node_(0) {} + : allocator_memory_strategy_(nullptr) {} void SetUp() override { @@ -86,21 +79,6 @@ class TestAllocatorMemoryStrategy : public ::testing::Test // Even though this is just using a basic allocator, the custom allocator constructor was // not covered in general testing. allocator_memory_strategy_ = std::make_shared>(allocator_); - - auto basic_node = std::make_shared("basic_node", "ns"); - auto dummy_memory_strategy = std::make_shared>(); - WeakNodeList nodes; - nodes.push_back(basic_node->get_node_base_interface()); - dummy_memory_strategy->collect_entities(nodes); - - num_ready_subscriptions_of_basic_node_ = - dummy_memory_strategy->number_of_ready_subscriptions(); - num_ready_services_of_basic_node_ = dummy_memory_strategy->number_of_ready_services(); - num_ready_events_of_basic_node_ = dummy_memory_strategy->number_of_ready_events(); - num_ready_clients_of_basic_node_ = dummy_memory_strategy->number_of_ready_clients(); - num_guard_conditions_of_basic_node_ = dummy_memory_strategy->number_of_guard_conditions(); - num_ready_timers_of_basic_node_ = dummy_memory_strategy->number_of_ready_timers(); - num_waitables_of_basic_node_ = dummy_memory_strategy->number_of_waitables(); } void TearDown() override @@ -119,23 +97,33 @@ class TestAllocatorMemoryStrategy : public ::testing::Test // interfaces remain alive and valid // - std::shared_ptr create_node_with_subscription( - const std::string & name, bool with_exclusive_callback_group) + std::shared_ptr create_node_with_disabled_callback_groups(const std::string & name) + { + auto node = std::make_shared(name, "ns"); + + for (auto & group_weak_ptr : node->get_callback_groups()) { + auto group = group_weak_ptr.lock(); + if (group) { + group->can_be_taken_from() = false; + } + } + return node; + } + + std::shared_ptr create_node_with_subscription(const std::string & name) { auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; const rclcpp::QoS qos(10); - auto node_with_subscription = std::make_shared(name, "ns"); + auto node_with_subscription = create_node_with_disabled_callback_groups(name); rclcpp::SubscriptionOptions subscription_options; - if (with_exclusive_callback_group) { - auto callback_group = - node_with_subscription->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); + auto callback_group = + node_with_subscription->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); - subscription_options.callback_group = callback_group; - callback_groups_.push_back(callback_group); - } + subscription_options.callback_group = callback_group; + callback_groups_.push_back(callback_group); auto subscription = node_with_subscription->create_subscription< test_msgs::msg::Empty, decltype(subscription_callback)>( @@ -145,23 +133,18 @@ class TestAllocatorMemoryStrategy : public ::testing::Test return node_with_subscription; } - std::shared_ptr create_node_with_service( - const std::string & name, bool with_exclusive_callback_group = false) + std::shared_ptr create_node_with_service(const std::string & name) { auto service_callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; - auto node_with_service = std::make_shared(name, "ns"); + auto node_with_service = create_node_with_disabled_callback_groups(name); - rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; + auto callback_group = + node_with_service->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); - if (with_exclusive_callback_group) { - callback_group = - node_with_service->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - - callback_groups_.push_back(callback_group); - } + callback_groups_.push_back(callback_group); services_.push_back( node_with_service->create_service( @@ -169,18 +152,14 @@ class TestAllocatorMemoryStrategy : public ::testing::Test return node_with_service; } - std::shared_ptr create_node_with_client( - const std::string & name, bool with_exclusive_callback_group = false) + std::shared_ptr create_node_with_client(const std::string & name) { - auto node_with_client = std::make_shared(name, "ns"); - rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; - if (with_exclusive_callback_group) { - callback_group = - node_with_client->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - - callback_groups_.push_back(callback_group); - } + auto node_with_client = create_node_with_disabled_callback_groups(name); + auto callback_group = + node_with_client->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + callback_groups_.push_back(callback_group); clients_.push_back( node_with_client->create_client( @@ -188,19 +167,15 @@ class TestAllocatorMemoryStrategy : public ::testing::Test return node_with_client; } - std::shared_ptr create_node_with_timer( - const std::string & name, bool with_exclusive_callback_group = false) + std::shared_ptr create_node_with_timer(const std::string & name) { auto timer_callback = []() {}; - auto node_with_timer = std::make_shared(name, "ns"); - - rclcpp::CallbackGroup::SharedPtr callback_group = nullptr; - if (with_exclusive_callback_group) { - callback_group = - node_with_timer->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - callback_groups_.push_back(callback_group); - } + auto node_with_timer = create_node_with_disabled_callback_groups(name); + + auto callback_group = + node_with_timer->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + callback_groups_.push_back(callback_group); timers_.push_back( node_with_timer->create_wall_timer( @@ -208,55 +183,6 @@ class TestAllocatorMemoryStrategy : public ::testing::Test return node_with_timer; } - size_t number_of_ready_subscriptions_in_addition_to_basic_node() const - { - return - allocator_memory_strategy_->number_of_ready_subscriptions() - - num_ready_subscriptions_of_basic_node_; - } - - size_t number_of_ready_services_in_addition_to_basic_node() const - { - return - allocator_memory_strategy_->number_of_ready_services() - - num_ready_services_of_basic_node_; - } - - size_t number_of_ready_events_in_addition_to_basic_node() const - { - return - allocator_memory_strategy_->number_of_ready_events() - - num_ready_events_of_basic_node_; - } - - size_t number_of_ready_clients_in_addition_to_basic_node() const - { - return - allocator_memory_strategy_->number_of_ready_clients() - - num_ready_clients_of_basic_node_; - } - - size_t number_of_guard_conditions_in_addition_to_basic_node() const - { - return - allocator_memory_strategy_->number_of_guard_conditions() - - num_guard_conditions_of_basic_node_; - } - - size_t number_of_ready_timers_in_addition_to_basic_node() const - { - return - allocator_memory_strategy_->number_of_ready_timers() - - num_ready_timers_of_basic_node_; - } - - size_t number_of_waitables_in_addition_to_basic_node() const - { - return - allocator_memory_strategy_->number_of_waitables() - - num_waitables_of_basic_node_; - } - ::testing::AssertionResult TestNumberOfEntitiesAfterCollection( std::shared_ptr node, const RclWaitSetSizes & expected) @@ -266,14 +192,14 @@ class TestAllocatorMemoryStrategy : public ::testing::Test allocator_memory_strategy()->collect_entities(nodes); EXPECT_EQ( - expected.size_of_subscriptions, number_of_ready_subscriptions_in_addition_to_basic_node()); + expected.size_of_subscriptions, allocator_memory_strategy()->number_of_ready_subscriptions()); EXPECT_EQ( - expected.size_of_guard_conditions, number_of_guard_conditions_in_addition_to_basic_node()); - EXPECT_EQ(expected.size_of_timers, number_of_ready_timers_in_addition_to_basic_node()); - EXPECT_EQ(expected.size_of_clients, number_of_ready_clients_in_addition_to_basic_node()); - EXPECT_EQ(expected.size_of_services, number_of_ready_services_in_addition_to_basic_node()); - EXPECT_EQ(expected.size_of_events, number_of_ready_events_in_addition_to_basic_node()); - EXPECT_EQ(expected.size_of_waitables, number_of_waitables_in_addition_to_basic_node()); + expected.size_of_guard_conditions, allocator_memory_strategy()->number_of_guard_conditions()); + EXPECT_EQ(expected.size_of_timers, allocator_memory_strategy()->number_of_ready_timers()); + EXPECT_EQ(expected.size_of_clients, allocator_memory_strategy()->number_of_ready_clients()); + EXPECT_EQ(expected.size_of_services, allocator_memory_strategy()->number_of_ready_services()); + EXPECT_EQ(expected.size_of_events, allocator_memory_strategy()->number_of_ready_events()); + EXPECT_EQ(expected.size_of_waitables, allocator_memory_strategy()->number_of_waitables()); if (::testing::Test::HasFailure()) { return ::testing::AssertionFailure() << "Expected number of entities did not match actual counts"; @@ -378,7 +304,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test return ::testing::AssertionFailure() << "A node was retrieved with the specified get_next_*() function despite" " none of the nodes that were passed to it were added to the" - " allocator_memory_strategy"; + " allocator_memory_strategy. Retrieved node: " << failed_result.node_base->get_name(); } return ::testing::AssertionSuccess(); } @@ -421,14 +347,6 @@ class TestAllocatorMemoryStrategy : public ::testing::Test std::shared_ptr> allocator_; std::shared_ptr> allocator_memory_strategy_; - size_t num_ready_subscriptions_of_basic_node_; - size_t num_ready_services_of_basic_node_; - size_t num_ready_events_of_basic_node_; - size_t num_ready_clients_of_basic_node_; - size_t num_guard_conditions_of_basic_node_; - size_t num_ready_timers_of_basic_node_; - size_t num_waitables_of_basic_node_; - // These are generally kept as weak pointers in the rclcpp::Node interfaces, so they need to be // owned by this class. std::vector subscriptions_; @@ -440,16 +358,16 @@ class TestAllocatorMemoryStrategy : public ::testing::Test TEST_F(TestAllocatorMemoryStrategy, construct_destruct) { WeakNodeList nodes; - auto basic_node = std::make_shared("node", "ns"); + auto basic_node = create_node_with_disabled_callback_groups("basic_node"); nodes.push_back(basic_node->get_node_base_interface()); allocator_memory_strategy()->collect_entities(nodes); - EXPECT_EQ(0u, number_of_ready_subscriptions_in_addition_to_basic_node()); - EXPECT_EQ(0u, number_of_ready_services_in_addition_to_basic_node()); - EXPECT_EQ(0u, number_of_ready_events_in_addition_to_basic_node()); - EXPECT_EQ(0u, number_of_ready_clients_in_addition_to_basic_node()); - EXPECT_EQ(0u, number_of_guard_conditions_in_addition_to_basic_node()); - EXPECT_EQ(0u, number_of_ready_timers_in_addition_to_basic_node()); - EXPECT_EQ(0u, number_of_waitables_in_addition_to_basic_node()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_subscriptions()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_services()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_events()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_clients()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_guard_conditions()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_ready_timers()); + EXPECT_EQ(0u, allocator_memory_strategy()->number_of_waitables()); } TEST_F(TestAllocatorMemoryStrategy, add_remove_guard_conditions) { @@ -495,7 +413,12 @@ TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) { TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { RclWaitSetSizes expected_sizes = {}; expected_sizes.size_of_subscriptions = 1; - auto node_with_subscription = create_node_with_subscription("subscription_node", false); + if (std::string("rmw_connext_cpp") == rmw_get_implementation_identifier()) { + // For connext, a subscription will also add an event and waitable + expected_sizes.size_of_events += 1; + expected_sizes.size_of_waitables += 1; + } + auto node_with_subscription = create_node_with_subscription("subscription_node"); EXPECT_TRUE(TestNumberOfEntitiesAfterCollection(node_with_subscription, expected_sizes)); } @@ -521,7 +444,7 @@ TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_timer) { } TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) { - auto node = create_node_with_subscription("subscription_node", false); + auto node = create_node_with_subscription("subscription_node"); WeakNodeList nodes; nodes.push_back(node->get_node_base_interface()); allocator_memory_strategy()->collect_entities(nodes); @@ -531,7 +454,7 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_bad_arguments) { } TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_subscription) { - auto node_with_subscription = create_node_with_subscription("subscription_node", false); + auto node_with_subscription = create_node_with_subscription("subscription_node"); RclWaitSetSizes insufficient_capacities = SufficientWaitSetCapacities(); insufficient_capacities.size_of_subscriptions = 0; EXPECT_TRUE(TestAddHandlesToWaitSet(node_with_subscription, insufficient_capacities)); @@ -552,7 +475,7 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_client) { } TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { - auto node = std::make_shared("node", "ns"); + auto node = create_node_with_disabled_callback_groups("node"); rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); auto context = node->get_node_base_interface()->get_context(); rcl_context_t * rcl_context = context->get_rcl_context().get(); @@ -581,6 +504,8 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_timer) { } TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { + rcl_reset_error(); + rclcpp::Waitable::SharedPtr waitable = std::make_shared(); EXPECT_NO_THROW(allocator_memory_strategy()->add_waitable_handle(waitable)); EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); @@ -596,8 +521,8 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_waitable) { } TEST_F(TestAllocatorMemoryStrategy, get_next_subscription) { - auto node1 = create_node_with_subscription("node1", false); - auto node2 = create_node_with_subscription("node2", false); + auto node1 = create_node_with_subscription("node1"); + auto node2 = create_node_with_subscription("node2"); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -609,8 +534,8 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_subscription) { } TEST_F(TestAllocatorMemoryStrategy, get_next_service) { - auto node1 = create_node_with_service("node1", false); - auto node2 = create_node_with_service("node2", false); + auto node1 = create_node_with_service("node1"); + auto node2 = create_node_with_service("node2"); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -622,8 +547,8 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service) { } TEST_F(TestAllocatorMemoryStrategy, get_next_client) { - auto node1 = create_node_with_client("node1", false); - auto node2 = create_node_with_client("node2", false); + auto node1 = create_node_with_client("node1"); + auto node2 = create_node_with_client("node2"); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -635,8 +560,8 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client) { } TEST_F(TestAllocatorMemoryStrategy, get_next_timer) { - auto node1 = create_node_with_timer("node1", false); - auto node2 = create_node_with_timer("node2", false); + auto node1 = create_node_with_timer("node1"); + auto node2 = create_node_with_timer("node2"); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -650,9 +575,10 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_timer) { TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) { auto node1 = std::make_shared("waitable_node", "ns"); auto node2 = std::make_shared("waitable_node2", "ns"); - rclcpp::Waitable::SharedPtr waitable = std::make_shared(); - node1->get_node_waitables_interface()->add_waitable(waitable, nullptr); - node2->get_node_waitables_interface()->add_waitable(waitable, nullptr); + rclcpp::Waitable::SharedPtr waitable1 = std::make_shared(); + rclcpp::Waitable::SharedPtr waitable2 = std::make_shared(); + node1->get_node_waitables_interface()->add_waitable(waitable1, nullptr); + node2->get_node_waitables_interface()->add_waitable(waitable2, nullptr); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -664,7 +590,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable) { } TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) { - auto node = create_node_with_subscription("node", true); + auto node = create_node_with_subscription("node"); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -676,7 +602,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_mutually_exclusive) { } TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) { - auto node = create_node_with_service("node", true); + auto node = create_node_with_service("node"); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -688,7 +614,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_service_mutually_exclusive) { } TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) { - auto node = create_node_with_client("node", true); + auto node = create_node_with_client("node"); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -700,7 +626,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client_mutually_exclusive) { } TEST_F(TestAllocatorMemoryStrategy, get_next_timer_mutually_exclusive) { - auto node = create_node_with_timer("node", true); + auto node = create_node_with_timer("node"); auto get_next_entity = [this](const WeakNodeList & nodes) { rclcpp::AnyExecutable result; @@ -733,50 +659,62 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_mutually_exclusive) { TEST_F(TestAllocatorMemoryStrategy, get_next_subscription_out_of_scope) { WeakNodeList nodes; - auto node = std::make_shared("node", "ns"); + auto node = create_node_with_disabled_callback_groups("node"); nodes.push_back(node->get_node_base_interface()); // Force subscription to go out of scope and cleanup after collecting entities. { + rclcpp::SubscriptionOptions subscription_options; + + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + subscription_options.callback_group = callback_group; + auto subscription_callback = [](const test_msgs::msg::Empty::SharedPtr) {}; const rclcpp::QoS qos(10); auto subscription = node->create_subscription< test_msgs::msg::Empty, decltype(subscription_callback)>( - "topic", qos, std::move(subscription_callback)); + "topic", qos, std::move(subscription_callback), subscription_options); allocator_memory_strategy()->collect_entities(nodes); } - EXPECT_EQ(1u, number_of_ready_subscriptions_in_addition_to_basic_node()); + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_subscriptions()); rclcpp::AnyExecutable result; allocator_memory_strategy()->get_next_subscription(result, nodes); - EXPECT_EQ(node->get_node_base_interface(), result.node_base); + EXPECT_EQ(nullptr, result.node_base); } TEST_F(TestAllocatorMemoryStrategy, get_next_service_out_of_scope) { WeakNodeList nodes; - auto node = std::make_shared("node", "ns"); + auto node = create_node_with_disabled_callback_groups("node"); nodes.push_back(node->get_node_base_interface()); // Force service to go out of scope and cleanup after collecting entities. { + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto service_callback = [](const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto service = node->create_service( - "service", std::move(service_callback), rmw_qos_profile_services_default, nullptr); + "service", std::move(service_callback), rmw_qos_profile_services_default, callback_group); allocator_memory_strategy()->collect_entities(nodes); } - EXPECT_EQ(1u, number_of_ready_services_in_addition_to_basic_node()); + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_services()); rclcpp::AnyExecutable result; allocator_memory_strategy()->get_next_service(result, nodes); - EXPECT_EQ(node->get_node_base_interface(), result.node_base); + EXPECT_EQ(nullptr, result.node_base); } TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { WeakNodeList nodes; - auto node = std::make_shared("node", "ns"); + auto node = create_node_with_disabled_callback_groups("node"); nodes.push_back(node->get_node_base_interface()); // Force client to go out of scope and cleanup after collecting entities. { @@ -788,7 +726,7 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { allocator_memory_strategy()->collect_entities(nodes); } - EXPECT_EQ(1u, number_of_ready_clients_in_addition_to_basic_node()); + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_clients()); rclcpp::AnyExecutable result; allocator_memory_strategy()->get_next_client(result, nodes); @@ -797,17 +735,21 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_client_out_of_scope) { TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) { WeakNodeList nodes; - auto node = std::make_shared("node", "ns"); + auto node = create_node_with_disabled_callback_groups("node"); nodes.push_back(node->get_node_base_interface()); // Force timer to go out of scope and cleanup after collecting entities. { + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto timer = node->create_wall_timer( - std::chrono::seconds(10), []() {}); + std::chrono::seconds(10), []() {}, callback_group); allocator_memory_strategy()->collect_entities(nodes); } - EXPECT_EQ(1u, number_of_ready_timers_in_addition_to_basic_node()); + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_ready_timers()); rclcpp::AnyExecutable result; allocator_memory_strategy()->get_next_timer(result, nodes); @@ -816,16 +758,21 @@ TEST_F(TestAllocatorMemoryStrategy, get_next_timer_out_of_scope) { TEST_F(TestAllocatorMemoryStrategy, get_next_waitable_out_of_scope) { WeakNodeList nodes; - auto node = std::make_shared("node", "ns"); + auto node = create_node_with_disabled_callback_groups("node"); nodes.push_back(node->get_node_base_interface()); // Force waitable to go out of scope and cleanup after collecting entities. { + auto callback_group = + node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); allocator_memory_strategy()->collect_entities(nodes); auto waitable = std::make_shared(); + node->get_node_waitables_interface()->add_waitable(waitable, callback_group); allocator_memory_strategy()->add_waitable_handle(waitable); } - EXPECT_EQ(1u, number_of_waitables_in_addition_to_basic_node()); + // Since all callback groups have been locked, except the one we added, this should only be 1 + EXPECT_EQ(1u, allocator_memory_strategy()->number_of_waitables()); rclcpp::AnyExecutable result; allocator_memory_strategy()->get_next_waitable(result, nodes); From 8272a8c736004c4ae45445130c5b82a48a855230 Mon Sep 17 00:00:00 2001 From: tomoya Date: Thu, 6 Aug 2020 01:23:06 +0900 Subject: [PATCH 19/61] Ability to configure domain_id via InitOptions. (#1165) Signed-off-by: Tomoya.Fujita Remove non-foxy api changes Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 5 +++ rclcpp/test/rclcpp/test_init_options.cpp | 46 ++++++++++++++++++++++++ 2 files changed, 51 insertions(+) create mode 100644 rclcpp/test/rclcpp/test_init_options.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 840c5de0e3..e9e70f8063 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -253,6 +253,11 @@ if(TARGET test_node_options) ament_target_dependencies(test_node_options "rcl") target_link_libraries(test_node_options ${PROJECT_NAME}) endif() +ament_add_gtest(test_init_options rclcpp/test_init_options.cpp) +if(TARGET test_init_options) + ament_target_dependencies(test_init_options "rcl") + target_link_libraries(test_init_options ${PROJECT_NAME}) +endif() ament_add_gtest(test_parameter_client rclcpp/test_parameter_client.cpp) if(TARGET test_parameter_client) ament_target_dependencies(test_parameter_client diff --git a/rclcpp/test/rclcpp/test_init_options.cpp b/rclcpp/test/rclcpp/test_init_options.cpp new file mode 100644 index 0000000000..ecbe1e72c8 --- /dev/null +++ b/rclcpp/test/rclcpp/test_init_options.cpp @@ -0,0 +1,46 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/domain_id.h" + +#include "rclcpp/init_options.hpp" + + +TEST(TestInitOptions, test_construction) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + auto options = rclcpp::InitOptions(allocator); + const rcl_init_options_t * rcl_options = options.get_rcl_init_options(); + ASSERT_TRUE(rcl_options != nullptr); + ASSERT_TRUE(rcl_options->impl != nullptr); + + { + auto options_copy = rclcpp::InitOptions(options); + const rcl_init_options_t * rcl_options_copy = options_copy.get_rcl_init_options(); + ASSERT_TRUE(rcl_options_copy != nullptr); + ASSERT_TRUE(rcl_options_copy->impl != nullptr); + } + + { + auto options_copy = options; + const rcl_init_options_t * rcl_options_copy = options_copy.get_rcl_init_options(); + ASSERT_TRUE(rcl_options_copy != nullptr); + ASSERT_TRUE(rcl_options_copy->impl != nullptr); + } +} From ae4bb5cb080f4efaecd0b37a3fc64ea3236f936d Mon Sep 17 00:00:00 2001 From: tomoya Date: Wed, 12 Aug 2020 00:22:33 +0900 Subject: [PATCH 20/61] initialize_logging_ should be copied. (#1272) Signed-off-by: Tomoya.Fujita --- rclcpp/src/rclcpp/init_options.cpp | 2 ++ rclcpp/test/rclcpp/test_init_options.cpp | 17 +++++++++++++++++ 2 files changed, 19 insertions(+) diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp index ad2a900b5f..f9fd6f5f60 100644 --- a/rclcpp/src/rclcpp/init_options.cpp +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -44,6 +44,7 @@ InitOptions::InitOptions(const InitOptions & other) : InitOptions(*other.get_rcl_init_options()) { shutdown_on_sigint = other.shutdown_on_sigint; + initialize_logging_ = other.initialize_logging_; } bool @@ -69,6 +70,7 @@ InitOptions::operator=(const InitOptions & other) rclcpp::exceptions::throw_from_rcl_error(ret, "failed to copy rcl init options"); } this->shutdown_on_sigint = other.shutdown_on_sigint; + this->initialize_logging_ = other.initialize_logging_; } return *this; } diff --git a/rclcpp/test/rclcpp/test_init_options.cpp b/rclcpp/test/rclcpp/test_init_options.cpp index ecbe1e72c8..3c0cae739a 100644 --- a/rclcpp/test/rclcpp/test_init_options.cpp +++ b/rclcpp/test/rclcpp/test_init_options.cpp @@ -44,3 +44,20 @@ TEST(TestInitOptions, test_construction) { ASSERT_TRUE(rcl_options_copy->impl != nullptr); } } + +TEST(TestInitOptions, test_initialize_logging) { + { + auto options = rclcpp::InitOptions(); + EXPECT_TRUE(options.auto_initialize_logging()); + } + + { + auto options = rclcpp::InitOptions().auto_initialize_logging(true); + EXPECT_TRUE(options.auto_initialize_logging()); + } + + { + auto options = rclcpp::InitOptions().auto_initialize_logging(false); + EXPECT_FALSE(options.auto_initialize_logging()); + } +} From 33575edabe9053b011395023e5cb4469695d112d Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 11 Aug 2020 09:55:32 -0700 Subject: [PATCH 21/61] Fixes for unit tests that fail under cyclonedds (#1270) Addresses #1268 and #1269 Signed-off-by: Stephen Brawner --- .../test/rclcpp/executors/test_executors.cpp | 50 ++++++++++++------- .../test_allocator_memory_strategy.cpp | 5 +- 2 files changed, 35 insertions(+), 20 deletions(-) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 035c154586..5474eb3f6c 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -41,28 +41,38 @@ template class TestExecutors : public ::testing::Test { public: - void SetUp() + static void SetUpTestCase() { rclcpp::init(0, nullptr); - node = std::make_shared("node", "ns"); + } - callback_count = 0; - std::stringstream topic_name; + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { const auto test_info = ::testing::UnitTest::GetInstance()->current_test_info(); - topic_name << "topic_" << test_info->test_case_name() << "_" << test_info->name(); + std::stringstream test_name; + test_name << test_info->test_case_name() << "_" << test_info->name(); + node = std::make_shared("node", test_name.str()); - publisher = node->create_publisher(topic_name.str(), rclcpp::QoS(10)); + callback_count = 0; + + const std::string topic_name = std::string("topic_") + test_name.str(); + publisher = node->create_publisher(topic_name, rclcpp::QoS(10)); auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; subscription = node->create_subscription( - topic_name.str(), rclcpp::QoS(10), std::move(callback)); + topic_name, rclcpp::QoS(10), std::move(callback)); } void TearDown() { - if (rclcpp::ok()) { - rclcpp::shutdown(); - } + publisher.reset(); + subscription.reset(); + node.reset(); } rclcpp::Node::SharedPtr node; @@ -147,7 +157,7 @@ TYPED_TEST(TestExecutorsStable, addTemporaryNode) { std::thread spinner([&]() {EXPECT_NO_THROW(executor.spin());}); std::this_thread::sleep_for(50ms); - rclcpp::shutdown(); + executor.cancel(); spinner.join(); } @@ -158,6 +168,7 @@ TYPED_TEST(TestExecutors, addNodeTwoExecutors) { ExecutorType executor2; EXPECT_NO_THROW(executor1.add_node(this->node)); EXPECT_THROW(executor2.add_node(this->node), std::runtime_error); + executor1.remove_node(this->node, true); } // Check simple spin example @@ -172,15 +183,15 @@ TYPED_TEST(TestExecutors, spinWithTimer) { std::thread spinner([&]() {executor.spin();}); auto start = std::chrono::steady_clock::now(); - while (!timer_completed && (std::chrono::steady_clock::now() - start) < 1s) { + while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) { std::this_thread::sleep_for(1ms); } EXPECT_TRUE(timer_completed); - - // Shutdown needs to be called before join, so that executor.spin() returns. - rclcpp::shutdown(); + // Cancel needs to be called before join, so that executor.spin() returns. + executor.cancel(); spinner.join(); + executor.remove_node(this->node, true); } TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { @@ -195,7 +206,7 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { // Sleep for a short time to verify executor.spin() is going, and didn't throw. auto start = std::chrono::steady_clock::now(); - while (!timer_completed && (std::chrono::steady_clock::now() - start) < 1s) { + while (!timer_completed && (std::chrono::steady_clock::now() - start) < 10s) { std::this_thread::sleep_for(1ms); } @@ -203,8 +214,9 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning) { EXPECT_THROW(executor.spin(), std::runtime_error); // Shutdown needs to be called before join, so that executor.spin() returns. - rclcpp::shutdown(); + executor.cancel(); spinner.join(); + executor.remove_node(this->node, true); } // Check executor exits immediately if future is complete. @@ -223,7 +235,7 @@ TYPED_TEST(TestExecutors, testSpinUntilFutureComplete) { auto start = std::chrono::steady_clock::now(); auto shared_future = future.share(); auto ret = executor.spin_until_future_complete(shared_future, 1s); - + executor.remove_node(this->node, true); // Check it didn't reach timeout EXPECT_GT(500ms, (std::chrono::steady_clock::now() - start)); EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); @@ -299,7 +311,7 @@ TYPED_TEST(TestExecutorsStable, spinSome) { bool spin_exited = false; std::thread spinner([&spin_exited, &executor, this]() { executor.spin_some(1s); - executor.remove_node(this->node); + executor.remove_node(this->node, true); spin_exited = true; }); diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 4c2a9c3527..70470ec17a 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -413,7 +413,10 @@ TEST_F(TestAllocatorMemoryStrategy, add_remove_waitables) { TEST_F(TestAllocatorMemoryStrategy, number_of_entities_with_subscription) { RclWaitSetSizes expected_sizes = {}; expected_sizes.size_of_subscriptions = 1; - if (std::string("rmw_connext_cpp") == rmw_get_implementation_identifier()) { + const std::string implementation_identifier = rmw_get_implementation_identifier(); + if (implementation_identifier == "rmw_connext_cpp" || + implementation_identifier == "rmw_cyclonedds_cpp") + { // For connext, a subscription will also add an event and waitable expected_sizes.size_of_events += 1; expected_sizes.size_of_waitables += 1; From a1ceffb7e92f25c938087c419bacb630cfb6460f Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Wed, 12 Aug 2020 21:30:01 -0700 Subject: [PATCH 22/61] fix topic stats test, wait for more messages, only check the ones with samples (#1274) Signed-off-by: Dirk Thomas --- .../test_subscription_topic_statistics.cpp | 48 ++++++++++++++++--- 1 file changed, 42 insertions(+), 6 deletions(-) diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index dc4e1fa86e..de3bd00b99 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -284,7 +284,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no auto statistics_listener = std::make_shared( "test_receive_single_empty_stats_message_listener", "/statistics", - 2); + 4); auto empty_subscriber = std::make_shared( kTestSubNodeName, @@ -301,12 +301,12 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no kTestDuration); // Compare message counts, sample count should be the same as published and received count - EXPECT_EQ(2, statistics_listener->GetNumberOfMessagesReceived()); + EXPECT_EQ(4, statistics_listener->GetNumberOfMessagesReceived()); // Check the received message and the data types const auto received_messages = statistics_listener->GetReceivedMessages(); - EXPECT_EQ(2u, received_messages.size()); + EXPECT_EQ(4u, received_messages.size()); std::set received_metrics; for (const auto & msg : received_messages) { @@ -318,10 +318,27 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no // Check the collected statistics for message period. // Message age statistics will not be calculated because Empty messages // don't have a `header` with timestamp. + bool any_samples = false; for (const auto & msg : received_messages) { if (msg.metrics_source != "message_period") { continue; } + // skip messages without samples + bool has_samples = false; + for (const auto & stats_point : msg.statistics) { + const auto type = stats_point.data_type; + if ( + StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT == type && + stats_point.data > 0) + { + has_samples = true; + break; + } + } + if (!has_samples) { + continue; + } + any_samples = true; for (const auto & stats_point : msg.statistics) { const auto type = stats_point.data_type; switch (type) { @@ -346,6 +363,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no } } } + EXPECT_TRUE(any_samples) << "All received metrics messages had zero samples"; } TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) @@ -361,7 +379,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi auto statistics_listener = std::make_shared( "test_receive_stats_for_message_with_header", "/statistics", - 2); + 4); auto msg_with_header_subscriber = std::make_shared( kTestSubNodeName, @@ -378,12 +396,12 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi kTestDuration); // Compare message counts, sample count should be the same as published and received count - EXPECT_EQ(2, statistics_listener->GetNumberOfMessagesReceived()); + EXPECT_EQ(4, statistics_listener->GetNumberOfMessagesReceived()); // Check the received message and the data types const auto received_messages = statistics_listener->GetReceivedMessages(); - EXPECT_EQ(2u, received_messages.size()); + EXPECT_EQ(4u, received_messages.size()); std::set received_metrics; for (const auto & msg : received_messages) { @@ -393,7 +411,24 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end()); // Check the collected statistics for message period. + bool any_samples = false; for (const auto & msg : received_messages) { + // skip messages without samples + bool has_samples = false; + for (const auto & stats_point : msg.statistics) { + const auto type = stats_point.data_type; + if ( + StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT == type && + stats_point.data > 0) + { + has_samples = true; + break; + } + } + if (!has_samples) { + continue; + } + any_samples = true; for (const auto & stats_point : msg.statistics) { const auto type = stats_point.data_type; switch (type) { @@ -418,4 +453,5 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi } } } + EXPECT_TRUE(any_samples) << "All received metrics messages had zero samples"; } From dca9f6cba89141e839cda2b643cdaa5932b1a583 Mon Sep 17 00:00:00 2001 From: Devin Bonnie <47613035+dabonnie@users.noreply.github.com> Date: Thu, 20 Aug 2020 13:10:35 -0700 Subject: [PATCH 23/61] Refactor Subscription Topic Statistics Tests (#1281) * Add check for the correct number of messages received Signed-off-by: Devin Bonnie * Refactor duplicate code into functions Add random jitter to generate non-zero standard deviation values Signed-off-by: Devin Bonnie * Fix warning Signed-off-by: Devin Bonnie * Fix conversion warnings Signed-off-by: Devin Bonnie * Fix style issues Signed-off-by: Devin Bonnie --- .../test_subscription_topic_statistics.cpp | 225 ++++++++++-------- .../test_topic_stats_utils.hpp | 8 +- 2 files changed, 127 insertions(+), 106 deletions(-) diff --git a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index de3bd00b99..eac1ed9b95 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -49,8 +50,13 @@ constexpr const char kTestSubNodeName[]{"test_sub_stats_node"}; constexpr const char kTestSubStatsTopic[]{"/test_sub_stats_topic"}; constexpr const char kTestSubStatsEmptyTopic[]{"/test_sub_stats_empty_topic"}; constexpr const char kTestTopicStatisticsTopic[]{"/test_topic_statistics_topic"}; +constexpr const char kMessageAgeSourceLabel[]{"message_age"}; +constexpr const char kMessagePeriodSourceLabel[]{"message_period"}; constexpr const uint64_t kNoSamples{0}; constexpr const std::chrono::seconds kTestDuration{10}; +constexpr const uint64_t kNumExpectedMessages{8}; +constexpr const uint64_t kNumExpectedMessageAgeMessages{kNumExpectedMessages / 2}; +constexpr const uint64_t kNumExpectedMessagePeriodMessages{kNumExpectedMessages / 2}; } // namespace using rclcpp::msg::MessageWithHeader; @@ -61,6 +67,10 @@ using statistics_msgs::msg::StatisticDataPoint; using statistics_msgs::msg::StatisticDataType; using libstatistics_collector::moving_average_statistics::StatisticData; +/** + * Wrapper class to test and expose parts of the SubscriptionTopicStatistics class. + * \tparam CallbackMessageT + */ template class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics { @@ -128,6 +138,7 @@ class MessageWithHeaderPublisher : public rclcpp::Node publish_period, [this]() { this->publish_message(); }); + uniform_dist_ = std::uniform_int_distribution{1000000, 100000000}; } virtual ~MessageWithHeaderPublisher() = default; @@ -135,14 +146,19 @@ class MessageWithHeaderPublisher : public rclcpp::Node private: void publish_message() { + std::random_device rd; + std::mt19937 gen{rd()}; + uint32_t d = uniform_dist_(gen); auto msg = MessageWithHeader{}; - // Subtract 1 sec from current time so the received message age is always > 0 - msg.header.stamp = this->now() - rclcpp::Duration{1, 0}; + // Subtract ~1 second (add some noise for a non-zero standard deviation) + // so the received message age calculation is always > 0 + msg.header.stamp = this->now() - rclcpp::Duration{1, d}; publisher_->publish(msg); } rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr publish_timer_; + std::uniform_int_distribution uniform_dist_; }; /** @@ -220,6 +236,66 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test } }; +/** + * Check if a received statistics message is empty (no data was observed) + * \param message_to_check + */ +void check_if_statistics_message_is_empty(const MetricsMessage & message_to_check) +{ + for (const auto & stats_point : message_to_check.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: + EXPECT_EQ(0, stats_point.data) << "unexpected sample count" << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_TRUE(std::isnan(stats_point.data)) << "unexpected value" << stats_point.data << + " for type:" << type; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } + } +} + +/** + * Check if a received statistics message observed data and contains some calculation + * \param message_to_check + */ +void check_if_statistic_message_is_populated(const MetricsMessage & message_to_check) +{ + for (const auto & stats_point : message_to_check.statistics) { + const auto type = stats_point.data_type; + switch (type) { + case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: + EXPECT_LT(0, stats_point.data) << "unexpected sample count " << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: + EXPECT_LT(0, stats_point.data) << "unexpected avg " << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected mi n" << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: + EXPECT_LT(0, stats_point.data) << "unexpected max " << stats_point.data; + break; + case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: + EXPECT_LT(0, stats_point.data) << "unexpected stddev " << stats_point.data; + break; + default: + FAIL() << "received unknown statistics type: " << std::dec << + static_cast(type); + } + } +} + +/** + * Test an invalid argument is thrown for a bad input publish period. + */ TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) { rclcpp::init(0 /* argc */, nullptr /* argv */); @@ -244,6 +320,10 @@ TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period) rclcpp::shutdown(); } +/** + * Test that we can manually construct the subscription topic statistics utility class + * without any errors and defaults to empty measurements. + */ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) { auto empty_subscriber = std::make_shared( @@ -271,6 +351,10 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction) } } +/** + * Publish messages that do not have a header timestamp, test that all statistics messages + * were received, and verify the statistics message contents. + */ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no_header) { // Create an empty publisher @@ -284,7 +368,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no auto statistics_listener = std::make_shared( "test_receive_single_empty_stats_message_listener", "/statistics", - 4); + kNumExpectedMessages); auto empty_subscriber = std::make_shared( kTestSubNodeName, @@ -301,69 +385,39 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_no kTestDuration); // Compare message counts, sample count should be the same as published and received count - EXPECT_EQ(4, statistics_listener->GetNumberOfMessagesReceived()); + EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived()); - // Check the received message and the data types + // Check the received message total count const auto received_messages = statistics_listener->GetReceivedMessages(); + EXPECT_EQ(kNumExpectedMessages, received_messages.size()); - EXPECT_EQ(4u, received_messages.size()); + // check the type of statistics that were received and their counts + uint64_t message_age_count{0}; + uint64_t message_period_count{0}; std::set received_metrics; for (const auto & msg : received_messages) { - received_metrics.insert(msg.metrics_source); + if (msg.metrics_source == "message_age") { + message_age_count++; + } + if (msg.metrics_source == "message_period") { + message_period_count++; + } } - EXPECT_TRUE(received_metrics.find("message_age") != received_metrics.end()); - EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end()); + EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count); + EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count); // Check the collected statistics for message period. // Message age statistics will not be calculated because Empty messages - // don't have a `header` with timestamp. - bool any_samples = false; + // don't have a `header` with timestamp. This means that we expect to receive a `message_age` + // and `message_period` message for each empty message published. for (const auto & msg : received_messages) { - if (msg.metrics_source != "message_period") { - continue; - } - // skip messages without samples - bool has_samples = false; - for (const auto & stats_point : msg.statistics) { - const auto type = stats_point.data_type; - if ( - StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT == type && - stats_point.data > 0) - { - has_samples = true; - break; - } - } - if (!has_samples) { - continue; - } - any_samples = true; - for (const auto & stats_point : msg.statistics) { - const auto type = stats_point.data_type; - switch (type) { - case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: - EXPECT_LT(0, stats_point.data) << "unexpected sample count"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: - EXPECT_LT(0, stats_point.data) << "unexpected avg"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: - EXPECT_LT(0, stats_point.data) << "unexpected min"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: - EXPECT_LT(0, stats_point.data) << "unexpected max"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: - EXPECT_LT(0, stats_point.data) << "unexpected stddev"; - break; - default: - FAIL() << "received unknown statistics type: " << std::dec << - static_cast(type); - } + if (msg.metrics_source == kMessageAgeSourceLabel) { + check_if_statistics_message_is_empty(msg); + } else if (msg.metrics_source == kMessagePeriodSourceLabel) { + check_if_statistic_message_is_populated(msg); } } - EXPECT_TRUE(any_samples) << "All received metrics messages had zero samples"; } TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_with_header) @@ -379,7 +433,7 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi auto statistics_listener = std::make_shared( "test_receive_stats_for_message_with_header", "/statistics", - 4); + kNumExpectedMessages); auto msg_with_header_subscriber = std::make_shared( kTestSubNodeName, @@ -396,62 +450,29 @@ TEST_F(TestSubscriptionTopicStatisticsFixture, test_receive_stats_for_message_wi kTestDuration); // Compare message counts, sample count should be the same as published and received count - EXPECT_EQ(4, statistics_listener->GetNumberOfMessagesReceived()); + EXPECT_EQ(kNumExpectedMessages, statistics_listener->GetNumberOfMessagesReceived()); - // Check the received message and the data types + // Check the received message total count const auto received_messages = statistics_listener->GetReceivedMessages(); + EXPECT_EQ(kNumExpectedMessages, received_messages.size()); - EXPECT_EQ(4u, received_messages.size()); + // check the type of statistics that were received and their counts + uint64_t message_age_count{0}; + uint64_t message_period_count{0}; std::set received_metrics; for (const auto & msg : received_messages) { - received_metrics.insert(msg.metrics_source); + if (msg.metrics_source == kMessageAgeSourceLabel) { + message_age_count++; + } + if (msg.metrics_source == kMessagePeriodSourceLabel) { + message_period_count++; + } } - EXPECT_TRUE(received_metrics.find("message_age") != received_metrics.end()); - EXPECT_TRUE(received_metrics.find("message_period") != received_metrics.end()); + EXPECT_EQ(kNumExpectedMessageAgeMessages, message_age_count); + EXPECT_EQ(kNumExpectedMessagePeriodMessages, message_period_count); - // Check the collected statistics for message period. - bool any_samples = false; for (const auto & msg : received_messages) { - // skip messages without samples - bool has_samples = false; - for (const auto & stats_point : msg.statistics) { - const auto type = stats_point.data_type; - if ( - StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT == type && - stats_point.data > 0) - { - has_samples = true; - break; - } - } - if (!has_samples) { - continue; - } - any_samples = true; - for (const auto & stats_point : msg.statistics) { - const auto type = stats_point.data_type; - switch (type) { - case StatisticDataType::STATISTICS_DATA_TYPE_SAMPLE_COUNT: - EXPECT_LT(0, stats_point.data) << "unexpected sample count"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_AVERAGE: - EXPECT_LT(0, stats_point.data) << "unexpected avg"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_MINIMUM: - EXPECT_LT(0, stats_point.data) << "unexpected min"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_MAXIMUM: - EXPECT_LT(0, stats_point.data) << "unexpected max"; - break; - case StatisticDataType::STATISTICS_DATA_TYPE_STDDEV: - EXPECT_LE(0, stats_point.data) << "unexpected stddev"; - break; - default: - FAIL() << "received unknown statistics type: " << std::dec << - static_cast(type); - } - } + check_if_statistic_message_is_populated(msg); } - EXPECT_TRUE(any_samples) << "All received metrics messages had zero samples"; } diff --git a/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp b/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp index ec50d16eaf..12c8514586 100644 --- a/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp +++ b/rclcpp/test/rclcpp/topic_statistics/test_topic_stats_utils.hpp @@ -90,7 +90,7 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter MetricsMessageSubscriber( const std::string & name, const std::string & topic_name, - const int number_of_messages_to_receive = 2) + const uint64_t number_of_messages_to_receive = 2) : rclcpp::Node(name), number_of_messages_to_receive_(number_of_messages_to_receive) { @@ -118,7 +118,7 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter * Return the number of messages received by this subscriber. * \return the number of messages received by the subscriber callback */ - int GetNumberOfMessagesReceived() const + uint64_t GetNumberOfMessagesReceived() const { return num_messages_received_; } @@ -142,8 +142,8 @@ class MetricsMessageSubscriber : public rclcpp::Node, public PromiseSetter std::vector received_messages_; rclcpp::Subscription::SharedPtr subscription_; mutable std::mutex mutex_; - std::atomic num_messages_received_{0}; - const int number_of_messages_to_receive_; + std::atomic num_messages_received_{0}; + const uint64_t number_of_messages_to_receive_; }; } // namespace topic_statistics From 31c4273c70d712aecc431ea49c4418dfaa812aff Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Wed, 9 Sep 2020 18:28:42 -0300 Subject: [PATCH 24/61] Adding tests basic getters (#1291) * Add tests serialize functions * Add test getter const get_service_handle * Add basic tests getters publisher * Add == operator tests * Improve check on QOS depth * Remove extra line, copy directly string * Expect specific error throws Signed-off-by: Jorge Perez --- rclcpp/test/CMakeLists.txt | 1 + rclcpp/test/rclcpp/test_publisher.cpp | 56 +++++++++++ .../test/rclcpp/test_serialized_message.cpp | 93 +++++++++++++++++++ rclcpp/test/rclcpp/test_service.cpp | 38 ++++++++ 4 files changed, 188 insertions(+) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index e9e70f8063..1b47912aca 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -366,6 +366,7 @@ if(TARGET test_service) "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" + "test_msgs" ) target_link_libraries(test_service ${PROJECT_NAME}) endif() diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index dce0b8caa8..0ae98cc74f 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -203,3 +203,59 @@ TEST_F(TestPublisherSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidTopicNameError); } } + +// Auxiliary class used to test getter for const PublisherBase +const rosidl_message_type_support_t EmptyTypeSupport() +{ + return *rosidl_typesupport_cpp::get_message_type_support_handle(); +} + +const rcl_publisher_options_t PublisherOptions() +{ + return rclcpp::PublisherOptionsWithAllocator>().template + to_rcl_publisher_options(rclcpp::QoS(10)); +} + +class TestPublisherBase : public rclcpp::PublisherBase +{ +public: + explicit TestPublisherBase(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} +}; + +/* + Testing some publisher getters + */ +TEST_F(TestPublisher, basic_getters) { + initialize(); + using test_msgs::msg::Empty; + { + using rclcpp::QoS; + using rclcpp::KeepLast; + const size_t qos_depth_size = 10u; + auto publisher = node->create_publisher("topic", QoS(KeepLast(qos_depth_size))); + + size_t publisher_queue_size = publisher->get_queue_size(); + EXPECT_EQ(qos_depth_size, publisher_queue_size); + + const rmw_gid_t & publisher_rmw_gid = publisher->get_gid(); + EXPECT_NE(nullptr, publisher_rmw_gid.implementation_identifier); + + std::shared_ptr publisher_handle = publisher->get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + + EXPECT_TRUE(publisher->assert_liveliness()); + } + { + const TestPublisherBase publisher = TestPublisherBase(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + + const rmw_gid_t & publisher_rmw_gid = publisher.get_gid(); + EXPECT_NE(nullptr, publisher_rmw_gid.implementation_identifier); + + // Test == operator of publisher with rmw_gid_t + EXPECT_EQ(publisher, publisher_rmw_gid); + } +} diff --git a/rclcpp/test/rclcpp/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp index 4e28106a88..e9f201fc0c 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -140,6 +140,20 @@ TEST(TestSerializedMessage, release) { EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&released_handle)); } +TEST(TestSerializedMessage, reserve) { + rclcpp::SerializedMessage serialized_msg(13); + EXPECT_EQ(13u, serialized_msg.capacity()); + + // Resize using reserve method + serialized_msg.reserve(15); + EXPECT_EQ(15u, serialized_msg.capacity()); + + // Use invalid value throws exception + EXPECT_THROW( + {serialized_msg.reserve(-1);}, + rclcpp::exceptions::RCLBadAlloc); +} + TEST(TestSerializedMessage, serialization) { using MessageT = test_msgs::msg::BasicTypes; @@ -159,6 +173,85 @@ TEST(TestSerializedMessage, serialization) { } } +TEST(TestSerializedMessage, assignment_operators) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator + auto default_allocator = rcl_get_default_allocator(); + auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator); + ASSERT_EQ(RCL_RET_OK, ret); + + // manually copy some content + std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content_size); + rcl_serialized_msg.buffer_length = content_size; + EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); + rclcpp::SerializedMessage serialized_message_to_assign(rcl_serialized_msg); + EXPECT_EQ(13u, serialized_message_to_assign.capacity()); + EXPECT_EQ(content_size, serialized_message_to_assign.size()); + + // Test copy assignment with = operator, on another rclcpp::SerializedMessage + rclcpp::SerializedMessage serialized_msg_copy(2); + EXPECT_EQ(2u, serialized_msg_copy.capacity()); + EXPECT_EQ(0u, serialized_msg_copy.size()); + serialized_msg_copy = serialized_message_to_assign; + EXPECT_EQ(13u, serialized_msg_copy.capacity()); + EXPECT_EQ(content_size, serialized_msg_copy.size()); + + // Test copy assignment with = operator, with a rcl_serialized_message_t + rclcpp::SerializedMessage serialized_msg_copy_rcl(2); + EXPECT_EQ(2u, serialized_msg_copy_rcl.capacity()); + EXPECT_EQ(0u, serialized_msg_copy_rcl.size()); + serialized_msg_copy_rcl = rcl_serialized_msg; + EXPECT_EQ(13u, serialized_msg_copy_rcl.capacity()); + EXPECT_EQ(content_size, serialized_msg_copy_rcl.size()); + + // Test move assignment with = operator + rclcpp::SerializedMessage serialized_msg_move(2); + EXPECT_EQ(2u, serialized_msg_move.capacity()); + EXPECT_EQ(0u, serialized_msg_move.size()); + serialized_msg_move = std::move(serialized_message_to_assign); + EXPECT_EQ(13u, serialized_msg_move.capacity()); + EXPECT_EQ(content_size, serialized_msg_move.size()); + EXPECT_EQ(0u, serialized_message_to_assign.capacity()); + EXPECT_EQ(0u, serialized_message_to_assign.size()); + + // Test move assignment with = operator, with a rcl_serialized_message_t + rclcpp::SerializedMessage serialized_msg_move_rcl(2); + EXPECT_EQ(2u, serialized_msg_move_rcl.capacity()); + EXPECT_EQ(0u, serialized_msg_move_rcl.size()); + serialized_msg_move_rcl = std::move(rcl_serialized_msg); + EXPECT_EQ(13u, serialized_msg_move_rcl.capacity()); + EXPECT_EQ(content_size, serialized_msg_move_rcl.size()); + EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity); + + // Error because it was moved + EXPECT_EQ(RCUTILS_RET_INVALID_ARGUMENT, rmw_serialized_message_fini(&rcl_serialized_msg)); +} + +TEST(TestSerializedMessage, failed_init_throws) { + rclcpp::SerializedMessage serialized_msg(13); + EXPECT_EQ(13u, serialized_msg.capacity()); + + // Constructor with invalid size throws exception + EXPECT_THROW( + {rclcpp::SerializedMessage serialized_msg_fail(-1);}, + rclcpp::exceptions::RCLBadAlloc); + + // Constructor copy with rmw_serialized bad msg throws + auto default_allocator = rcl_get_default_allocator(); + auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator); + ASSERT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); + rcl_serialized_msg.buffer_capacity = -1; + EXPECT_THROW( + {rclcpp::SerializedMessage serialized_msg_fail_2(rcl_serialized_msg);}, + rclcpp::exceptions::RCLBadAlloc); + + rcl_serialized_msg.buffer_capacity = 13; + EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&rcl_serialized_msg)); +} + void serialize_default_ros_msg() { using MessageT = test_msgs::msg::BasicTypes; diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 9ab90e1208..8043178b7e 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -21,6 +21,8 @@ #include "rclcpp/rclcpp.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/empty.h" class TestService : public ::testing::Test { @@ -105,3 +107,39 @@ TEST_F(TestServiceSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidServiceNameError); } } + +/* Testing basic getters */ +TEST_F(TestService, basic_public_getters) { + using rcl_interfaces::srv::ListParameters; + auto callback = + [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { + }; + auto service = node->create_service("service", callback); + EXPECT_STREQ(service->get_service_name(), "/ns/service"); + std::shared_ptr service_handle = service->get_service_handle(); + EXPECT_NE(nullptr, service_handle); + + { + // Create a extern defined const service + auto node_handle_int = rclcpp::Node::make_shared("base_node"); + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + const rosidl_service_type_support_t * ts = + rosidl_typesupport_cpp::get_service_type_support_handle(); + rcl_ret_t ret = rcl_service_init( + &service_handle, + node_handle_int->get_node_base_interface()->get_rcl_node_handle(), + ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + rclcpp::AnyServiceCallback cb; + const rclcpp::Service base( + node_handle_int->get_node_base_interface()->get_shared_rcl_node_handle(), + &service_handle, cb); + // Use get_service_handle specific to const service + std::shared_ptr const_service_handle = base.get_service_handle(); + EXPECT_NE(nullptr, const_service_handle); + } +} From c82c70d3a29479214e74f46b859f4755d975ad50 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Fri, 11 Sep 2020 13:42:10 -0700 Subject: [PATCH 25/61] Replace std_msgs with test_msgs in executors test (#1310) Without this change, I am unable to build locally. std_msgs is not declared as a test dependency or find_package'd anywhere, so I'm not sure why CI ever passed the build phase. Signed-off-by: Jacob Perron --- rclcpp/test/CMakeLists.txt | 3 ++- rclcpp/test/rclcpp/executors/test_executors.cpp | 14 +++++++------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 1b47912aca..1ba554fbc6 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -491,7 +491,8 @@ ament_add_gtest( TIMEOUT 180) if(TARGET test_executors) ament_target_dependencies(test_executors - "rcl") + "rcl" + "test_msgs") target_link_libraries(test_executors ${PROJECT_NAME}) endif() diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 5474eb3f6c..9128e17061 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -33,7 +33,7 @@ #include "rclcpp/duration.hpp" #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/empty.hpp" +#include "test_msgs/msg/empty.hpp" using namespace std::chrono_literals; @@ -61,10 +61,10 @@ class TestExecutors : public ::testing::Test callback_count = 0; const std::string topic_name = std::string("topic_") + test_name.str(); - publisher = node->create_publisher(topic_name, rclcpp::QoS(10)); - auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; + publisher = node->create_publisher(topic_name, rclcpp::QoS(10)); + auto callback = [this](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; subscription = - node->create_subscription( + node->create_subscription( topic_name, rclcpp::QoS(10), std::move(callback)); } @@ -76,8 +76,8 @@ class TestExecutors : public ::testing::Test } rclcpp::Node::SharedPtr node; - rclcpp::Publisher::SharedPtr publisher; - rclcpp::Subscription::SharedPtr subscription; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; int callback_count; }; @@ -323,7 +323,7 @@ TYPED_TEST(TestExecutorsStable, spinSome) { !spin_exited && (std::chrono::steady_clock::now() - start < 1s)) { - this->publisher->publish(std_msgs::msg::Empty()); + this->publisher->publish(test_msgs::msg::Empty()); std::this_thread::sleep_for(1ms); } From 0dc7b27a363bfa629841b8a72262dbc430a6b4e5 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 15 Sep 2020 10:14:16 -0300 Subject: [PATCH 26/61] Add tests type_support module (#1308) * Add tests getters msg type support * Add missing fini * Add tests type_support services * Reformat to re use test structure * Remove not needed headers * Improve teardown test cases * Add nullptr checks to type_support tests * Reformat type_support testing * Replace expect tests with asserts * "Improve error msg for rcl_service_ini/fini fail" * Improve test readability Signed-off-by: Jorge Perez --- rclcpp/test/CMakeLists.txt | 10 ++ rclcpp/test/rclcpp/test_service.cpp | 5 + rclcpp/test/rclcpp/test_type_support.cpp | 199 +++++++++++++++++++++++ 3 files changed, 214 insertions(+) create mode 100644 rclcpp/test/rclcpp/test_type_support.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 1ba554fbc6..598798f838 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -400,6 +400,16 @@ if(TARGET test_subscription_traits) ) target_link_libraries(test_subscription_traits ${PROJECT_NAME}) endif() +ament_add_gtest(test_type_support rclcpp/test_type_support.cpp) +if(TARGET test_type_support) + ament_target_dependencies(test_type_support + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_type_support ${PROJECT_NAME}) +endif() ament_add_gtest(test_find_weak_nodes rclcpp/test_find_weak_nodes.cpp) if(TARGET test_find_weak_nodes) ament_target_dependencies(test_find_weak_nodes diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 8043178b7e..7ad25e71e5 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -141,5 +141,10 @@ TEST_F(TestService, basic_public_getters) { // Use get_service_handle specific to const service std::shared_ptr const_service_handle = base.get_service_handle(); EXPECT_NE(nullptr, const_service_handle); + + EXPECT_EQ( + RCL_RET_OK, rcl_service_fini( + &service_handle, + node_handle_int->get_node_base_interface()->get_rcl_node_handle())); } } diff --git a/rclcpp/test/rclcpp/test_type_support.cpp b/rclcpp/test/rclcpp/test_type_support.cpp new file mode 100644 index 0000000000..1732031029 --- /dev/null +++ b/rclcpp/test/rclcpp/test_type_support.cpp @@ -0,0 +1,199 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/type_support_decl.hpp" +#include "test_msgs/msg/empty.hpp" + +class TestTypeSupport : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + ::testing::AssertionResult test_type_support_init_fini(const rosidl_service_type_support_t * ts) + { + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + auto node_handle = node->get_node_base_interface()->get_rcl_node_handle(); + rcl_ret_t ret = rcl_service_init( + &service_handle, node_handle, ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { + return ::testing::AssertionFailure() << + "Failed rcl_service_init with error string: " << rcl_get_error_string().str; + } + ret = rcl_service_fini(&service_handle, node_handle); + if (ret != RCL_RET_OK) { + return ::testing::AssertionFailure() << + "Failed rcl_service_fini with error string: " << rcl_get_error_string().str; + } + return ::testing::AssertionSuccess(); + } + +protected: + rclcpp::Node::SharedPtr node = std::make_shared("my_node", "/ns"); +}; + +const rcl_publisher_options_t PublisherOptions() +{ + return rclcpp::PublisherOptionsWithAllocator>().template + to_rcl_publisher_options(rclcpp::QoS(10)); +} + +// Auxiliary classes used to test rosidl_message_type_support_t getters +// defined in type_support.hpp +const rosidl_message_type_support_t * ts_parameter_event = + rclcpp::type_support::get_parameter_event_msg_type_support(); + +class TestTSParameterEvent : public rclcpp::PublisherBase +{ +public: + explicit TestTSParameterEvent(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), + "topicTSParameterEvent", + *ts_parameter_event, + PublisherOptions()) {} +}; + +const rosidl_message_type_support_t * ts_set_parameter_result = + rclcpp::type_support::get_set_parameters_result_msg_type_support(); + +class TestTSSetParameterResult : public rclcpp::PublisherBase +{ +public: + explicit TestTSSetParameterResult(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), + "topicTSSetParameterResult", + *ts_set_parameter_result, + PublisherOptions()) {} +}; + +const rosidl_message_type_support_t * ts_parameter_descriptor = + rclcpp::type_support::get_parameter_descriptor_msg_type_support(); + +class TestTSParameterDescriptor : public rclcpp::PublisherBase +{ +public: + explicit TestTSParameterDescriptor(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), + "topicTSParameterDescriptor", + *ts_parameter_descriptor, + PublisherOptions()) {} +}; + +const rosidl_message_type_support_t * ts_list_parameter_result = + rclcpp::type_support::get_list_parameters_result_msg_type_support(); + +class TestTSListParametersResult : public rclcpp::PublisherBase +{ +public: + explicit TestTSListParametersResult(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), + "topicTSListParametersResult", + *ts_list_parameter_result, + PublisherOptions()) {} +}; + +/* + Test that the publisher is created properly for different msg typesupport + */ +TEST_F(TestTypeSupport, basic_getters) { + { + auto publisher = TestTSParameterEvent(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } + { + auto publisher = TestTSSetParameterResult(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } + { + auto publisher = TestTSParameterDescriptor(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } + { + auto publisher = TestTSListParametersResult(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } +} + +/* Testing type support getters */ +TEST_F(TestTypeSupport, test_service_ts_get_params_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_get_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_get_params_srv_type) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_get_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_get_parameters_types_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_get_parameter_types_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_set_params_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_set_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_list_params_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_list_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_describe_params_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_describe_parameters_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} + +TEST_F(TestTypeSupport, test_service_ts_set_params_atomically_srv) { + const rosidl_service_type_support_t * ts = + rclcpp::type_support::get_set_parameters_atomically_srv_type_support(); + ASSERT_NE(nullptr, ts); + EXPECT_TRUE(test_type_support_init_fini(ts)); +} From db37a32defc89f8e490831fbe7f1d53c07c1e4b5 Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 18 Sep 2020 12:44:19 -0700 Subject: [PATCH 27/61] Add coverage for wait_set_policies (#1316) * Add mocking utils for rclcpp Signed-off-by: Stephen Brawner * Add coverage for wait_set_policies Signed-off-by: Stephen Brawner * Address PR feedback Signed-off-by: Stephen Brawner * Fix windows issues Signed-off-by: Stephen Brawner * Add test comment Signed-off-by: Stephen Brawner --- rclcpp/package.xml | 1 + rclcpp/test/CMakeLists.txt | 24 + rclcpp/test/mocking_utils/patch.hpp | 522 ++++++++++++++++++ rclcpp/test/rclcpp/test_wait_set.cpp | 39 ++ .../test_dynamic_storage.cpp | 299 ++++++++++ .../wait_set_policies/test_static_storage.cpp | 202 +++++++ .../test_storage_policy_common.cpp | 170 ++++++ .../test_thread_safe_synchronization.cpp | 303 ++++++++++ 8 files changed, 1560 insertions(+) create mode 100644 rclcpp/test/mocking_utils/patch.hpp create mode 100644 rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp create mode 100644 rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp create mode 100644 rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp create mode 100644 rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 6be632dcaf..15eaea4af0 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -35,6 +35,7 @@ ament_cmake_gtest ament_lint_auto ament_lint_common + mimick_vendor rmw rmw_implementation_cmake rosidl_default_generators diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 598798f838..92796b42cc 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -566,6 +566,30 @@ if(TARGET test_subscription_options) target_link_libraries(test_subscription_options ${PROJECT_NAME}) endif() +ament_add_gtest(test_dynamic_storage rclcpp/wait_set_policies/test_dynamic_storage.cpp) +if(TARGET test_dynamic_storage) + ament_target_dependencies(test_dynamic_storage "rcl" "test_msgs") + target_link_libraries(test_dynamic_storage ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_storage_policy_common rclcpp/wait_set_policies/test_storage_policy_common.cpp) +if(TARGET test_storage_policy_common) + ament_target_dependencies(test_storage_policy_common "rcl" "test_msgs") + target_link_libraries(test_storage_policy_common ${PROJECT_NAME} mimick) +endif() + +ament_add_gtest(test_static_storage rclcpp/wait_set_policies/test_static_storage.cpp) +if(TARGET test_static_storage) + ament_target_dependencies(test_static_storage "rcl" "test_msgs") + target_link_libraries(test_static_storage ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_thread_safe_synchronization rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp) +if(TARGET test_thread_safe_synchronization) + ament_target_dependencies(test_thread_safe_synchronization "rcl" "test_msgs") + target_link_libraries(test_thread_safe_synchronization ${PROJECT_NAME}) +endif() + ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp) if(TARGET test_rclcpp_gtest_macros) target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME}) diff --git a/rclcpp/test/mocking_utils/patch.hpp b/rclcpp/test/mocking_utils/patch.hpp new file mode 100644 index 0000000000..b9931d33e6 --- /dev/null +++ b/rclcpp/test/mocking_utils/patch.hpp @@ -0,0 +1,522 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Original file taken from: +// https://github.com/ros2/rcutils/blob/master/test/mocking_utils/patch.hpp + +#ifndef MOCKING_UTILS__PATCH_HPP_ +#define MOCKING_UTILS__PATCH_HPP_ + +#define MOCKING_UTILS_SUPPORT_VA_LIST +#if (defined(__aarch64__) || defined(__arm__) || defined(_M_ARM) || defined(__thumb__)) +// In ARM machines, va_list does not define comparison operators +// nor the compiler allows defining them via operator overloads. +// Thus, Mimick argument matching code will not compile. +#undef MOCKING_UTILS_SUPPORT_VA_LIST +#endif + +#ifdef MOCKING_UTILS_SUPPORT_VA_LIST +#include +#endif + +#include +#include +#include +#include + +#include "mimick/mimick.h" + +#include "rcutils/error_handling.h" +#include "rcutils/macros.h" + +namespace mocking_utils +{ + +/// Mimick specific traits for each mocking_utils::Patch instance. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam SignatureT Type of the symbol to be patched. +*/ +template +struct PatchTraits; + +/// Traits specialization for ReturnT(void) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT); +}; + +/// Traits specialization for void(void) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void); +}; + +/// Traits specialization for ReturnT(ArgT0) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgT0 Argument type. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0); +}; + +/// Traits specialization for void(ArgT0) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgT0 Argument type. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1); +}; + +/// Traits specialization for void(ArgT0, ArgT1) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0, ArgT1); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4) +/// free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6); +}; + +/// Generic trampoline to wrap generalized callables in plain functions. +/** + * \tparam ID Numerical identifier of this trampoline. Ought to be unique. + * \tparam SignatureT Type of the symbol this trampoline replaces. + */ +template +struct Trampoline; + +/// Trampoline specialization for free functions. +template +struct Trampoline +{ + static ReturnT base(ArgTs... args) + { + return target(std::forward(args)...); + } + + static std::function target; +}; + +template +std::function +Trampoline::target; + +/// Setup trampoline with the given @p target. +/** + * \param[in] target Callable that this trampoline will target. + * \return the plain base function of this trampoline. + * + * \tparam ID Numerical identifier of this trampoline. Ought to be unique. + * \tparam SignatureT Type of the symbol this trampoline replaces. + */ +template +auto prepare_trampoline(std::function target) +{ + Trampoline::target = target; + return Trampoline::base; +} + +/// Patch class for binary API mocking +/** + * Built on top of Mimick, to enable symbol mocking on a per dynamically + * linked binary object basis. + * + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam SignatureT Type of the symbol to be patched. + */ +template +class Patch; + +/// Patch specialization for ReturnT(ArgTs...) free functions. +/** + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTs Argument types. + */ +template +class Patch +{ +public: + using mock_type = typename PatchTraits::mock_type; + + /// Construct a patch. + /** + * \param[in] target Symbol target string, using Mimick syntax + * i.e. "symbol(@scope)?", where scope may be "self" to target the current + * binary, "lib:library_name" to target a given library, "file:path/to/library" + * to target a given file, or "sym:other_symbol" to target the first library + * that defines said symbol. + * \param[in] proxy An indirection to call the target function. + * This indirection must ensure this call goes through the function's + * trampoline, as setup by the dynamic linker. + * \return a mocking_utils::Patch instance. + */ + explicit Patch(const std::string & target, std::function proxy) + : target_(target), proxy_(proxy) + { + } + + // Copy construction and assignment are disabled. + Patch(const Patch &) = delete; + Patch & operator=(const Patch &) = delete; + + Patch(Patch && other) + { + mock_ = other.mock_; + other.mock_ = nullptr; + } + + Patch & operator=(Patch && other) + { + if (mock_) { + mmk_reset(mock_); + } + mock_ = other.mock_; + other.mock_ = nullptr; + } + + ~Patch() + { + if (mock_) { + mmk_reset(mock_); + } + } + + /// Inject a @p replacement for the patched function. + Patch & then_call(std::function replacement) & + { + replace_with(replacement); + return *this; + } + + /// Inject a @p replacement for the patched function. + Patch && then_call(std::function replacement) && + { + replace_with(replacement); + return std::move(*this); + } + +private: + // Helper for template parameter pack expansion using `mmk_any` + // macro as pattern. + template + T any() {return mmk_any(T);} + + void replace_with(std::function replacement) + { + if (mock_) { + throw std::logic_error("Cannot configure patch more than once"); + } + auto type_erased_trampoline = + reinterpret_cast(prepare_trampoline(replacement)); + auto MMK_MANGLE(mock_type, create) = + PatchTraits::MMK_MANGLE(mock_type, create); + mock_ = mmk_mock(target_.c_str(), mock_type); + mmk_when(proxy_(any()...), .then_call = type_erased_trampoline); + } + + mock_type mock_{nullptr}; + std::string target_; + std::function proxy_; +}; + +/// Make a patch for a `target` function. +/** + * Useful for type deduction during \ref mocking_utils::Patch construction. + * + * \param[in] target Symbol target string, using Mimick syntax. + * \param[in] proxy An indirection to call the target function. + * \return a mocking_utils::Patch instance. + * + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam SignatureT Type of the function to be patched. + * + * \sa mocking_utils::Patch for further reference. + */ +template +auto make_patch(const std::string & target, std::function proxy) +{ + return Patch(target, proxy); +} + +/// Define a dummy operator `op` for a given `type`. +/** + * Useful to enable patching functions that take arguments whose types + * do not define basic comparison operators, as required by Mimick. +*/ +#define MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(type_, op) \ + template \ + typename std::enable_if::value, bool>::type \ + operator op(const T &, const T &) { \ + return false; \ + } + +/// Get the exact \ref mocking_utils::Patch type for a given `id` and `function`. +/** + * Useful to avoid ignored attribute warnings when using the \b decltype operator. + */ +#define MOCKING_UTILS_PATCH_TYPE(id, function) \ + decltype(mocking_utils::make_patch("", nullptr)) + +/// A transparent forwarding proxy to a given `function`. +/** + * Useful to ensure a call to `function` goes through its trampoline. + */ +#define MOCKING_UTILS_PATCH_PROXY(function) \ + [] (auto && ... args)->decltype(auto) { \ + return function(std::forward(args)...); \ + } + +/// Compute a Mimick symbol target string based on which `function` is to be patched +/// in which `scope`. +#define MOCKING_UTILS_PATCH_TARGET(scope, function) \ + (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope)) + +/// Prepare a mocking_utils::Patch for patching a `function` in a given `scope` +/// but defer applying any changes. +#define prepare_patch(scope, function) \ + make_patch<__COUNTER__, decltype(function)>( \ + MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \ + ) + +/// Patch a `function` with a used-provided `replacement` in a given `scope`. +#define patch(scope, function, replacement) \ + prepare_patch(scope, function).then_call(replacement) + +/// Patch a `function` to always yield a given `return_code` in a given `scope`. +#define patch_and_return(scope, function, return_code) \ + patch(scope, function, [&](auto && ...) {return return_code;}) + +/// Patch a `function` to always yield a given `return_code` in a given `scope`. +#define patch_to_fail(scope, function, error_message, return_code) \ + patch( \ + scope, function, [&](auto && ...) { \ + RCUTILS_SET_ERROR_MSG(error_message); \ + return return_code; \ + }) + +/// Patch a `function` to execute normally but always yield a given `return_code` +/// in a given `scope`. +/** + * \warning On some Linux distributions (e.g. CentOS), pointers to function + * reference their PLT trampolines. In such cases, it is not possible to + * call `function` from within the mock. + */ +#define inject_on_return(scope, function, return_code) \ + patch( \ + scope, function, ([&, base = function](auto && ... __args) { \ + if (base != function) { \ + static_cast(base(std::forward(__args)...)); \ + } else { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + "[WARNING] mocking_utils::inject_on_return() cannot forward call to " \ + "original '" RCUTILS_STRINGIFY(function) "' function before injection\n" \ + " at " __FILE__ ":" RCUTILS_STRINGIFY(__LINE__) "\n"); \ + } \ + return return_code; \ + })) + +} // namespace mocking_utils + +#ifdef MOCKING_UTILS_SUPPORT_VA_LIST +// Define dummy comparison operators for C standard va_list type +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, >) +#endif + +#endif // MOCKING_UTILS__PATCH_HPP_ diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index 39fe4c1262..7a5f7b3fbe 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -15,12 +15,15 @@ #include #include +#include #include #include "rcl_interfaces/srv/list_parameters.hpp" #include "rclcpp/rclcpp.hpp" #include "test_msgs/msg/basic_types.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + class TestWaitSet : public ::testing::Test { protected: @@ -262,3 +265,39 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { }, std::runtime_error); } } + +/* + * Get wait_set from result. + */ +TEST_F(TestWaitSet, get_result_from_wait_result) { + rclcpp::WaitSet wait_set; + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + guard_condition->trigger(); + + rclcpp::WaitResult result = wait_set.wait(); + ASSERT_EQ(rclcpp::WaitResultKind::Ready, result.kind()); + EXPECT_EQ(&wait_set, &result.get_wait_set()); + + const rclcpp::WaitResult const_result(std::move(result)); + ASSERT_EQ(rclcpp::WaitResultKind::Ready, const_result.kind()); + EXPECT_EQ(&wait_set, &const_result.get_wait_set()); +} + +TEST_F(TestWaitSet, get_result_from_wait_result_not_ready_error) { + rclcpp::WaitSet wait_set; + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + rclcpp::WaitResult result = wait_set.wait(std::chrono::milliseconds(10)); + ASSERT_EQ(rclcpp::WaitResultKind::Timeout, result.kind()); + RCLCPP_EXPECT_THROW_EQ( + result.get_wait_set(), + std::runtime_error("cannot access wait set when the result was not ready")); + + const rclcpp::WaitResult const_result(std::move(result)); + ASSERT_EQ(rclcpp::WaitResultKind::Timeout, const_result.kind()); + RCLCPP_EXPECT_THROW_EQ( + const_result.get_wait_set(), + std::runtime_error("cannot access wait set when the result was not ready")); +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp new file mode 100644 index 0000000000..c361231831 --- /dev/null +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -0,0 +1,299 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestDynamicStorage : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + } + + std::shared_ptr node; +}; + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + : is_ready_(false) {} + + bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + + bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + + void execute() override {} + + void set_is_ready(bool value) {is_ready_ = value;} + +private: + bool is_ready_; +}; + +TEST_F(TestDynamicStorage, default_construct_destruct) { + rclcpp::WaitSet wait_set; + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait().kind()); +} + +TEST_F(TestDynamicStorage, iterables_construct_destruct) { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto guard_condition = std::make_shared(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + auto subscriptions = + std::vector{{subscription}}; + auto guard_conditions = + std::vector{guard_condition}; + auto timers = + std::vector{timer}; + auto clients = + std::vector{client}; + auto services = + std::vector{service}; + auto waitables = + std::vector{{waitable}}; + rclcpp::WaitSet wait_set(subscriptions, guard_conditions, timers, clients, services, waitables); + + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); +} + +TEST_F(TestDynamicStorage, add_remove_dynamically) { + rclcpp::WaitSet wait_set; + + // Adds more coverage + rclcpp::SubscriptionOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options); + + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + wait_set.add_subscription(subscription, mask); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_subscription(subscription, mask), + std::runtime_error("subscription already associated with a wait set")); + wait_set.remove_subscription(subscription, mask); + + // This is long, so it can stick around and be removed + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + wait_set.add_timer(timer); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_timer(timer), + std::runtime_error("timer already in use by another wait set")); + wait_set.remove_timer(timer); + + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_guard_condition(guard_condition), + std::runtime_error("guard condition already in use by another wait set")); + wait_set.remove_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_service(service), + std::runtime_error("service already in use by another wait set")); + wait_set.remove_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_client(client), + std::runtime_error("client already in use by another wait set")); + wait_set.remove_client(client); + + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_waitable(waitable), + std::runtime_error("waitable already in use by another wait set")); + + wait_set.remove_waitable(waitable); + wait_set.prune_deleted_entities(); + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait().kind()); +} + +TEST_F(TestDynamicStorage, add_remove_nullptr) { + rclcpp::WaitSet wait_set; + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_subscription(nullptr), std::invalid_argument("subscription is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_subscription(nullptr), std::invalid_argument("subscription is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_timer(nullptr), std::invalid_argument("timer is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_timer(nullptr), std::invalid_argument("timer is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_client(nullptr), std::invalid_argument("client is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_client(nullptr), std::invalid_argument("client is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_service(nullptr), std::invalid_argument("service is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_service(nullptr), std::invalid_argument("service is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_waitable(nullptr), std::invalid_argument("waitable is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_waitable(nullptr), std::invalid_argument("waitable is nullptr")); +} + +TEST_F(TestDynamicStorage, add_remove_out_of_scope) { + rclcpp::WaitSet wait_set; + + { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + wait_set.add_subscription(subscription); + + // This is short, so if it's not cleaned up, it will trigger wait and it won't timeout + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + wait_set.add_timer(timer); + + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + } + + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind()); +} + +TEST_F(TestDynamicStorage, wait_subscription) { + rclcpp::WaitSet wait_set; + + // Not added to wait_set, just used for publishing to the topic + auto publisher = node->create_publisher("topic", 10); + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + wait_set.add_subscription(subscription); + + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + + publisher->publish(test_msgs::msg::Empty()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestDynamicStorage, wait_timer) { + rclcpp::WaitSet wait_set; + + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + wait_set.add_timer(timer); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestDynamicStorage, wait_client_service) { + rclcpp::WaitSet wait_set; + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + client->async_send_request(std::make_shared()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestDynamicStorage, wait_waitable) { + rclcpp::WaitSet wait_set; + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + { + // This waitable doesn't add itself to the rcl_wait_set_t, so Empty is to be expected + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_result.kind()); + } +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp new file mode 100644 index 0000000000..2fdb71db25 --- /dev/null +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -0,0 +1,202 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "rclcpp/wait_set_policies/static_storage.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestStaticStorage : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + } + + std::shared_ptr node; +}; + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + : is_ready_(false) {} + bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + + bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + + void execute() override {} + + void set_is_ready(bool value) {is_ready_ = value;} + +private: + bool is_ready_; +}; + +TEST_F(TestStaticStorage, iterables_construct_destruct) { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + // This is long, so it can stick around and be removed + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto guard_condition = std::make_shared(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + rclcpp::StaticWaitSet<1, 1, 1, 1, 1, 1> wait_set( + {{{subscription}}}, {guard_condition}, {timer}, {client}, {service}, {{{waitable}}}); + + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); +} + +// Because these StaticWaitSet's have templated sizes larger than the input arguments passed +// to the constructor, their shared-pointer contents will be default constructed to null. This +// test just checks the appropriate exception is thrown. +// std::shared_ptr::reset() is not required for these exceptions, it just +// disables the unused return value warning of std::make_shared +TEST_F(TestStaticStorage, fixed_storage_needs_pruning) { + { + using StaticWaitSet = rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 1, 0, 0, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 1, 0, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 0, 1, 0>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } + { + using StaticWaitSet = rclcpp::StaticWaitSet<0, 0, 0, 0, 0, 1>; + RCLCPP_EXPECT_THROW_EQ( + std::make_shared().reset(), + std::runtime_error("unexpected condition, fixed storage policy needs pruning")); + } +} + +TEST_F(TestStaticStorage, wait_subscription) { + auto publisher = node->create_publisher("topic", 10); + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{subscription, mask}}}); + + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + + publisher->publish(test_msgs::msg::Empty()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestStaticStorage, wait_timer) { + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + rclcpp::StaticWaitSet<0, 0, 1, 0, 0, 0> wait_set({}, {}, {timer}); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestStaticStorage, wait_guard_condition) { + auto guard_condition = std::make_shared(); + rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0> wait_set({}, {guard_condition}); + + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + + guard_condition->trigger(); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestStaticStorage, wait_client_service) { + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + + auto client = node->create_client("service"); + rclcpp::StaticWaitSet<0, 0, 0, 1, 1, 0> wait_set({}, {}, {}, {client}, {service}); + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + client->async_send_request(std::make_shared()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestStaticStorage, wait_waitable) { + auto waitable = std::make_shared(); + rclcpp::StaticWaitSet<0, 0, 0, 0, 0, 1> wait_set({}, {}, {}, {}, {}, {{{waitable}}}); + { + // This waitable doesn't add itself to the rcl_wait_set_t, so Empty is to be expected + auto wait_result = wait_set.wait(std::chrono::seconds(-1)); + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_result.kind()); + } +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp new file mode 100644 index 0000000000..d9b35cd7bd --- /dev/null +++ b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp @@ -0,0 +1,170 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestStoragePolicyCommon : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + } + + std::shared_ptr node; +}; + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + : is_ready_(false), add_to_wait_set_(false) {} + bool add_to_wait_set(rcl_wait_set_t *) override {return add_to_wait_set_;} + + bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + + void execute() override {} + + void set_is_ready(bool value) {is_ready_ = value;} + + void set_add_to_wait_set(bool value) {add_to_wait_set_ = value;} + +private: + bool is_ready_; + bool add_to_wait_set_; +}; + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_fini_error) { + auto wait_set = std::make_shared(); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR); + EXPECT_NO_THROW(wait_set.reset()); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_resize_error) { + rclcpp::WaitSet wait_set; + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR); + wait_set.add_subscription(subscription, mask); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_clear_error) { + rclcpp::WaitSet wait_set; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_subscription_error) { + rclcpp::WaitSet wait_set; + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_subscription, RCL_RET_ERROR); + wait_set.add_subscription(subscription, mask); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_guard_condition_error) { + rclcpp::WaitSet wait_set; + auto guard_condition = std::make_shared(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); + wait_set.add_guard_condition(guard_condition); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_timer_error) { + rclcpp::WaitSet wait_set; + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_timer, RCL_RET_ERROR); + wait_set.add_timer(timer); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_service_error) { + rclcpp::WaitSet wait_set; + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_service, RCL_RET_ERROR); + wait_set.add_service(service); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, rcl_wait_set_add_client_error) { + rclcpp::WaitSet wait_set; + auto client = node->create_client("service"); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_client, RCL_RET_ERROR); + wait_set.add_client(client); + EXPECT_THROW( + wait_set.wait(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestStoragePolicyCommon, add_waitable_error) { + rclcpp::WaitSet wait_set; + auto waitable = std::make_shared(); + waitable->set_add_to_wait_set(false); + wait_set.add_waitable(waitable); + RCLCPP_EXPECT_THROW_EQ( + wait_set.wait(), + std::runtime_error("waitable unexpectedly failed to be added to wait set")); +} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp new file mode 100644 index 0000000000..ba0ccda59c --- /dev/null +++ b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp @@ -0,0 +1,303 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_set.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +class TestThreadSafeStorage : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + } + + std::shared_ptr node; +}; + +class TestWaitable : public rclcpp::Waitable +{ +public: + TestWaitable() + : is_ready_(false) {} + bool add_to_wait_set(rcl_wait_set_t *) override {return true;} + + bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + + void execute() override {} + + void set_is_ready(bool value) {is_ready_ = value;} + +private: + bool is_ready_; +}; + +TEST_F(TestThreadSafeStorage, default_construct_destruct) { + rclcpp::ThreadSafeWaitSet wait_set; + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); + + // Expected behavior of thread-safe is to timeout here + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind()); +} + +TEST_F(TestThreadSafeStorage, iterables_construct_destruct) { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + // This is long, so it can stick around + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + auto guard_condition = std::make_shared(); + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + auto client = node->create_client("service"); + auto waitable = std::make_shared(); + auto subscriptions = + std::vector{{subscription}}; + auto guard_conditions = + std::vector{guard_condition}; + auto timers = + std::vector{timer}; + auto clients = + std::vector{client}; + auto services = + std::vector{service}; + auto waitables = + std::vector{{waitable}}; + rclcpp::ThreadSafeWaitSet wait_set( + subscriptions, guard_conditions, timers, clients, services, waitables); + + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set.get_rcl_wait_set())); +} + +TEST_F(TestThreadSafeStorage, add_remove_dynamically) { + rclcpp::ThreadSafeWaitSet wait_set; + + // Adds more coverage + rclcpp::SubscriptionOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}, options); + + rclcpp::SubscriptionWaitSetMask mask{true, true, true}; + wait_set.add_subscription(subscription, mask); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_subscription(subscription, mask), + std::runtime_error("subscription already associated with a wait set")); + wait_set.remove_subscription(subscription, mask); + + // This is long, so it can stick around and be removed + auto timer = node->create_wall_timer(std::chrono::seconds(100), []() {}); + wait_set.add_timer(timer); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_timer(timer), + std::runtime_error("timer already in use by another wait set")); + wait_set.remove_timer(timer); + + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_guard_condition(guard_condition), + std::runtime_error("guard condition already in use by another wait set")); + wait_set.remove_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_service(service), + std::runtime_error("service already in use by another wait set")); + wait_set.remove_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_client(client), + std::runtime_error("client already in use by another wait set")); + wait_set.remove_client(client); + + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_waitable(waitable), + std::runtime_error("waitable already in use by another wait set")); + wait_set.remove_waitable(waitable); + wait_set.prune_deleted_entities(); + + // Expected behavior of thread-safe is to timeout here + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind()); +} + +TEST_F(TestThreadSafeStorage, add_remove_nullptr) { + rclcpp::ThreadSafeWaitSet wait_set; + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_subscription(nullptr), std::invalid_argument("subscription is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_subscription(nullptr), std::invalid_argument("subscription is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_guard_condition(nullptr), std::invalid_argument("guard_condition is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_timer(nullptr), std::invalid_argument("timer is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_timer(nullptr), std::invalid_argument("timer is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_client(nullptr), std::invalid_argument("client is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_client(nullptr), std::invalid_argument("client is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_service(nullptr), std::invalid_argument("service is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_service(nullptr), std::invalid_argument("service is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + wait_set.add_waitable(nullptr), std::invalid_argument("waitable is nullptr")); + RCLCPP_EXPECT_THROW_EQ( + wait_set.remove_waitable(nullptr), std::invalid_argument("waitable is nullptr")); +} + +TEST_F(TestThreadSafeStorage, add_remove_out_of_scope) { + rclcpp::ThreadSafeWaitSet wait_set; + + { + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + wait_set.add_subscription(subscription); + + // This is short, so if it's not cleaned up, it will trigger wait + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + wait_set.add_timer(timer); + + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + } + + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_set.wait(std::chrono::milliseconds(10)).kind()); +} + +TEST_F(TestThreadSafeStorage, wait_subscription) { + rclcpp::ThreadSafeWaitSet wait_set; + + // Not added to wait_set, just used for publishing to the topic + auto publisher = node->create_publisher("topic", 10); + + auto subscription = node->create_subscription( + "topic", 10, [](test_msgs::msg::Empty::SharedPtr) {}); + wait_set.add_subscription(subscription); + + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + + publisher->publish(test_msgs::msg::Empty()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestThreadSafeStorage, wait_timer) { + rclcpp::ThreadSafeWaitSet wait_set; + + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + wait_set.add_timer(timer); + { + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestThreadSafeStorage, wait_client_service) { + rclcpp::ThreadSafeWaitSet wait_set; + auto guard_condition = std::make_shared(); + wait_set.add_guard_condition(guard_condition); + + auto service = + node->create_service( + "service", + []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}); + wait_set.add_service(service); + + auto client = node->create_client("service"); + wait_set.add_client(client); + { + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } + client->async_send_request(std::make_shared()); + { + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_result.kind()); + } +} + +TEST_F(TestThreadSafeStorage, wait_waitable) { + rclcpp::ThreadSafeWaitSet wait_set; + auto waitable = std::make_shared(); + wait_set.add_waitable(waitable); + { + // This waitable doesn't add itself to the rcl_wait_set_t, so Timeout is to be expected + auto wait_result = wait_set.wait(std::chrono::milliseconds(10)); + EXPECT_EQ(rclcpp::WaitResultKind::Timeout, wait_result.kind()); + } +} From fce59a2be70f1cf592455176e2c715b028348ab9 Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 22 Sep 2020 11:00:07 -0700 Subject: [PATCH 28/61] Increase coverage of node_interfaces, including with mocking rcl errors (#1322) * Increase coverage of node_interfaces, including with mocking rcl errors Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 14 +- .../rclcpp/node_interfaces/test_node_base.cpp | 133 +++++++ .../node_interfaces/test_node_graph.cpp | 350 ++++++++++++++---- .../node_interfaces/test_node_parameters.cpp | 140 ++++++- .../node_interfaces/test_node_services.cpp | 64 ++-- .../node_interfaces/test_node_timers.cpp | 37 +- .../node_interfaces/test_node_topics.cpp | 49 ++- .../node_interfaces/test_node_waitables.cpp | 40 +- 8 files changed, 689 insertions(+), 138 deletions(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 92796b42cc..4df68578c3 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -173,7 +173,7 @@ endif() ament_add_gtest(test_node_interfaces__node_base rclcpp/node_interfaces/test_node_base.cpp) if(TARGET test_node_interfaces__node_base) - target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_clock rclcpp/node_interfaces/test_node_clock.cpp) @@ -186,22 +186,22 @@ if(TARGET test_node_interfaces__node_graph) ament_target_dependencies( test_node_interfaces__node_graph "test_msgs") - target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_parameters rclcpp/node_interfaces/test_node_parameters.cpp) if(TARGET test_node_interfaces__node_parameters) - target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_services rclcpp/node_interfaces/test_node_services.cpp) if(TARGET test_node_interfaces__node_services) - target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_timers rclcpp/node_interfaces/test_node_timers.cpp) if(TARGET test_node_interfaces__node_timers) - target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_topics rclcpp/node_interfaces/test_node_topics.cpp) @@ -209,12 +209,12 @@ if(TARGET test_node_interfaces__node_topics) ament_target_dependencies( test_node_interfaces__node_topics "test_msgs") - target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_waitables rclcpp/node_interfaces/test_node_waitables.cpp) if(TARGET test_node_interfaces__node_waitables) - target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick) endif() # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp index cfe42d7819..a40bc712bd 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp @@ -21,6 +21,11 @@ #include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_base.hpp" #include "rclcpp/rclcpp.hpp" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" class TestNodeBase : public ::testing::Test { @@ -36,6 +41,12 @@ class TestNodeBase : public ::testing::Test } }; +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) + TEST_F(TestNodeBase, construct_from_node) { std::shared_ptr node = std::make_shared("node", "ns"); @@ -58,3 +69,125 @@ TEST_F(TestNodeBase, construct_from_node) EXPECT_NE(nullptr, const_node_base->get_rcl_node_handle()); EXPECT_NE(nullptr, const_node_base->get_shared_rcl_node_handle()); } + +TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_init_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_error) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_ERROR); + + // This function is called only if rcl_node_init fails, so both mocked functions are required + // This just logs an error, so behavior shouldn't change + auto mock_guard_condition_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock_validate_node_name = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_invalid_argument) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock_validate_node_name = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLInvalidArgument); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_valid_rmw_node_name) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock = mocking_utils::patch( + "lib:rclcpp", rmw_validate_node_name, [](const char *, int * validation_result, size_t *) + { + *validation_result = RMW_NODE_NAME_VALID; + return RMW_RET_OK; + }); + + RCLCPP_EXPECT_THROW_EQ( + std::make_shared("node", "ns").reset(), + std::runtime_error("valid rmw node name but invalid rcl node name")); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock_validate_namespace = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_rmw_invalid_argument) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock_validate_namespace = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLInvalidArgument); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_valid_rmw_namespace) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock = mocking_utils::patch( + "lib:rclcpp", rmw_validate_namespace, [](const char *, int * validation_result, size_t *) + { + *validation_result = RMW_NAMESPACE_VALID; + return RMW_RET_OK; + }); + + RCLCPP_EXPECT_THROW_EQ( + std::make_shared("node", "ns").reset(), + std::runtime_error("valid rmw node namespace but invalid rcl node namespace")); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_fini_error) { + auto mock_node_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_node_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW(std::make_shared("node", "ns").reset()); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_fini_error) { + auto mock_node_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW(std::make_shared("node", "ns").reset()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index a1922d56af..6a10366f8b 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -15,105 +15,172 @@ #include #include +#include #include +#include "rcl/graph.h" #include "rcl/node_options.h" +#include "rcl/remap.h" #include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_base.hpp" #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/rclcpp.hpp" +#include "rcutils/strdup.h" #include "test_msgs/msg/empty.hpp" #include "test_msgs/srv/empty.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +namespace +{ + +constexpr char node_name[] = "node"; +constexpr char node_namespace[] = "ns"; +constexpr char absolute_namespace[] = "/ns"; + +} // namespace + class TestNodeGraph : public ::testing::Test { public: void SetUp() { rclcpp::init(0, nullptr); + node_ = std::make_shared(node_name, node_namespace); + + // This dynamic cast is not necessary for the unittests, but instead is used to ensure + // the proper type is being tested and covered. + node_graph_ = + dynamic_cast(node_->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph_); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node() {return node_;} + + const rclcpp::node_interfaces::NodeGraph * node_graph() const {return node_graph_;} + +private: + std::shared_ptr node_; + rclcpp::node_interfaces::NodeGraph * node_graph_; }; TEST_F(TestNodeGraph, construct_from_node) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); - - auto topic_names_and_types = node_graph->get_topic_names_and_types(false); + auto topic_names_and_types = node_graph()->get_topic_names_and_types(false); EXPECT_LT(0u, topic_names_and_types.size()); - auto service_names_and_types = node_graph->get_service_names_and_types(); + auto service_names_and_types = node_graph()->get_service_names_and_types(); EXPECT_LT(0u, service_names_and_types.size()); - auto names = node_graph->get_node_names(); + auto names = node_graph()->get_node_names(); EXPECT_EQ(1u, names.size()); - auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); + auto names_and_namespaces = node_graph()->get_node_names_and_namespaces(); EXPECT_EQ(1u, names_and_namespaces.size()); - EXPECT_EQ(0u, node_graph->count_publishers("not_a_topic")); - EXPECT_EQ(0u, node_graph->count_subscribers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic")); + EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition()); + + // get_graph_event is non-const + EXPECT_NE(nullptr, node()->get_node_graph_interface()->get_graph_event()); + EXPECT_EQ(1u, node()->get_node_graph_interface()->count_graph_users()); } TEST_F(TestNodeGraph, get_topic_names_and_types) { - auto node = std::make_shared("node2", "ns"); - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); - auto topic_names_and_types = node_graph->get_topic_names_and_types(); + auto topic_names_and_types = node_graph()->get_topic_names_and_types(); EXPECT_LT(0u, topic_names_and_types.size()); } +TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_error) +{ + auto mock_get_topic_names = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_topic_names_and_types, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_topic_names_and_types(), + std::runtime_error( + "failed to get topic names and types: error not set, failed also to cleanup topic names and" + " types, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_names_and_types_fini_error) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_topic_names_and_types(), + std::runtime_error("could not destroy topic names and types: error not set")); +} + TEST_F(TestNodeGraph, get_service_names_and_types) { - auto node = std::make_shared("node2", "ns"); - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); - auto service_names_and_types = node_graph->get_service_names_and_types(); + auto service_names_and_types = node_graph()->get_service_names_and_types(); EXPECT_LT(0u, service_names_and_types.size()); } +TEST_F(TestNodeGraph, get_service_names_and_types_rcl_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_service_names_and_types, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types(), + std::runtime_error( + "failed to get service names and types: error not set, failed also to cleanup service names" + " and types, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_rcl_names_and_types_fini) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types(), + std::runtime_error("could not destroy service names and types: error not set")); +} + TEST_F(TestNodeGraph, get_service_names_and_types_by_node) { - auto node1 = std::make_shared("node1", "ns"); auto callback = []( const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto service = - node1->create_service("node1_service", std::move(callback)); - auto node2 = std::make_shared("node2", "ns"); - const auto * node_graph = - dynamic_cast(node1->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + node()->create_service("node1_service", std::move(callback)); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); // rcl_get_service_names_and_types_by_node() expects the node to exist, otherwise it fails EXPECT_THROW( - node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + node_graph()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), std::runtime_error); // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. auto services_of_node1 = - node_graph->get_service_names_and_types_by_node("node1", "/ns"); + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace); auto services_of_node2 = - node_graph->get_service_names_and_types_by_node("node2", "/ns"); + node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace); auto start = std::chrono::steady_clock::now(); while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { - services_of_node1 = node_graph->get_service_names_and_types_by_node("node1", "/ns"); - services_of_node2 = node_graph->get_service_names_and_types_by_node("node2", "/ns"); + services_of_node1 = + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace); + services_of_node2 = + node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace); if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) { break; } @@ -123,70 +190,134 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node) EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end()); } -TEST_F(TestNodeGraph, get_node_names_and_namespaces) +TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors) { - auto node = std::make_shared("node", "ns"); - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node()->create_service("node1_service", std::move(callback)); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_service_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get service names and types by node: error not set, failed also to cleanup" + " service names and types, leaking memory: error not set")); +} - auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); +TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_error) +{ + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node()->create_service("node1_service", std::move(callback)); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_THROW( + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_node_names_and_namespaces) +{ + auto names_and_namespaces = node_graph()->get_node_names_and_namespaces(); EXPECT_EQ(1u, names_and_namespaces.size()); } -TEST_F(TestNodeGraph, notify_shutdown) +TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors) { - auto node = std::make_shared("node", "ns"); - auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_node_names, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_and_namespaces(), + std::runtime_error( + "failed to get node names: error not set, failed also to cleanup node names, leaking memory:" + " error not set, failed also to cleanup node namespaces, leaking memory: error not set")); +} - EXPECT_NO_THROW(node_graph->notify_shutdown()); +TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_and_namespaces(), + std::runtime_error("could not destroy node names, could not destroy node namespaces")); } -TEST_F(TestNodeGraph, wait_for_graph_change) +TEST_F(TestNodeGraph, count_publishers_rcl_error) { - std::shared_ptr node = std::make_shared("node", "ns"); - auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_publishers, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_publishers("topic"), + std::runtime_error("could not count publishers: error not set")); +} + +TEST_F(TestNodeGraph, count_subscribers_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_subscribers, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_subscribers("topic"), + std::runtime_error("could not count subscribers: error not set")); +} + +TEST_F(TestNodeGraph, notify_shutdown) +{ + EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown()); +} - EXPECT_NO_THROW(node_graph->notify_graph_change()); +TEST_F(TestNodeGraph, wait_for_graph_change) +{ + auto node_graph_interface = node()->get_node_graph_interface(); + EXPECT_NO_THROW(node_graph_interface->notify_graph_change()); EXPECT_THROW( - node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), + node_graph_interface->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), rclcpp::exceptions::InvalidEventError); auto event = std::make_shared(); EXPECT_THROW( - node_graph->wait_for_graph_change(event, std::chrono::milliseconds(0)), + node_graph_interface->wait_for_graph_change(event, std::chrono::milliseconds(0)), rclcpp::exceptions::EventNotRegisteredError); } +TEST_F(TestNodeGraph, notify_graph_change_rcl_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + EXPECT_THROW( + node()->get_node_graph_interface()->notify_graph_change(), + rclcpp::exceptions::RCLError); +} + TEST_F(TestNodeGraph, get_info_by_topic) { - std::shared_ptr node = std::make_shared("node", "ns"); const rclcpp::QoS publisher_qos(1); - auto publisher = node->create_publisher("topic", publisher_qos); + auto publisher = node()->create_publisher("topic", publisher_qos); auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; const rclcpp::QoS subscriber_qos(10); auto subscription = - node->create_subscription( + node()->create_subscription( "topic", subscriber_qos, std::move(callback)); - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + EXPECT_EQ(0u, node_graph()->get_publishers_info_by_topic("topic", true).size()); - auto publishers = node_graph->get_publishers_info_by_topic("topic", false); + auto publishers = node_graph()->get_publishers_info_by_topic("topic", false); ASSERT_EQ(1u, publishers.size()); auto publisher_endpoint_info = publishers[0]; const auto const_publisher_endpoint_info = publisher_endpoint_info; - EXPECT_STREQ("node", publisher_endpoint_info.node_name().c_str()); - EXPECT_STREQ("node", const_publisher_endpoint_info.node_name().c_str()); - EXPECT_STREQ("/ns", publisher_endpoint_info.node_namespace().c_str()); - EXPECT_STREQ("/ns", const_publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ(node_name, publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ(node_name, const_publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ(absolute_namespace, publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ(absolute_namespace, const_publisher_endpoint_info.node_namespace().c_str()); EXPECT_STREQ("test_msgs/msg/Empty", publisher_endpoint_info.topic_type().c_str()); EXPECT_STREQ("test_msgs/msg/Empty", const_publisher_endpoint_info.topic_type().c_str()); EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_endpoint_info.endpoint_type()); @@ -225,3 +356,90 @@ TEST_F(TestNodeGraph, get_info_by_topic) } EXPECT_FALSE(endpoint_gid_is_all_zeros); } + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_node_get_options_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_node_get_options, nullptr); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_publishers_info_by_topic("topic", false), + std::runtime_error("Need valid node options in get_info_by_topic()")); +} + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_remap_topic_name, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_publishers_info_by_topic("topic", false), + std::runtime_error("Failed to remap topic name /ns/topic: error not set")); +} + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_nullptr) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + // Should be cleaned up by get_info_by_topic + char * some_string = rcutils_strdup("", rcl_get_default_allocator()); + ASSERT_NE(nullptr, some_string); + auto mock = + mocking_utils::patch( + "lib:rclcpp", rcl_remap_topic_name, [&some_string]( + const rcl_arguments_t *, const rcl_arguments_t *, const char *, const char *, const char *, + rcl_allocator_t, char ** output_name) + { + *output_name = some_string; + return RCL_RET_OK; + }); + EXPECT_NO_THROW(node_graph()->get_publishers_info_by_topic("topic", false)); +} + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_errors) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_ERROR); + auto mock_info_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_info_by_topic_unsupported) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_UNSUPPORTED); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_info_by_topic_endpoint_info_array_fini_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock_info_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 68a7546c83..786422d0d2 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -28,31 +28,47 @@ #include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + class TestNodeParameters : public ::testing::Test { public: void SetUp() { rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + node = std::make_shared("node", "ns", options); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeParameters * node_parameters; }; +TEST_F(TestNodeParameters, construct_destruct_rcl_errors) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_arguments_get_param_overrides, RCL_RET_ERROR); + EXPECT_THROW( + std::make_shared("node2", "ns").reset(), + rclcpp::exceptions::RCLError); +} + TEST_F(TestNodeParameters, list_parameters) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_parameters = - dynamic_cast( - node->get_node_parameters_interface().get()); - ASSERT_NE(nullptr, node_parameters); - std::vector prefixes; const auto list_result = node_parameters->list_parameters(prefixes, 1u); @@ -73,17 +89,113 @@ TEST_F(TestNodeParameters, list_parameters) EXPECT_NE( std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name), list_result2.names.end()); + + // Check prefixes + const std::string parameter_name2 = "prefix.new_parameter"; + const rclcpp::ParameterValue value2(true); + const rcl_interfaces::msg::ParameterDescriptor descriptor2; + const auto added_parameter_value2 = + node_parameters->declare_parameter(parameter_name2, value2, descriptor2, false); + EXPECT_EQ(value.get(), added_parameter_value.get()); + prefixes = {"prefix"}; + auto list_result3 = node_parameters->list_parameters(prefixes, 2u); + EXPECT_EQ(1u, list_result3.names.size()); + EXPECT_NE( + std::find(list_result3.names.begin(), list_result3.names.end(), parameter_name2), + list_result3.names.end()); + + // Check if prefix equals parameter name + prefixes = {"new_parameter"}; + auto list_result4 = node_parameters->list_parameters(prefixes, 2u); + EXPECT_EQ(1u, list_result4.names.size()); + EXPECT_NE( + std::find(list_result4.names.begin(), list_result4.names.end(), parameter_name), + list_result4.names.end()); } TEST_F(TestNodeParameters, parameter_overrides) { - std::shared_ptr node = std::make_shared("node", "ns"); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_options.append_parameter_override("param1", true); + node_options.append_parameter_override("param2", 42); + + std::shared_ptr node2 = std::make_shared("node2", "ns", node_options); - auto * node_parameters = + auto * node_parameters_interface = dynamic_cast( - node->get_node_parameters_interface().get()); + node2->get_node_parameters_interface().get()); ASSERT_NE(nullptr, node_parameters); - const auto & parameter_overrides = node_parameters->get_parameter_overrides(); - EXPECT_EQ(0u, parameter_overrides.size()); + const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides(); + EXPECT_EQ(2u, parameter_overrides.size()); +} + +TEST_F(TestNodeParameters, set_parameters) { + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true); + + rcl_interfaces::msg::ParameterDescriptor bool_descriptor; + bool_descriptor.name = "bool_parameter"; + bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + bool_descriptor.read_only = false; + node_parameters->declare_parameter( + "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false); + + rcl_interfaces::msg::ParameterDescriptor read_only_descriptor; + read_only_descriptor.name = "read_only_parameter"; + read_only_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + read_only_descriptor.read_only = true; + node_parameters->declare_parameter( + "read_only_parameter", rclcpp::ParameterValue(42), read_only_descriptor, false); + + const std::vector parameters = { + rclcpp::Parameter("bool_parameter", true), + rclcpp::Parameter("read_only_parameter", 42), + }; + auto result = node_parameters->set_parameters(parameters); + ASSERT_EQ(parameters.size(), result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_FALSE(result[1].successful); + EXPECT_STREQ( + "parameter 'read_only_parameter' cannot be set because it is read-only", + result[1].reason.c_str()); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->set_parameters({rclcpp::Parameter("", true)}), + rclcpp::exceptions::InvalidParametersException("parameter name must not be empty")); + + result = node_parameters->set_parameters({rclcpp::Parameter("undeclared_parameter", 3.14159)}); + ASSERT_EQ(1u, result.size()); + EXPECT_TRUE(result[0].successful); +} + +TEST_F(TestNodeParameters, add_remove_parameters_callback) { + rcl_interfaces::msg::ParameterDescriptor bool_descriptor; + bool_descriptor.name = "bool_parameter"; + bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + bool_descriptor.read_only = false; + node_parameters->declare_parameter( + "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false); + const std::vector parameters = {rclcpp::Parameter("bool_parameter", true)}; + + const std::string reason = "some totally not made up reason"; + auto callback = [reason](const std::vector &) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = reason; + return result; + }; + + auto handle = node_parameters->add_on_set_parameters_callback(callback); + auto result = node_parameters->set_parameters(parameters); + ASSERT_EQ(1u, result.size()); + EXPECT_FALSE(result[0].successful); + EXPECT_EQ(reason, result[0].reason); + + EXPECT_NO_THROW(node_parameters->remove_on_set_parameters_callback(handle.get())); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_on_set_parameters_callback(handle.get()), + std::runtime_error("Callback doesn't exist")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index 893340d33a..a48c50254a 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -22,6 +22,9 @@ #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/rclcpp.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + class TestService : public rclcpp::ServiceBase { public: @@ -51,25 +54,29 @@ class TestNodeService : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + + node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_services = + dynamic_cast( + node->get_node_services_interface().get()); + ASSERT_NE(nullptr, node_services); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeServices * node_services; }; TEST_F(TestNodeService, add_service) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_services = - dynamic_cast( - node->get_node_services_interface().get()); - ASSERT_NE(nullptr, node_services); - auto service = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW( @@ -80,22 +87,24 @@ TEST_F(TestNodeService, add_service) auto callback_group_in_different_node = different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node_services->add_service(service, callback_group_in_different_node), - std::runtime_error); + std::runtime_error("Cannot create service, group not in node.")); } -TEST_F(TestNodeService, add_client) +TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_services = - dynamic_cast( - node->get_node_services_interface().get()); - ASSERT_NE(nullptr, node_services); + auto service = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_services->add_service(service, callback_group), + std::runtime_error("Failed to notify wait set on service creation: error not set")); +} +TEST_F(TestNodeService, add_client) +{ auto client = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW(node_services->add_client(client, callback_group)); @@ -105,7 +114,18 @@ TEST_F(TestNodeService, add_client) auto callback_group_in_different_node = different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node_services->add_client(client, callback_group_in_different_node), - std::runtime_error); + std::runtime_error("Cannot create client, group not in node.")); +} + +TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) +{ + auto client = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_services->add_client(client, callback_group), + std::runtime_error("Failed to notify wait set on client creation: error not set")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index af340d292e..e206e75b6c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -22,6 +22,9 @@ #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/rclcpp.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + class TestTimer : public rclcpp::TimerBase { public: @@ -39,23 +42,27 @@ class TestNodeTimers : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_timers = + dynamic_cast(node->get_node_timers_interface().get()); + ASSERT_NE(nullptr, node_timers); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeTimers * node_timers; }; TEST_F(TestNodeTimers, add_timer) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto node_timers = - dynamic_cast(node->get_node_timers_interface().get()); - ASSERT_NE(nullptr, node_timers); auto timer = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW(node_timers->add_timer(timer, callback_group)); @@ -65,7 +72,19 @@ TEST_F(TestNodeTimers, add_timer) auto callback_group_in_different_node = different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node_timers->add_timer(timer, callback_group_in_different_node), - std::runtime_error); + std::runtime_error("Cannot create timer, group not in node.")); +} + +TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) +{ + auto timer = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_timers->add_timer(timer, callback_group), + std::runtime_error("Failed to notify wait set on timer creation: error not set")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index 86b4e72e36..9b1d4f374e 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -24,6 +24,9 @@ #include "rclcpp/rclcpp.hpp" #include "test_msgs/msg/empty.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + namespace { @@ -77,23 +80,27 @@ class TestNodeTopics : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_topics = + dynamic_cast(node->get_node_topics_interface().get()); + ASSERT_NE(nullptr, node_topics); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeTopics * node_topics; }; TEST_F(TestNodeTopics, add_publisher) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_topics = - dynamic_cast(node->get_node_topics_interface().get()); - ASSERT_NE(nullptr, node_topics); auto publisher = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW(node_topics->add_publisher(publisher, callback_group)); @@ -108,12 +115,20 @@ TEST_F(TestNodeTopics, add_publisher) std::runtime_error); } +TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error) +{ + auto publisher = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_topics->add_publisher(publisher, callback_group), + std::runtime_error("Failed to notify wait set on publisher creation: error not set")); +} + TEST_F(TestNodeTopics, add_subscription) { - std::shared_ptr node = std::make_shared("node", "ns"); - auto * node_topics = - dynamic_cast(node->get_node_topics_interface().get()); - ASSERT_NE(nullptr, node_topics); auto subscription = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW(node_topics->add_subscription(subscription, callback_group)); @@ -127,3 +142,15 @@ TEST_F(TestNodeTopics, add_subscription) node_topics->add_subscription(subscription, callback_group_in_different_node), std::runtime_error); } + +TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error) +{ + auto subscription = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_topics->add_subscription(subscription, callback_group), + std::runtime_error("failed to notify wait set on subscription creation: error not set")); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index 19cc63fe07..b20aff864c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -22,6 +22,9 @@ #include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/rclcpp.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + class TestWaitable : public rclcpp::Waitable { public: @@ -36,23 +39,26 @@ class TestNodeWaitables : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + node_waitables = + dynamic_cast( + node->get_node_waitables_interface().get()); + ASSERT_NE(nullptr, node_waitables); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeWaitables * node_waitables; }; TEST_F(TestNodeWaitables, add_remove_waitable) { - std::shared_ptr node = std::make_shared("node", "ns"); - - auto * node_waitables = - dynamic_cast( - node->get_node_waitables_interface().get()); - ASSERT_NE(nullptr, node_waitables); - std::shared_ptr node2 = std::make_shared("node2", "ns"); auto callback_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -60,9 +66,25 @@ TEST_F(TestNodeWaitables, add_remove_waitable) auto waitable = std::make_shared(); EXPECT_NO_THROW( node_waitables->add_waitable(waitable, callback_group1)); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node_waitables->add_waitable(waitable, callback_group2), - std::runtime_error); + std::runtime_error("Cannot create waitable, group not in node.")); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1)); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2)); + + auto waitable2 = std::make_shared(); + EXPECT_NO_THROW(node_waitables->add_waitable(waitable2, nullptr)); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable2, nullptr)); +} + +TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error) +{ + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto waitable = std::make_shared(); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_waitables->add_waitable(waitable, callback_group), + std::runtime_error("Failed to notify wait set on waitable creation: error not set")); } From 53b0aa9e25a7baba9436d9e5a7f1aab11eed131b Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 22 Sep 2020 16:46:12 -0300 Subject: [PATCH 29/61] Add coverage tests context functions (#1321) * Add basic tests context access * Add expected interrupt_guard get/release * Add mocking utilities to rclcpp * Add tests interrupt_guard_condition * Add tests ini/fini error context * Add destructor test error * Create context directly in block* Use scope exit to clean context Signed-off-by: Jorge Perez --- rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_utilities.cpp | 131 ++++++++++++++++++++++++++ 2 files changed, 132 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 4df68578c3..825792ec3d 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -473,7 +473,7 @@ ament_add_gtest(test_utilities rclcpp/test_utilities.cpp if(TARGET test_utilities) ament_target_dependencies(test_utilities "rcl") - target_link_libraries(test_utilities ${PROJECT_NAME}) + target_link_libraries(test_utilities ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_init rclcpp/test_init.cpp diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index d0e05f9bbb..905ff1a4f0 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -17,9 +17,15 @@ #include #include +#include "rcl/init.h" +#include "rcl/logging.h" + #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/utilities.hpp" +#include "rclcpp/scope_exit.hpp" + +#include "../mocking_utils/patch.hpp" TEST(TestUtilities, remove_ros_arguments) { const char * const argv[] = { @@ -78,3 +84,128 @@ TEST(TestUtilities, multi_init) { EXPECT_FALSE(rclcpp::ok(context1)); EXPECT_FALSE(rclcpp::ok(context2)); } + +TEST(TestUtilities, test_context_basic_access) { + auto context1 = std::make_shared(); + EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); + EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + EXPECT_EQ(std::string{""}, context1->shutdown_reason()); +} + +TEST(TestUtilities, test_context_basic_access_const_methods) { + auto context1 = std::make_shared(); + + EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); + EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + // EXPECT_EQ(std::string{""}, context1->shutdown_reason()); not available for const +} + +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) + +TEST(TestUtilities, test_context_release_interrupt_guard_condition) { + auto context1 = std::make_shared(); + context1->init(0, nullptr); + RCLCPP_SCOPE_EXIT(rclcpp::shutdown(context1);); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, 0, 2, 0, 0, 0, 0, context1->get_rcl_context().get(), + rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret); + + // Expected usage + rcl_guard_condition_t * interrupt_guard_condition = + context1->get_interrupt_guard_condition(&wait_set); + EXPECT_NE(nullptr, interrupt_guard_condition); + EXPECT_NO_THROW(context1->release_interrupt_guard_condition(&wait_set)); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + EXPECT_THROW( + {interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set);}, + rclcpp::exceptions::RCLError); + } + + { + interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + EXPECT_THROW( + {context1->release_interrupt_guard_condition(&wait_set);}, + rclcpp::exceptions::RCLError); + } + + { + interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + EXPECT_NO_THROW({context1->release_interrupt_guard_condition(&wait_set, std::nothrow);}); + } + + { + EXPECT_THROW( + context1->release_interrupt_guard_condition(nullptr), + std::runtime_error); + } + + // Test it works after restore mocks + interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set); + EXPECT_NE(nullptr, interrupt_guard_condition); + EXPECT_NO_THROW(context1->release_interrupt_guard_condition(&wait_set)); +} + + +TEST(TestUtilities, test_context_init_shutdown_fails) { + { + auto context = std::make_shared(); + auto context_fail_init = std::make_shared(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init, RCL_RET_ERROR); + EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError); + } + + { + auto context_fail_init = std::make_shared(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_logging_configure_with_output_handler, RCL_RET_ERROR); + EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW(context->shutdown("")); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_shutdown, RCL_RET_ERROR); + EXPECT_THROW(context->shutdown(""), rclcpp::exceptions::RCLError); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_logging_fini, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW(context->shutdown("")); + } + + { + auto context_to_destroy = std::make_shared(); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW({context_to_destroy.reset();}); + } +} From 6959698947dc32d7d6505dad366b1c6436dbfc4e Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 23 Sep 2020 16:41:18 -0700 Subject: [PATCH 30/61] Add coverage for missing API (except executors) (#1326) * Add coverage for missing API (except executors Signed-off-by: Stephen Brawner * PR Fixup Signed-off-by: Stephen Brawner * Do not check state Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 25 +++++ .../allocator/test_allocator_common.cpp | 67 ++++++++++++ .../allocator/test_allocator_deleter.cpp | 101 ++++++++++++++++++ .../rclcpp/exceptions/test_exceptions.cpp | 58 ++++++++++ .../test/rclcpp/test_future_return_code.cpp | 34 ++++++ rclcpp/test/rclcpp/test_node_options.cpp | 9 ++ rclcpp/test/rclcpp/test_parameter.cpp | 11 ++ rclcpp/test/rclcpp/test_parameter_client.cpp | 23 ++++ rclcpp/test/rclcpp/test_rate.cpp | 21 ++++ rclcpp/test/rclcpp/test_timer.cpp | 17 +++ 10 files changed, 366 insertions(+) create mode 100644 rclcpp/test/rclcpp/allocator/test_allocator_common.cpp create mode 100644 rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp create mode 100644 rclcpp/test/rclcpp/exceptions/test_exceptions.cpp create mode 100644 rclcpp/test/rclcpp/test_future_return_code.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 825792ec3d..8536b3ec50 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -17,6 +17,25 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs SKIP_INSTALL ) +ament_add_gtest( + test_allocator_common + rclcpp/allocator/test_allocator_common.cpp) +if(TARGET test_allocator_common) + target_link_libraries(test_allocator_common ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_allocator_deleter + rclcpp/allocator/test_allocator_deleter.cpp) +if(TARGET test_allocator_deleter) + target_link_libraries(test_allocator_deleter ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_exceptions + rclcpp/exceptions/test_exceptions.cpp) +if(TARGET test_exceptions) + target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) +endif() + # Increasing timeout because connext can take a long time to destroy nodes # TODO(brawner) remove when destroying Node for Connext is resolved. See: # https://github.com/ros2/rclcpp/issues/1250 @@ -102,6 +121,12 @@ if(TARGET test_function_traits) "rosidl_typesupport_cpp" ) endif() +ament_add_gtest( + test_future_return_code + rclcpp/test_future_return_code.cpp) +if(TARGET test_future_return_code) + target_link_libraries(test_future_return_code ${PROJECT_NAME}) +endif() ament_add_gmock(test_intra_process_manager rclcpp/test_intra_process_manager.cpp) if(TARGET test_intra_process_manager) ament_target_dependencies(test_intra_process_manager diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp new file mode 100644 index 0000000000..d3270e8e12 --- /dev/null +++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp @@ -0,0 +1,67 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/allocator/allocator_common.hpp" + +TEST(TestAllocatorCommon, retyped_allocate) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_allocate>(1u, untyped_allocator); + ASSERT_NE(nullptr, allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); + + allocated_mem = allocator.allocate(1); + ASSERT_NE(nullptr, allocated_mem); + void * reallocated_mem = + rclcpp::allocator::retyped_reallocate>( + allocated_mem, 2u, untyped_allocator); + ASSERT_NE(nullptr, reallocated_mem); + + auto code2 = [&untyped_allocator, reallocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + reallocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code2()); +} + +TEST(TestAllocatorCommon, get_rcl_allocator) { + std::allocator allocator; + auto rcl_allocator = rclcpp::allocator::get_rcl_allocator(allocator); + EXPECT_NE(nullptr, rcl_allocator.allocate); + EXPECT_NE(nullptr, rcl_allocator.deallocate); + EXPECT_NE(nullptr, rcl_allocator.reallocate); + EXPECT_NE(nullptr, rcl_allocator.zero_allocate); + // Not testing state as that may or may not be null depending on platform +} + +TEST(TestAllocatorCommon, get_void_rcl_allocator) { + std::allocator allocator; + auto rcl_allocator = + rclcpp::allocator::get_rcl_allocator>(allocator); + EXPECT_NE(nullptr, rcl_allocator.allocate); + EXPECT_NE(nullptr, rcl_allocator.deallocate); + EXPECT_NE(nullptr, rcl_allocator.reallocate); + EXPECT_NE(nullptr, rcl_allocator.zero_allocate); + // Not testing state as that may or may not be null depending on platform +} diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp new file mode 100644 index 0000000000..ae963e9160 --- /dev/null +++ b/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp @@ -0,0 +1,101 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/allocator/allocator_deleter.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + +TEST(TestAllocatorDeleter, construct_destruct) { + std::allocator allocator; + + rclcpp::allocator::AllocatorDeleter> deleter; + EXPECT_EQ(nullptr, deleter.get_allocator()); + deleter.set_allocator(&allocator); + EXPECT_EQ(&allocator, deleter.get_allocator()); + + rclcpp::allocator::AllocatorDeleter> deleter2(&allocator); + EXPECT_EQ(&allocator, deleter2.get_allocator()); + + rclcpp::allocator::AllocatorDeleter> deleter3(deleter2); + EXPECT_EQ(&allocator, deleter3.get_allocator()); +} + +TEST(TestAllocatorDeleter, delete) { + std::allocator allocator; + int * some_mem = allocator.allocate(1u); + ASSERT_NE(nullptr, some_mem); + + rclcpp::allocator::AllocatorDeleter> deleter(&allocator); + EXPECT_NO_THROW(deleter(some_mem)); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_AllocatorDeleter) { + using AllocatorT = std::allocator; + using DeleterT = rclcpp::allocator::AllocatorDeleter; + AllocatorT allocator; + DeleterT deleter(&allocator); + + std::allocator allocator2; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator2); + EXPECT_EQ(&allocator2, deleter.get_allocator()); + + auto throwing_statement = [&allocator]() { + DeleterT * null_del_ptr = nullptr; + rclcpp::allocator::set_allocator_for_deleter( + null_del_ptr, &allocator); + }; + RCLCPP_EXPECT_THROW_EQ( + throwing_statement(), + std::invalid_argument("Argument was NULL to set_allocator_for_deleter")); + + auto throwing_statement2 = [&deleter]() { + AllocatorT * null_alloc_ptr = nullptr; + rclcpp::allocator::set_allocator_for_deleter( + &deleter, null_alloc_ptr); + }; + + RCLCPP_EXPECT_THROW_EQ( + throwing_statement2(), + std::invalid_argument("Argument was NULL to set_allocator_for_deleter")); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_std_default_delete) { + using AllocatorT = std::allocator; + using DeleterT = std::default_delete; + auto not_throwing_statement = []() { + AllocatorT allocator; + DeleterT deleter; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator); + }; + EXPECT_NO_THROW(not_throwing_statement()); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_unexpected_template) { + class SomeAllocatorClass {}; + class SomeDeleterClass {}; + using AllocatorT = SomeAllocatorClass; + using DeleterT = SomeDeleterClass; + auto throwing_statement = []() { + DeleterT deleter; + AllocatorT allocator; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator); + }; + RCLCPP_EXPECT_THROW_EQ( + throwing_statement(), + std::runtime_error("Reached unexpected template specialization")); +} diff --git a/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp b/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp new file mode 100644 index 0000000000..184dfc7a06 --- /dev/null +++ b/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp @@ -0,0 +1,58 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rclcpp/exceptions/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +TEST(TestExceptions, throw_from_rcl_error) { + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, ""), + rclcpp::exceptions::RCLBadAlloc); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, ""), + rclcpp::exceptions::RCLInvalidArgument); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ROS_ARGS, ""), + rclcpp::exceptions::RCLInvalidROSArgsError); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""), + rclcpp::exceptions::RCLError); + + RCLCPP_EXPECT_THROW_EQ( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_OK, ""), + std::invalid_argument("ret is RCL_RET_OK")); + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_get_error_state, nullptr); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""), + std::runtime_error("rcl error state is not set")); + } +} + +TEST(TestExceptions, basic_constructors) { + EXPECT_STREQ("node is invalid", rclcpp::exceptions::InvalidNodeError().what()); + rcl_error_state_t error_state{"error", __FILE__, __LINE__}; + EXPECT_STREQ( + "prefix: error not set", + rclcpp::exceptions::RCLInvalidROSArgsError(RCL_RET_ERROR, &error_state, "prefix: ").what()); +} diff --git a/rclcpp/test/rclcpp/test_future_return_code.cpp b/rclcpp/test/rclcpp/test_future_return_code.cpp new file mode 100644 index 0000000000..f4647b6e29 --- /dev/null +++ b/rclcpp/test/rclcpp/test_future_return_code.cpp @@ -0,0 +1,34 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rclcpp/future_return_code.hpp" + +TEST(TestFutureReturnCode, to_string) { + EXPECT_EQ( + "Unknown enum value (-1)", rclcpp::to_string(rclcpp::FutureReturnCode(-1))); + EXPECT_EQ( + "SUCCESS (0)", rclcpp::to_string(rclcpp::FutureReturnCode::SUCCESS)); + EXPECT_EQ( + "INTERRUPTED (1)", rclcpp::to_string(rclcpp::FutureReturnCode::INTERRUPTED)); + EXPECT_EQ( + "TIMEOUT (2)", rclcpp::to_string(rclcpp::FutureReturnCode::TIMEOUT)); + EXPECT_EQ( + "Unknown enum value (3)", rclcpp::to_string(rclcpp::FutureReturnCode(3))); + EXPECT_EQ( + "Unknown enum value (100)", rclcpp::to_string(rclcpp::FutureReturnCode(100))); +} diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 8543d9b975..d522b72b54 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -198,3 +198,12 @@ TEST(TestNodeOptions, copy) { rcl_arguments_get_count_unparsed(&rcl_options->arguments)); } } + +TEST(TestNodeOptions, append_parameter_override) { + std::vector expected_args{"--unknown-flag", "arg"}; + auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false); + rclcpp::Parameter parameter("some_parameter", 10); + options.append_parameter_override("some_parameter", 10); + EXPECT_EQ(1u, options.parameter_overrides().size()); + EXPECT_EQ(std::string("some_parameter"), options.parameter_overrides()[0].get_name()); +} diff --git a/rclcpp/test/rclcpp/test_parameter.cpp b/rclcpp/test/rclcpp/test_parameter.cpp index f7244e4488..0c93204dcb 100644 --- a/rclcpp/test/rclcpp/test_parameter.cpp +++ b/rclcpp/test/rclcpp/test_parameter.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -32,6 +33,12 @@ class TestParameter : public ::testing::Test } }; +TEST_F(TestParameter, construct_destruct) { + EXPECT_NO_THROW(std::make_shared().reset()); + EXPECT_NO_THROW(std::make_shared("some_parameter").reset()); + EXPECT_NO_THROW(std::make_shared("some_parameter", 10).reset()); +} + TEST_F(TestParameter, not_set_variant) { // Direct instantiation rclcpp::Parameter not_set_variant; @@ -56,6 +63,10 @@ TEST_F(TestParameter, not_set_variant) { EXPECT_EQ( rclcpp::ParameterType::PARAMETER_NOT_SET, rclcpp::Parameter::from_parameter_msg(not_set_param).get_type()); + + EXPECT_THROW( + not_set_variant.get_value(), + rclcpp::exceptions::InvalidParameterTypeException); } TEST_F(TestParameter, bool_variant) { diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 17b44d6490..523cd3fdce 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -19,6 +19,8 @@ #include "rclcpp/rclcpp.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + #include "rcl_interfaces/msg/parameter_event.hpp" class TestParameterClient : public ::testing::Test @@ -159,3 +161,24 @@ TEST_F(TestParameterClient, sync_parameter_event_subscription) { (void)event_sub; } } + +/* + Coverage for simple get_parameter methods + */ +TEST_F(TestParameterClient, sync_parameter_get_parameter) { + rclcpp::SyncParametersClient client(node); + EXPECT_EQ(10, client.get_parameter("not_a_parameter", 10)); + + RCLCPP_EXPECT_THROW_EQ( + client.get_parameter("not_a_parameter"), + std::runtime_error("Parameter 'not_a_parameter' is not set")); +} + +/* + Coverage for async waiting/is_ready + */ +TEST_F(TestParameterClient, sync_parameter_is_ready) { + rclcpp::SyncParametersClient client(node); + EXPECT_TRUE(client.wait_for_service()); + EXPECT_TRUE(client.service_is_ready()); +} diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 5833e02b21..d6608d59f6 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -29,6 +29,7 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); + EXPECT_EQ(period, r.period()); ASSERT_FALSE(r.is_steady()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); @@ -68,6 +69,7 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); + EXPECT_EQ(period, r.period()); ASSERT_TRUE(r.is_steady()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); @@ -98,3 +100,22 @@ TEST(TestRate, wall_rate_basics) { delta = five - four; EXPECT_GT(epsilon, delta); } + +TEST(TestRate, from_double) { + { + rclcpp::WallRate rate(1.0); + EXPECT_EQ(std::chrono::seconds(1), rate.period()); + } + { + rclcpp::WallRate rate(2.0); + EXPECT_EQ(std::chrono::milliseconds(500), rate.period()); + } + { + rclcpp::WallRate rate(0.5); + EXPECT_EQ(std::chrono::seconds(2), rate.period()); + } + { + rclcpp::WallRate rate(4.0); + EXPECT_EQ(std::chrono::milliseconds(250), rate.period()); + } +} diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 7f85a1c4c5..cc568b06c5 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -54,6 +54,7 @@ class TestTimer : public ::testing::Test this->executor->cancel(); } ); + EXPECT_TRUE(timer->is_steady()); executor->add_node(test_node); // don't start spinning, let the test dictate when @@ -191,3 +192,19 @@ TEST_F(TestTimer, test_bad_arguments) { rclcpp::GenericTimer(unitialized_clock, 1us, []() {}, context), rclcpp::exceptions::RCLError); } + +TEST_F(TestTimer, callback_with_timer) { + rclcpp::TimerBase * timer_ptr = nullptr; + timer = test_node->create_wall_timer( + std::chrono::milliseconds(1), + [&timer_ptr](rclcpp::TimerBase & timer) { + timer_ptr = &timer; + }); + auto start = std::chrono::steady_clock::now(); + while (nullptr == timer_ptr && + (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) + { + executor->spin_once(std::chrono::milliseconds(10)); + } + EXPECT_EQ(timer.get(), timer_ptr); +} From bad0460dcfe23ab6b3d4478c5dd294331955d696 Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 24 Sep 2020 13:58:39 -0700 Subject: [PATCH 31/61] Increase coverage of publisher/subscription API (#1325) * Increase coverage of publisher/subscription API Signed-off-by: Stephen Brawner * PR Feedback Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 5 +- rclcpp/test/rclcpp/test_publisher.cpp | 185 +++++++++++++++++++++++ rclcpp/test/rclcpp/test_subscription.cpp | 112 +++++++++++++- 3 files changed, 299 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 8536b3ec50..b6d3ba517c 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -317,12 +317,13 @@ endif() ament_add_gtest(test_publisher rclcpp/test_publisher.cpp) if(TARGET test_publisher) ament_target_dependencies(test_publisher + "rcl" "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" "test_msgs" ) - target_link_libraries(test_publisher ${PROJECT_NAME}) + target_link_libraries(test_publisher ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_publisher_subscription_count_api rclcpp/test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) @@ -404,7 +405,7 @@ if(TARGET test_subscription) "rosidl_typesupport_cpp" "test_msgs" ) - target_link_libraries(test_subscription ${PROJECT_NAME}) + target_link_libraries(test_subscription ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_subscription_publisher_count_api rclcpp/test_subscription_publisher_count_api.cpp) if(TARGET test_subscription_publisher_count_api) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 0ae98cc74f..01a1ee2fde 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -16,11 +16,17 @@ #include #include +#include #include +#include "rcl/publisher.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + #include "test_msgs/msg/empty.hpp" class TestPublisher : public ::testing::Test @@ -259,3 +265,182 @@ TEST_F(TestPublisher, basic_getters) { EXPECT_EQ(publisher, publisher_rmw_gid); } } + +TEST_F(TestPublisher, rcl_publisher_init_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_init, RCL_RET_ERROR); + EXPECT_THROW( + node->create_publisher("topic", 10).reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestPublisher, rcl_publisher_get_rmw_handle_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_get_rmw_handle, nullptr); + RCLCPP_EXPECT_THROW_EQ( + node->create_publisher("topic", 10), + std::runtime_error("failed to get rmw handle: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_get_gid_for_publisher_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_get_gid_for_publisher, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node->create_publisher("topic", 10), + std::runtime_error("failed to get publisher gid: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_fini_error) { + initialize(); + auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_publisher_fini, RCL_RET_ERROR); + auto publisher = node->create_publisher("topic", 10); + ASSERT_EQ(1, publisher.use_count()); + // Failure in rcl_publisher_fini should just log error + EXPECT_NO_THROW(publisher.reset()); +} + +TEST_F(TestPublisher, rcl_publisher_get_options_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_get_options, nullptr); + auto publisher = node->create_publisher("topic", 10); + RCLCPP_EXPECT_THROW_EQ( + publisher->get_queue_size(), + std::runtime_error("failed to get publisher options: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_get_subscription_count_publisher_invalid) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_subscription_count, RCL_RET_PUBLISHER_INVALID); + auto publisher = node->create_publisher("topic", 10); + EXPECT_THROW( + publisher->get_subscription_count(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestPublisher, rcl_publisher_get_actual_qos_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_actual_qos, nullptr); + auto publisher = node->create_publisher("topic", 10); + RCLCPP_EXPECT_THROW_EQ( + publisher->get_actual_qos(), + std::runtime_error("failed to get qos settings: error not set")); +} + +TEST_F(TestPublisher, publishers_equal_rmw_compare_gids_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_compare_gids_equal, RMW_RET_ERROR); + const auto publisher = node->create_publisher("topic", 10); + const rmw_gid_t * gid = nullptr; + auto throwing_fn = [publisher, gid]() + { + // The == operator is expected to throw here, but this lambda avoids unused result warning + return (*publisher.get() == gid) ? true : false; + }; + + RCLCPP_EXPECT_THROW_EQ( + throwing_fn(), + std::runtime_error("failed to compare gids: error not set")); +} + +TEST_F(TestPublisher, intra_process_publish_failures) { + initialize(); + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + auto publisher = node->create_publisher("topic", 10, options); + + auto msg_unique = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_unique))); + + rclcpp::SerializedMessage serialized_msg; + RCLCPP_EXPECT_THROW_EQ( + publisher->publish(serialized_msg), + std::runtime_error("storing serialized messages in intra process is not supported yet")); + + std::allocator allocator; + { + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + RCLCPP_EXPECT_THROW_EQ( + publisher->publish(std::move(loaned_msg)), + std::runtime_error("storing loaned messages in intra process is not supported yet")); + } + + { + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + loaned_msg.release(); + RCLCPP_EXPECT_THROW_EQ( + publisher->publish(std::move(loaned_msg)), + std::runtime_error("loaned message is not valid")); + } +} + +TEST_F(TestPublisher, inter_process_publish_failures) { + initialize(); + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto publisher = node->create_publisher("topic", 10, options); + + auto msg_unique = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_unique))); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publish, RCL_RET_PUBLISHER_INVALID); + test_msgs::msg::Empty msg; + EXPECT_THROW(publisher->publish(msg), rclcpp::exceptions::RCLError); + } + + rclcpp::SerializedMessage serialized_msg; + EXPECT_NO_THROW(publisher->publish(serialized_msg)); + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_serialized_message` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_serialized_message, RCL_RET_ERROR); + EXPECT_THROW(publisher->publish(serialized_msg), rclcpp::exceptions::RCLError); + } + + std::allocator allocator; + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); +} + +template> +class TestPublisherProtectedMethods : public rclcpp::Publisher +{ +public: + using rclcpp::Publisher::Publisher; + + void publish_loaned_message(MessageT * msg) + { + this->do_loaned_message_publish(msg); + } +}; + +TEST_F(TestPublisher, do_loaned_message_publish_error) { + initialize(); + using PublisherT = TestPublisherProtectedMethods>; + auto publisher = + node->create_publisher, PublisherT>("topic", 10); + + auto msg = std::make_shared(); + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_loaned_message` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_loaned_message, RCL_RET_PUBLISHER_INVALID); + EXPECT_THROW(publisher->publish_loaned_message(msg.get()), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestPublisher, run_event_handlers) { + initialize(); + auto publisher = node->create_publisher("topic", 10); + for (const auto & handler : publisher->get_event_handlers()) { + EXPECT_NO_THROW(handler->execute()); + } +} diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index a265f05e17..ef626849a0 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -23,6 +23,9 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + #include "test_msgs/msg/empty.hpp" using namespace std::chrono_literals; @@ -150,7 +153,19 @@ TEST_F(TestSubscription, construction_and_destruction) { (void)msg; }; { - auto sub = node->create_subscription("topic", 10, callback); + constexpr size_t depth = 10u; + auto sub = node->create_subscription("topic", depth, callback); + + EXPECT_NE(nullptr, sub->get_subscription_handle()); + // Converting to base class was necessary for the compiler to choose the const version of + // get_subscription_handle() + const rclcpp::SubscriptionBase * const_sub = sub.get(); + EXPECT_NE(nullptr, const_sub->get_subscription_handle()); + EXPECT_FALSE(sub->use_take_shared_method()); + + EXPECT_NE(nullptr, sub->get_message_type_support_handle().typesupport_identifier); + EXPECT_NE(nullptr, sub->get_message_type_support_handle().data); + EXPECT_EQ(depth, sub->get_actual_qos().get_rmw_qos_profile().depth); } { @@ -229,6 +244,21 @@ TEST_F(TestSubscription, various_creation_signatures) { node, "topic", 42, cb, rclcpp::SubscriptionOptions()); (void)sub; } + { + rclcpp::SubscriptionOptionsWithAllocator> options; + options.allocator = std::make_shared>(); + EXPECT_NE(nullptr, options.get_allocator()); + auto sub = rclcpp::create_subscription( + node, "topic", 42, cb, options); + (void)sub; + } + { + rclcpp::SubscriptionOptionsBase options_base; + rclcpp::SubscriptionOptionsWithAllocator> options(options_base); + auto sub = rclcpp::create_subscription( + node, "topic", 42, cb, options); + (void)sub; + } } /* @@ -329,6 +359,86 @@ TEST_F(TestSubscription, take_serialized) { } } +TEST_F(TestSubscription, rcl_subscription_init_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_init, RCL_RET_TOPIC_NAME_INVALID); + + // reset() is not needed for triggering exception, just to avoid an unused return value warning + EXPECT_THROW( + node->create_subscription("topic", 10, callback).reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_subscription_fini_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_subscription_fini, RCL_RET_ERROR); + + // Cleanup just fails, no exception expected + EXPECT_NO_THROW( + node->create_subscription("topic", 10, callback).reset()); +} + +TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_actual_qos, nullptr); + + auto sub = node->create_subscription("topic", 10, callback); + RCLCPP_EXPECT_THROW_EQ( + sub->get_actual_qos(), std::runtime_error("failed to get qos settings: error not set")); +} + +TEST_F(TestSubscription, rcl_take_type_erased_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take, RCL_RET_ERROR); + + auto sub = node->create_subscription("topic", 10, callback); + test_msgs::msg::Empty msg; + rclcpp::MessageInfo message_info; + + EXPECT_THROW(sub->take_type_erased(&msg, message_info), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_take_serialized_message_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_serialized_message, RCL_RET_ERROR); + + auto sub = node->create_subscription("topic", 10, callback); + rclcpp::SerializedMessage msg; + rclcpp::MessageInfo message_info; + + EXPECT_THROW(sub->take_serialized(msg, message_info), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_subscription_get_publisher_count_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_publisher_count, RCL_RET_ERROR); + + auto sub = node->create_subscription("topic", 10, callback); + EXPECT_THROW(sub->get_publisher_count(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, handle_loaned_message) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto sub = node->create_subscription("topic", 10, callback); + + test_msgs::msg::Empty msg; + rclcpp::MessageInfo message_info; + EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info)); +} + /* Testing subscription with intraprocess enabled and invalid QoS */ From 9aed2ece290a3715e425483a456e7b9f52fb76b9 Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 9 Oct 2020 10:47:43 -0700 Subject: [PATCH 32/61] [foxy backport] Add ostream test for FutureReturnCode (#1327) (#1393) * Remove deprecated executor::FutureReturnCode APIs. (#1327) While we are here, add in another test for the stream operator for future_return_code.cpp Signed-off-by: Chris Lalancette * Revert removing deprecated API Signed-off-by: Stephen Brawner Co-authored-by: Chris Lalancette --- rclcpp/test/rclcpp/test_future_return_code.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_future_return_code.cpp b/rclcpp/test/rclcpp/test_future_return_code.cpp index f4647b6e29..0ad2dcd44b 100644 --- a/rclcpp/test/rclcpp/test_future_return_code.cpp +++ b/rclcpp/test/rclcpp/test_future_return_code.cpp @@ -1,4 +1,4 @@ -// Copyright 2015 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,6 +14,7 @@ #include +#include #include #include "rclcpp/future_return_code.hpp" @@ -32,3 +33,10 @@ TEST(TestFutureReturnCode, to_string) { EXPECT_EQ( "Unknown enum value (100)", rclcpp::to_string(rclcpp::FutureReturnCode(100))); } + +TEST(FutureReturnCode, ostream) { + std::ostringstream ostream; + + ostream << rclcpp::FutureReturnCode::SUCCESS; + ASSERT_EQ("SUCCESS (0)", ostream.str()); +} From a350fd00aed7f7c116694d526094da5e377b97bb Mon Sep 17 00:00:00 2001 From: brawner Date: Mon, 28 Sep 2020 10:39:02 -0700 Subject: [PATCH 33/61] Increase service coverage (#1332) Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_service.cpp | 87 +++++++++++++++++++++++++++++ 2 files changed, 88 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index b6d3ba517c..b9c6049497 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -394,7 +394,7 @@ if(TARGET test_service) "rosidl_typesupport_cpp" "test_msgs" ) - target_link_libraries(test_service ${PROJECT_NAME}) + target_link_libraries(test_service ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_subscription rclcpp/test_subscription.cpp) if(TARGET test_subscription) diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 7ad25e71e5..16ab31cf1b 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -16,10 +16,13 @@ #include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" + #include "rcl_interfaces/srv/list_parameters.hpp" #include "test_msgs/srv/empty.hpp" #include "test_msgs/srv/empty.h" @@ -32,6 +35,11 @@ class TestService : public ::testing::Test rclcpp::init(0, nullptr); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + void SetUp() { node = std::make_shared("my_node", "/ns"); @@ -50,6 +58,12 @@ class TestServiceSub : public ::testing::Test protected: static void SetUpTestCase() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); } void SetUp() @@ -77,6 +91,9 @@ TEST_F(TestService, construction_and_destruction) { }; { auto service = node->create_service("service", callback); + EXPECT_NE(nullptr, service->get_service_handle()); + const rclcpp::ServiceBase * const_service_base = service.get(); + EXPECT_NE(nullptr, const_service_base->get_service_handle()); } { @@ -108,6 +125,25 @@ TEST_F(TestServiceSub, construction_and_destruction) { } } +TEST_F(TestService, construction_and_destruction_rcl_errors) { + auto callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_init, RCL_RET_ERROR); + // reset() isn't necessary for this exception, it just avoids unused return value warning + EXPECT_THROW( + node->create_service("service", callback).reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_service_fini, RCL_RET_ERROR); + EXPECT_NO_THROW(node->create_service("service", callback).reset()); + } +} + /* Testing basic getters */ TEST_F(TestService, basic_public_getters) { using rcl_interfaces::srv::ListParameters; @@ -148,3 +184,54 @@ TEST_F(TestService, basic_public_getters) { node_handle_int->get_node_base_interface()->get_rcl_node_handle())); } } + +TEST_F(TestService, take_request) { + auto callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto server = node->create_service("service", callback); + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_OK); + EXPECT_TRUE(server->take_request(request, *request_id.get())); + } + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_SERVICE_TAKE_FAILED); + EXPECT_FALSE(server->take_request(request, *request_id.get())); + } + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Request request; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_request, RCL_RET_ERROR); + EXPECT_THROW(server->take_request(request, *request_id.get()), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestService, send_response) { + auto callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto server = node->create_service("service", callback); + + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Response response; + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_OK); + EXPECT_NO_THROW(server->send_response(*request_id.get(), response)); + } + + { + auto request_id = server->create_request_header(); + test_msgs::srv::Empty::Response response; + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_response, RCL_RET_ERROR); + EXPECT_THROW( + server->send_response(*request_id.get(), response), + rclcpp::exceptions::RCLError); + } +} From 9e59de053a9c08692994193348ca3ebd6a3361c5 Mon Sep 17 00:00:00 2001 From: brawner Date: Mon, 28 Sep 2020 11:25:43 -0700 Subject: [PATCH 34/61] Add coverage for client API (#1329) * Add coverage for client API Signed-off-by: Stephen Brawner * PR feedback Signed-off-by: Stephen Brawner * PR Feedback Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 3 +- rclcpp/test/rclcpp/test_client.cpp | 205 +++++++++++++++++++++++++++++ 2 files changed, 207 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index b9c6049497..e0ed1dc294 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -79,8 +79,9 @@ if(TARGET test_client) "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" + "test_msgs" ) - target_link_libraries(test_client ${PROJECT_NAME}) + target_link_libraries(test_client ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_create_timer rclcpp/test_create_timer.cpp) if(TARGET test_create_timer) diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index fb3fefdb47..ea64340f03 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -16,12 +16,17 @@ #include #include +#include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" +#include "../mocking_utils/patch.hpp" + +#include "test_msgs/srv/empty.hpp" + class TestClient : public ::testing::Test { protected: @@ -30,6 +35,11 @@ class TestClient : public ::testing::Test rclcpp::init(0, nullptr); } + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + void SetUp() { node = std::make_shared("my_node", "/ns"); @@ -48,6 +58,12 @@ class TestClientSub : public ::testing::Test protected: static void SetUpTestCase() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); } void SetUp() @@ -106,6 +122,38 @@ TEST_F(TestClient, construction_with_free_function) { } } +TEST_F(TestClient, construct_with_rcl_error) { + { + // reset() is not necessary for this exception, but handles unused return value warning + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_init, RCL_RET_ERROR); + EXPECT_THROW( + node->create_client("service").reset(), + rclcpp::exceptions::RCLError); + } + { + // reset() is required for this one + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_client_fini, RCL_RET_ERROR); + EXPECT_NO_THROW(node->create_client("service").reset()); + } +} + +TEST_F(TestClient, wait_for_service) { + const std::string service_name = "service"; + auto client = node->create_client(service_name); + EXPECT_FALSE(client->wait_for_service(std::chrono::nanoseconds(0))); + EXPECT_FALSE(client->wait_for_service(std::chrono::milliseconds(10))); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + auto service = + node->create_service(service_name, std::move(callback)); + + EXPECT_TRUE(client->wait_for_service(std::chrono::nanoseconds(-1))); + EXPECT_TRUE(client->service_is_ready()); +} + /* Testing client construction and destruction for subnodes. */ @@ -123,3 +171,160 @@ TEST_F(TestClientSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidServiceNameError); } } + +class TestClientWithServer : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("node", "ns"); + + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + + service = node->create_service(service_name, std::move(callback)); + } + + ::testing::AssertionResult SendEmptyRequestAndWait( + std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) + { + using SharedFuture = rclcpp::Client::SharedFuture; + + auto client = node->create_client(service_name); + if (!client->wait_for_service()) { + return ::testing::AssertionFailure() << "Waiting for service failed"; + } + + auto request = std::make_shared(); + bool received_response = false; + ::testing::AssertionResult request_result = ::testing::AssertionSuccess(); + auto callback = [&received_response, &request_result](SharedFuture future_response) { + if (nullptr == future_response.get()) { + request_result = ::testing::AssertionFailure() << "Future response was null"; + } + received_response = true; + }; + + auto future = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < timeout) + { + rclcpp::spin_some(node); + } + + if (!received_response) { + return ::testing::AssertionFailure() << "Waiting for response timed out"; + } + + return request_result; + } + + std::shared_ptr node; + std::shared_ptr> service; + const std::string service_name{"empty_service"}; +}; + +TEST_F(TestClientWithServer, async_send_request) { + EXPECT_TRUE(SendEmptyRequestAndWait()); +} + +TEST_F(TestClientWithServer, async_send_request_callback_with_request) { + using SharedFutureWithRequest = + rclcpp::Client::SharedFutureWithRequest; + + auto client = node->create_client(service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + + auto request = std::make_shared(); + bool received_response = false; + auto callback = [&request, &received_response](SharedFutureWithRequest future) { + auto request_response_pair = future.get(); + EXPECT_EQ(request, request_response_pair.first); + EXPECT_NE(nullptr, request_response_pair.second); + received_response = true; + }; + auto future = client->async_send_request(request, std::move(callback)); + + auto start = std::chrono::steady_clock::now(); + while (!received_response && + (std::chrono::steady_clock::now() - start) < std::chrono::seconds(1)) + { + rclcpp::spin_some(node); + } + EXPECT_TRUE(received_response); +} + +TEST_F(TestClientWithServer, async_send_request_rcl_send_request_error) { + // Checking rcl_send_request in rclcpp::Client::async_send_request() + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_send_request, RCL_RET_ERROR); + EXPECT_THROW(SendEmptyRequestAndWait(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestClientWithServer, async_send_request_rcl_service_server_is_available_error) { + { + // Checking rcl_service_server_is_available in rclcpp::ClientBase::service_is_ready + auto client = node->create_client(service_name); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_NODE_INVALID); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } + { + // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready + auto client = node->create_client(service_name); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } + { + // Checking rcl_service_server_is_available exception in rclcpp::ClientBase::service_is_ready + auto client = node->create_client(service_name); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_service_server_is_available, RCL_RET_ERROR); + EXPECT_THROW(client->service_is_ready(), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestClientWithServer, take_response) { + auto client = node->create_client(service_name); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto request = std::make_shared(); + auto request_header = client->create_request_header(); + test_msgs::srv::Empty::Response response; + + client->async_send_request(request); + EXPECT_FALSE(client->take_response(response, *request_header.get())); + + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_OK); + EXPECT_TRUE(client->take_response(response, *request_header.get())); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_CLIENT_TAKE_FAILED); + EXPECT_FALSE(client->take_response(response, *request_header.get())); + } + { + // Checking rcl_take_response in rclcpp::ClientBase::take_type_erased_response + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_response, RCL_RET_ERROR); + EXPECT_THROW( + client->take_response(response, *request_header.get()), + rclcpp::exceptions::RCLError); + } +} From 44b8bf4ed75f92313ae56645ee947abbc64a6eaf Mon Sep 17 00:00:00 2001 From: brawner Date: Fri, 9 Oct 2020 11:33:20 -0700 Subject: [PATCH 35/61] [foxy backport] Add executor unit tests #1336 (#1395) * Improve the error messages in the Executor class. In particular, make sure to use 'throw_from_rcl_error' as much as possible. Signed-off-by: Chris Lalancette * Allow mimick patching of methods with up to 9 arguments. This will be needed by the executor tests. Signed-off-by: Chris Lalancette * Add in unit tests for the Executor class. Signed-off-by: Chris Lalancette * Adjust test_executor for foxy Signed-off-by: Stephen Brawner Co-authored-by: Chris Lalancette --- rclcpp/src/rclcpp/executor.cpp | 22 +- rclcpp/test/CMakeLists.txt | 8 + rclcpp/test/mocking_utils/patch.hpp | 58 ++++- rclcpp/test/rclcpp/test_executor.cpp | 323 +++++++++++++++++++++++++++ 4 files changed, 400 insertions(+), 11 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_executor.cpp diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 5c295f320f..81d03f2af7 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -75,7 +75,7 @@ Executor::Executor(const rclcpp::ExecutorOptions & options) "failed to destroy guard condition: %s", rcl_get_error_string().str); rcl_reset_error(); } - throw std::runtime_error("Failed to create wait set in Executor constructor"); + throw_from_rcl_error(ret, "Failed to create wait set in Executor constructor"); } } @@ -279,8 +279,9 @@ void Executor::cancel() { spinning.store(false); - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); + rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "Failed to trigger guard condition in cancel"); } } @@ -318,8 +319,9 @@ Executor::execute_any_executable(AnyExecutable & any_exec) any_exec.callback_group->can_be_taken_from().store(true); // Wake the wait, because it may need to be recalculated or work that // was previously blocked is now available. - if (rcl_trigger_guard_condition(&interrupt_guard_condition_) != RCL_RET_OK) { - throw std::runtime_error(rcl_get_error_string().str); + rcl_ret_t ret = rcl_trigger_guard_condition(&interrupt_guard_condition_); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "Failed to trigger guard condition from execute_any_executable"); } } @@ -484,19 +486,19 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout) } } // clear wait set - if (rcl_wait_set_clear(&wait_set_) != RCL_RET_OK) { - throw std::runtime_error("Couldn't clear wait set"); + rcl_ret_t ret = rcl_wait_set_clear(&wait_set_); + if (ret != RCL_RET_OK) { + throw_from_rcl_error(ret, "Couldn't clear wait set"); } // The size of waitables are accounted for in size of the other entities - rcl_ret_t ret = rcl_wait_set_resize( + ret = rcl_wait_set_resize( &wait_set_, memory_strategy_->number_of_ready_subscriptions(), memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(), memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(), memory_strategy_->number_of_ready_events()); if (RCL_RET_OK != ret) { - throw std::runtime_error( - std::string("Couldn't resize the wait set : ") + rcl_get_error_string().str); + throw_from_rcl_error(ret, "Couldn't resize the wait set"); } if (!memory_strategy_->add_handles_to_wait_set(&wait_set_)) { diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index e0ed1dc294..bbebbb85c3 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -622,6 +622,14 @@ if(TARGET test_rclcpp_gtest_macros) target_link_libraries(test_rclcpp_gtest_macros ${PROJECT_NAME}) endif() +ament_add_gtest(test_executor rclcpp/test_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}" +) +if(TARGET test_executor) + ament_target_dependencies(test_executor "rcl") + target_link_libraries(test_executor ${PROJECT_NAME} mimick) +endif() + # Install test resources install( DIRECTORY resources diff --git a/rclcpp/test/mocking_utils/patch.hpp b/rclcpp/test/mocking_utils/patch.hpp index b9931d33e6..8f23d543b9 100644 --- a/rclcpp/test/mocking_utils/patch.hpp +++ b/rclcpp/test/mocking_utils/patch.hpp @@ -253,7 +253,7 @@ struct PatchTraits mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); }; -/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6) /// free functions. /** * \tparam ID Numerical identifier of the patch. Ought to be unique. @@ -270,6 +270,62 @@ struct PatchTraits mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6); }; +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, +/// ArgT8, ArgT9) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5, ArgT6, ArgT7, ArgT8, ArgT9); +}; + /// Generic trampoline to wrap generalized callables in plain functions. /** * \tparam ID Numerical identifier of this trampoline. Ought to be unique. diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp new file mode 100644 index 0000000000..49a836a054 --- /dev/null +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -0,0 +1,323 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/executor.hpp" +#include "rclcpp/memory_strategy.hpp" +#include "rclcpp/strategies/allocator_memory_strategy.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +// This file tests the abstract rclcpp::Executor class. For tests of the concrete classes +// that implement this class, please see the test/rclcpp/executors subdirectory. + +class DummyExecutor : public rclcpp::Executor +{ +public: + DummyExecutor() + : rclcpp::Executor() + { + } + + void spin() override + { + } + + void spin_nanoseconds(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node) + { + spin_node_once_nanoseconds(node, std::chrono::milliseconds(100)); + } + + rclcpp::memory_strategy::MemoryStrategy * memory_strategy_ptr() + { + return memory_strategy_.get(); + } + + rclcpp::CallbackGroup::SharedPtr local_get_group_by_timer(rclcpp::TimerBase::SharedPtr timer) + { + return get_group_by_timer(timer); + } +}; + +class TestExecutor : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST_F(TestExecutor, constructor_bad_guard_condition_init) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + new DummyExecutor(), + std::runtime_error( + "Failed to create interrupt guard condition in Executor constructor: error not set")); +} + +TEST_F(TestExecutor, constructor_bad_wait_set_init) { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + new DummyExecutor(), + std::runtime_error("Failed to create wait set in Executor constructor: error not set")); +} + +TEST_F(TestExecutor, spin_node_once_nanoseconds) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_fired = false; + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;}); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(timer_fired); + dummy.spin_nanoseconds(node->get_node_base_interface()); + EXPECT_TRUE(timer_fired); +} + +TEST_F(TestExecutor, spin_node_some) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_fired = false; + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&timer_fired]() {timer_fired = true;}); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(timer_fired); + dummy.spin_node_some(node); + EXPECT_TRUE(timer_fired); +} + +TEST_F(TestExecutor, spin_some_in_spin_some) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_some_in_spin_some = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + try { + dummy.spin_some(std::chrono::milliseconds(1)); + } catch (const std::runtime_error & err) { + if (err.what() == std::string("spin_some() called while already spinning")) { + spin_some_in_spin_some = true; + } + } + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_some_in_spin_some); + dummy.spin_some(std::chrono::milliseconds(1)); + EXPECT_TRUE(spin_some_in_spin_some); +} + +TEST_F(TestExecutor, spin_some_elapsed) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool timer_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + timer_called = true; + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + dummy.spin_some(std::chrono::milliseconds(1)); + + ASSERT_TRUE(timer_called); +} + +TEST_F(TestExecutor, spin_once_in_spin_once) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_once_in_spin_once = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + try { + dummy.spin_once(std::chrono::milliseconds(1)); + } catch (const std::runtime_error & err) { + if (err.what() == std::string("spin_once() called while already spinning")) { + spin_once_in_spin_once = true; + } + } + }); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_once_in_spin_once); + dummy.spin_once(std::chrono::milliseconds(1)); + EXPECT_TRUE(spin_once_in_spin_once); +} + +TEST_F(TestExecutor, cancel_failed_trigger_guard_condition) { + DummyExecutor dummy; + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.cancel(), + std::runtime_error("Failed to trigger guard condition in cancel: error not set")); +} + +TEST_F(TestExecutor, set_memory_strategy_nullptr) { + DummyExecutor dummy; + + RCLCPP_EXPECT_THROW_EQ( + dummy.set_memory_strategy(nullptr), + std::runtime_error("Received NULL memory strategy in executor.")); +} + +TEST_F(TestExecutor, set_memory_strategy) { + DummyExecutor dummy; + rclcpp::memory_strategy::MemoryStrategy::SharedPtr strategy = + std::make_shared< + rclcpp::memory_strategies::allocator_memory_strategy::AllocatorMemoryStrategy<>>(); + + dummy.set_memory_strategy(strategy); + EXPECT_EQ(dummy.memory_strategy_ptr(), strategy.get()); +} + +TEST_F(TestExecutor, spin_once_failed_trigger_guard_condition) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_once(std::chrono::milliseconds(1)), + std::runtime_error( + "Failed to trigger guard condition from execute_any_executable: error not set")); +} + +TEST_F(TestExecutor, spin_some_fail_wait_set_clear) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_some(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't clear wait set: error not set")); +} + +TEST_F(TestExecutor, spin_some_fail_wait_set_resize) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_some(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't resize the wait set: error not set")); +} + +TEST_F(TestExecutor, spin_some_fail_add_handles_to_wait_set) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_subscription, + RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_some(std::chrono::milliseconds(1)), + std::runtime_error("Couldn't fill wait set")); +} + +TEST_F(TestExecutor, spin_some_fail_wait) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}); + + dummy.add_node(node); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + dummy.spin_some(std::chrono::milliseconds(1)), + std::runtime_error("rcl_wait() failed: error not set")); +} + +TEST_F(TestExecutor, get_group_by_timer_nullptr) { + DummyExecutor dummy; + ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(nullptr)); +} + +TEST_F(TestExecutor, get_group_by_timer) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); + dummy.add_node(node); + + ASSERT_EQ(cb_group.get(), dummy.local_get_group_by_timer(timer).get()); +} + +TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + rclcpp::CallbackGroup::SharedPtr cb_group = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto timer = + node->create_wall_timer(std::chrono::milliseconds(1), [&]() {}, cb_group); + dummy.add_node(node); + + cb_group.reset(); + + ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get()); +} From 436f2ce951ae277c592feb86bddbbe501fcaaa80 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 28 Sep 2020 18:52:59 -0300 Subject: [PATCH 36/61] Add coverage tests graph_listener (#1330) * Add file to test graph_listener * Add tests start graph listener * Add tests errors run graph listener * Add tests add/remove node * Remove dynamic cast * Remove repeated line * Remove comment * Add reset to avoid warning * Add checks construction graph listener * Add tests shutdown * Change node_graph definition * Remove test failing MacOS * Remove test not working on Windows Signed-off-by: Jorge Perez --- rclcpp/test/CMakeLists.txt | 5 + rclcpp/test/rclcpp/test_graph_listener.cpp | 293 +++++++++++++++++++++ 2 files changed, 298 insertions(+) create mode 100644 rclcpp/test/rclcpp/test_graph_listener.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index bbebbb85c3..2db9a99b11 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -630,6 +630,11 @@ if(TARGET test_executor) target_link_libraries(test_executor ${PROJECT_NAME} mimick) endif() +ament_add_gtest(test_graph_listener rclcpp/test_graph_listener.cpp) +if(TARGET test_graph_listener) + target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) +endif() + # Install test resources install( DIRECTORY resources diff --git a/rclcpp/test/rclcpp/test_graph_listener.cpp b/rclcpp/test/rclcpp/test_graph_listener.cpp new file mode 100644 index 0000000000..39966c2eda --- /dev/null +++ b/rclcpp/test/rclcpp/test_graph_listener.cpp @@ -0,0 +1,293 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rclcpp/contexts/default_context.hpp" +#include "rclcpp/graph_listener.hpp" +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +namespace +{ + +constexpr char node_name[] = "node"; +constexpr char node_namespace[] = "ns"; +constexpr char shutdown_error_str[] = "GraphListener already shutdown"; + +} // namespace + +class TestGraphListener : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + node_ = std::make_shared(node_name, node_namespace); + + node_graph_ = node_->get_node_graph_interface(); + ASSERT_NE(nullptr, node_graph_); + + graph_listener_ = + std::make_shared( + rclcpp::contexts::get_global_default_context()); + } + + void TearDown() + { + rclcpp::shutdown(); + } + +protected: + std::shared_ptr node() {return node_;} + rclcpp::node_interfaces::NodeGraphInterface * node_graph() {return node_graph_.get();} + std::shared_ptr graph_listener() {return graph_listener_;} + +private: + std::shared_ptr node_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + std::shared_ptr graph_listener_; +}; + +/* Run base class init/shutdown */ +TEST_F(TestGraphListener, construction_and_destruction) { + EXPECT_FALSE(graph_listener()->has_node(node_graph())); + EXPECT_FALSE(graph_listener()->is_shutdown()); +} + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) + +/* Error creating a new graph listener */ +TEST_F(TestGraphListener, error_construct_graph_listener) { + using rclcpp::contexts::get_global_default_context; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + { + auto graph_listener_error = + std::make_shared(get_global_default_context()); + graph_listener_error.reset(); + }, std::runtime_error("failed to create interrupt guard condition: error not set")); +} + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +/* Errors that occur when initializing the graph_listener */ +TEST_F(TestGraphListener, error_start_graph_listener) { + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->start_if_not_started(), + std::runtime_error("failed to initialize wait set: error not set")); + } + { + EXPECT_NO_THROW(graph_listener()->shutdown()); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->start_if_not_started(), + std::runtime_error(shutdown_error_str)); + } +} + +class TestGraphListenerProtectedMethods : public rclcpp::graph_listener::GraphListener +{ +public: + explicit TestGraphListenerProtectedMethods(std::shared_ptr parent_context) + : rclcpp::graph_listener::GraphListener{parent_context} + {} + + void run_protected() + { + this->run(); + } + + void mock_start_thread() + { + // This function prepares the loop thread to be run, but leave + // early with the failure thrown. That way the graph_listener wait_set + // is init, without being started + auto mock_wait_set_init = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_wait_set_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + this->start_if_not_started(), + std::runtime_error("failed to initialize wait set: error not set")); + } +}; + +/* Errors running graph protected methods */ +TEST_F(TestGraphListener, error_run_graph_listener_destroy_context) { + auto context_to_destroy = std::make_shared(); + context_to_destroy->init(0, nullptr); + auto graph_listener_error = + std::make_shared(context_to_destroy); + context_to_destroy.reset(); + EXPECT_NO_THROW(graph_listener_error->run_protected()); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_clear) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_start_thread(); + auto mock_wait_set_clear = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("failed to clear wait set: error not set")); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condition) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_start_thread(); + auto mock_wait_set_clear = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("failed to add interrupt guard condition to wait set: error not set")); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_set_add_guard_condition_twice) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_start_thread(); + auto mock = mocking_utils::patch( + "lib:rclcpp", rcl_wait_set_add_guard_condition, [](auto, ...) { + static int counter = 1; + if (counter == 1) { + counter++; + return RCL_RET_OK; + } else { + return RCL_RET_ERROR; + } + }); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("failed to add shutdown guard condition to wait set: error not set")); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_error) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_start_thread(); + auto mock_wait_set_clear = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("failed to wait on wait set: error not set")); +} + +TEST_F(TestGraphListener, error_run_graph_listener_mock_wait_timeout) { + auto global_context = rclcpp::contexts::get_global_default_context(); + auto graph_listener_test = + std::make_shared(global_context); + graph_listener_test->mock_start_thread(); + auto mock_wait_set_clear = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait, RCL_RET_TIMEOUT); + RCLCPP_EXPECT_THROW_EQ( + graph_listener_test->run_protected(), + std::runtime_error("rcl_wait unexpectedly timed out")); +} + +/* Add/Remove node usage */ +TEST_F(TestGraphListener, test_graph_listener_add_remove_node) { + EXPECT_FALSE(graph_listener()->has_node(node_graph())); + + graph_listener()->add_node(node_graph()); + EXPECT_TRUE(graph_listener()->has_node(node_graph())); + + graph_listener()->remove_node(node_graph()); + EXPECT_FALSE(graph_listener()->has_node(node_graph())); +} + +/* Add/Remove node error usage */ +TEST_F(TestGraphListener, test_errors_graph_listener_add_remove_node) { + // nullptrs tests + EXPECT_FALSE(graph_listener()->has_node(nullptr)); + + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->add_node(nullptr), + std::invalid_argument("node is nullptr")); + + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->remove_node(nullptr), + std::invalid_argument("node is nullptr")); + + // Already added + graph_listener()->add_node(node_graph()); + EXPECT_TRUE(graph_listener()->has_node(node_graph())); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->add_node(node_graph()), + std::runtime_error("node already added")); + + // Remove node not found + graph_listener()->remove_node(node_graph()); + EXPECT_FALSE(graph_listener()->has_node(node_graph())); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->remove_node(node_graph()), + std::runtime_error("node not found")); + + // Add and remove after shutdown + EXPECT_NO_THROW(graph_listener()->shutdown()); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->add_node(node_graph()), + std::runtime_error(shutdown_error_str)); + // Remove works the same + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->remove_node(node_graph()), + std::runtime_error("node not found")); +} + +/* Shutdown errors */ +TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_nothrow) { + graph_listener()->start_if_not_started(); + auto mock_wait_set_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR); + // Exception is logged when using nothrow_t + EXPECT_NO_THROW(graph_listener()->shutdown(std::nothrow_t())); +} + +TEST_F(TestGraphListener, test_graph_listener_shutdown_wait_fini_error_throw) { + graph_listener()->start_if_not_started(); + auto mock_wait_set_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_wait_set_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->shutdown(), + std::runtime_error("failed to finalize wait set: error not set")); +} + +TEST_F(TestGraphListener, test_graph_listener_shutdown_guard_fini_error_throw) { + graph_listener()->start_if_not_started(); + auto mock_wait_set_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + graph_listener()->shutdown(), + std::runtime_error("failed to finalize interrupt guard condition: error not set")); +} From af16b385cd44f3adf44d9fc2a1aa697d9ea3caa0 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 29 Sep 2020 16:01:13 -0400 Subject: [PATCH 37/61] Add in more coverage for expand_topic_or_service_name. (#1346) This gets this file up to 97% coverage. Signed-off-by: Chris Lalancette --- rclcpp/test/CMakeLists.txt | 2 +- .../test_expand_topic_or_service_name.cpp | 148 ++++++++++++++++++ 2 files changed, 149 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 2db9a99b11..53a2da36d7 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -110,7 +110,7 @@ if(TARGET test_expand_topic_or_service_name) "rosidl_runtime_cpp" "rosidl_typesupport_cpp" ) - target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME}) + target_link_libraries(test_expand_topic_or_service_name ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_function_traits rclcpp/test_function_traits.cpp) if(TARGET test_function_traits) diff --git a/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp b/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp index 98c10609b4..37c876dd34 100644 --- a/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp +++ b/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp @@ -14,9 +14,20 @@ #include +#include + +#include "rcl/expand_topic_name.h" +#include "rcl/validate_topic_name.h" +#include "rmw/validate_full_topic_name.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + /* Testing expand_topic_or_service_name. */ @@ -78,3 +89,140 @@ TEST(TestExpandTopicOrServiceName, exceptions) { }, rclcpp::exceptions::InvalidServiceNameError); } } + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_bad_alloc) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_BAD_ALLOC); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::bad_alloc()); +} + +TEST(TestExpandTopicOrServiceName, rcutils_string_map_init_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_map_init, RCUTILS_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("error not set")); +} + +TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_fail) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("error not set")); +} + +TEST(TestExpandTopicOrServiceName, rcl_get_default_topic_name_substitution_and_map_fini_fail) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_default_topic_name_substitutions, RCL_RET_ERROR); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("error not set")); +} + +TEST(TestExpandTopicOrServiceName, rcutils_string_map_fini_fail_bad_alloc) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_map_fini, RCUTILS_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("error not set")); +} + +TEST(TestExpandTopicOrServiceName, rmw_valid_full_topic_name_fail_invalid_argument) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_INVALID_ARGUMENT); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLInvalidArgument( + RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate full topic name")); +} + +TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("topic name unexpectedly valid")); +} + +TEST(TestExpandTopicOrServiceName, rcl_validate_topic_name_fail) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_TOPIC_NAME_INVALID); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_validate_topic_name, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_invalid_argument) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLInvalidArgument( + RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate node name")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_node_name_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to validate node name")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_invalid_argument) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLInvalidArgument( + RCL_RET_INVALID_ARGUMENT, rcl_get_error_state(), "failed to validate namespace")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_namespace_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE); + auto mock2 = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to validate namespace")); +} + +TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError(RCL_RET_ERROR, rcl_get_error_state(), "error not set")); +} + +TEST(TestExpandTopicOrServiceName, rmw_validate_full_topic_name_fail_other) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to validate full topic name")); +} From 5619a4b99f007b1253e8c7fcf1650977d1b2a34a Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 29 Sep 2020 13:39:51 -0700 Subject: [PATCH 38/61] Add tests for node_options API (#1343) * Add tests for node_options API Signed-off-by: Stephen Brawner * Remove c-style casts Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_node_options.cpp | 109 +++++++++++++++++++++++ 2 files changed, 110 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 53a2da36d7..d708473056 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -277,7 +277,7 @@ endif() ament_add_gtest(test_node_options rclcpp/test_node_options.cpp) if(TARGET test_node_options) ament_target_dependencies(test_node_options "rcl") - target_link_libraries(test_node_options ${PROJECT_NAME}) + target_link_libraries(test_node_options ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_init_options rclcpp/test_init_options.cpp) if(TARGET test_init_options) diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index d522b72b54..3fd0a0d52b 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -14,6 +14,7 @@ #include +#include #include #include @@ -23,6 +24,8 @@ #include "rclcpp/node_options.hpp" +#include "../mocking_utils/patch.hpp" + TEST(TestNodeOptions, ros_args_only) { rcl_allocator_t allocator = rcl_get_default_allocator(); @@ -207,3 +210,109 @@ TEST(TestNodeOptions, append_parameter_override) { EXPECT_EQ(1u, options.parameter_overrides().size()); EXPECT_EQ(std::string("some_parameter"), options.parameter_overrides()[0].get_name()); } + +TEST(TestNodeOptions, rcl_node_options_fini_error) { + auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_node_options_fini, RCL_RET_ERROR); + auto options = std::make_shared(); + // Necessary to setup internal pointer + ASSERT_NE(nullptr, options->get_rcl_node_options()); + // If fini fails, this should just log an error and continue + EXPECT_NO_THROW(options.reset()); +} + +TEST(TestNodeOptions, bool_setters_and_getters) { + rclcpp::NodeOptions options; + + options.use_global_arguments(false); + EXPECT_FALSE(options.use_global_arguments()); + EXPECT_FALSE(options.get_rcl_node_options()->use_global_arguments); + options.use_global_arguments(true); + EXPECT_TRUE(options.use_global_arguments()); + EXPECT_TRUE(options.get_rcl_node_options()->use_global_arguments); + + options.enable_rosout(false); + EXPECT_FALSE(options.enable_rosout()); + EXPECT_FALSE(options.get_rcl_node_options()->enable_rosout); + options.enable_rosout(true); + EXPECT_TRUE(options.enable_rosout()); + EXPECT_TRUE(options.get_rcl_node_options()->enable_rosout); + + options.use_intra_process_comms(false); + EXPECT_FALSE(options.use_intra_process_comms()); + options.use_intra_process_comms(true); + EXPECT_TRUE(options.use_intra_process_comms()); + + options.enable_topic_statistics(false); + EXPECT_FALSE(options.enable_topic_statistics()); + options.enable_topic_statistics(true); + EXPECT_TRUE(options.enable_topic_statistics()); + + options.start_parameter_services(false); + EXPECT_FALSE(options.start_parameter_services()); + options.start_parameter_services(true); + EXPECT_TRUE(options.start_parameter_services()); + + options.allow_undeclared_parameters(false); + EXPECT_FALSE(options.allow_undeclared_parameters()); + options.allow_undeclared_parameters(true); + EXPECT_TRUE(options.allow_undeclared_parameters()); + + options.start_parameter_event_publisher(false); + EXPECT_FALSE(options.start_parameter_event_publisher()); + options.start_parameter_event_publisher(true); + EXPECT_TRUE(options.start_parameter_event_publisher()); + + options.automatically_declare_parameters_from_overrides(false); + EXPECT_FALSE(options.automatically_declare_parameters_from_overrides()); + options.automatically_declare_parameters_from_overrides(true); + EXPECT_TRUE(options.automatically_declare_parameters_from_overrides()); +} + +TEST(TestNodeOptions, parameter_event_qos) { + rclcpp::NodeOptions options; + rclcpp::QoS qos1(1); + rclcpp::QoS qos2(2); + EXPECT_NE(qos1, options.parameter_event_qos()); + EXPECT_NE(qos2, options.parameter_event_qos()); + options.parameter_event_qos(qos1); + EXPECT_EQ(qos1, options.parameter_event_qos()); + options.parameter_event_qos(qos2); + EXPECT_EQ(qos2, options.parameter_event_qos()); +} + +TEST(TestNodeOptions, parameter_event_publisher_options) { + rclcpp::NodeOptions options; + rclcpp::PublisherOptionsBase publisher_options; + publisher_options.use_default_callbacks = true; + options.parameter_event_publisher_options(publisher_options); + EXPECT_TRUE(options.parameter_event_publisher_options().use_default_callbacks); + + publisher_options.use_default_callbacks = false; + options.parameter_event_publisher_options(publisher_options); + EXPECT_FALSE(options.parameter_event_publisher_options().use_default_callbacks); +} + +TEST(TestNodeOptions, set_get_allocator) { + rclcpp::NodeOptions options; + EXPECT_NE(nullptr, options.allocator().allocate); + EXPECT_NE(nullptr, options.allocator().deallocate); + EXPECT_NE(nullptr, options.allocator().reallocate); + EXPECT_NE(nullptr, options.allocator().zero_allocate); + + rcl_allocator_t fake_allocator; + fake_allocator.allocate = [](size_t, void *) -> void * {return nullptr;}; + fake_allocator.deallocate = [](void *, void *) {}; + fake_allocator.reallocate = [](void *, size_t, void *) -> void * {return nullptr;}; + fake_allocator.zero_allocate = [](size_t, size_t, void *) -> void * {return nullptr;}; + fake_allocator.state = rcl_get_default_allocator().state; + + options.allocator(fake_allocator); + EXPECT_EQ(fake_allocator.allocate, options.allocator().allocate); + EXPECT_EQ(fake_allocator.deallocate, options.allocator().deallocate); + EXPECT_EQ(fake_allocator.reallocate, options.allocator().reallocate); + EXPECT_EQ(fake_allocator.zero_allocate, options.allocator().zero_allocate); + EXPECT_EQ(fake_allocator.state, options.allocator().state); + + // Check invalid allocator + EXPECT_THROW(options.get_rcl_node_options(), std::bad_alloc); +} From 739db14245263e10e1deb61c95dbd23fe093c841 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 29 Sep 2020 18:23:33 -0400 Subject: [PATCH 39/61] Add in two more tests for expand_topic_or_service_name. (#1350) This gets us to 100% line coverage. Signed-off-by: Chris Lalancette --- .../rclcpp/test_expand_topic_or_service_name.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp b/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp index 37c876dd34..841aa8fd1c 100644 --- a/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp +++ b/rclcpp/test/rclcpp/test_expand_topic_or_service_name.cpp @@ -218,6 +218,22 @@ TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_other) { rclcpp::exceptions::RCLError(RCL_RET_ERROR, rcl_get_error_state(), "error not set")); } +TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_name) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAME); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("invalid rcl node name but valid rmw node name")); +} + +TEST(TestExpandTopicOrServiceName, rcl_expand_topic_name_fail_invalid_node_namespace) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_expand_topic_name, RCL_RET_NODE_INVALID_NAMESPACE); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::expand_topic_or_service_name("chatter", "node", "/ns"), + std::runtime_error("invalid rcl namespace but valid rmw namespace")); +} + TEST(TestExpandTopicOrServiceName, rmw_validate_full_topic_name_fail_other) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rmw_validate_full_topic_name, RMW_RET_ERROR); From ad9dd39597c900e5d102b9cfa44340fd3ff9e39e Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Tue, 29 Sep 2020 18:39:24 -0400 Subject: [PATCH 40/61] Add in more tests for the utilities. (#1349) * Add in more tests for the utilities. Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/utilities.cpp | 2 + rclcpp/test/rclcpp/test_utilities.cpp | 89 ++++++++++++++++++++++++++- 2 files changed, 88 insertions(+), 3 deletions(-) diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 0c4f0cd705..a043fc2db0 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -14,6 +14,8 @@ #include "rclcpp/utilities.hpp" +#include +#include #include #include diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index 905ff1a4f0..3c085c6607 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -14,8 +14,9 @@ #include -#include #include +#include +#include #include "rcl/init.h" #include "rcl/logging.h" @@ -26,6 +27,7 @@ #include "rclcpp/scope_exit.hpp" #include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" TEST(TestUtilities, remove_ros_arguments) { const char * const argv[] = { @@ -46,12 +48,14 @@ TEST(TestUtilities, remove_ros_arguments) { } TEST(TestUtilities, init_with_args) { + EXPECT_FALSE(rclcpp::signal_handlers_installed()); const char * const argv[] = {"process_name"}; int argc = sizeof(argv) / sizeof(const char *); auto other_args = rclcpp::init_and_remove_ros_arguments(argc, argv); ASSERT_EQ(1u, other_args.size()); ASSERT_EQ(std::string{"process_name"}, other_args[0]); + EXPECT_TRUE(rclcpp::signal_handlers_installed()); EXPECT_TRUE(rclcpp::ok()); rclcpp::shutdown(); @@ -97,7 +101,6 @@ TEST(TestUtilities, test_context_basic_access_const_methods) { EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); - // EXPECT_EQ(std::string{""}, context1->shutdown_reason()); not available for const } MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) @@ -158,7 +161,6 @@ TEST(TestUtilities, test_context_release_interrupt_guard_condition) { EXPECT_NO_THROW(context1->release_interrupt_guard_condition(&wait_set)); } - TEST(TestUtilities, test_context_init_shutdown_fails) { { auto context = std::make_shared(); @@ -209,3 +211,84 @@ TEST(TestUtilities, test_context_init_shutdown_fails) { EXPECT_NO_THROW({context_to_destroy.reset();}); } } + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST(TestUtilities, remove_ros_arguments_rcl_parse_arguments_failed) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_parse_arguments, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::remove_ros_arguments(argc, argv), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to parse arguments")); +} + +TEST(TestUtilities, remove_ros_arguments_rcl_remove_ros_arguments_failed) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_remove_ros_arguments, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::remove_ros_arguments(argc, argv), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "")); +} + +TEST(TestUtilities, remove_ros_arguments_rcl_remove_ros_arguments_failed_and_fini) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_remove_ros_arguments, RCL_RET_ERROR); + auto mock2 = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_arguments_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::remove_ros_arguments(argc, argv), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), + ", failed also to cleanup parsed arguments, leaking memory: ")); +} + +TEST(TestUtilities, remove_ros_arguments_rcl_arguments_fini_failed) { + const char * const argv[] = { + "process_name", + "-d", "--ros-args", + "-r", "__ns:=/foo/bar", + "-r", "__ns:=/fiz/buz", + "--", "--foo=bar", "--baz" + }; + int argc = sizeof(argv) / sizeof(const char *); + + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_arguments_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::remove_ros_arguments(argc, argv), + rclcpp::exceptions::RCLError( + RCL_RET_ERROR, rcl_get_error_state(), "failed to cleanup parsed arguments, leaking memory")); +} From d0064529ecc7f7b03871cac0bdfdaf7607fcfd81 Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 29 Sep 2020 16:10:44 -0700 Subject: [PATCH 41/61] Complete coverage of Parameter and ParameterValue API (#1344) * Complete coverage of Parameter and ParameterValue API Signed-off-by: Stephen Brawner * Adding comments Signed-off-by: Stephen Brawner --- rclcpp/test/rclcpp/test_parameter.cpp | 169 ++++++++++++++++++++++++++ 1 file changed, 169 insertions(+) diff --git a/rclcpp/test/rclcpp/test_parameter.cpp b/rclcpp/test/rclcpp/test_parameter.cpp index 0c93204dcb..0d798dc451 100644 --- a/rclcpp/test/rclcpp/test_parameter.cpp +++ b/rclcpp/test/rclcpp/test_parameter.cpp @@ -44,6 +44,9 @@ TEST_F(TestParameter, not_set_variant) { rclcpp::Parameter not_set_variant; EXPECT_EQ(rclcpp::PARAMETER_NOT_SET, not_set_variant.get_type()); EXPECT_EQ("not set", not_set_variant.get_type_name()); + std::stringstream ss; + ss << not_set_variant; + EXPECT_EQ("{\"name\": \"\", \"type\": \"not set\", \"value\": \"not set\"}", ss.str()); EXPECT_THROW(not_set_variant.as_bool(), rclcpp::ParameterTypeException); EXPECT_THROW(not_set_variant.as_int(), rclcpp::ParameterTypeException); @@ -67,6 +70,13 @@ TEST_F(TestParameter, not_set_variant) { EXPECT_THROW( not_set_variant.get_value(), rclcpp::exceptions::InvalidParameterTypeException); + + // Check == and != operators work as expected + EXPECT_EQ(not_set_variant, not_set_variant); + rclcpp::Parameter not_set_variant2; + EXPECT_EQ(not_set_variant, not_set_variant2); + rclcpp::Parameter bool_variant("bool_param", true); + EXPECT_NE(not_set_variant, bool_variant); } TEST_F(TestParameter, bool_variant) { @@ -92,6 +102,10 @@ TEST_F(TestParameter, bool_variant) { EXPECT_THROW(bool_variant_true.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ("true", bool_variant_true.value_to_string()); + std::stringstream ss; + ss << bool_variant_true; + EXPECT_EQ("{\"name\": \"bool_param\", \"type\": \"bool\", \"value\": \"true\"}", ss.str()); + rclcpp::Parameter bool_variant_false("bool_param", false); EXPECT_FALSE(bool_variant_false.get_value()); @@ -125,6 +139,11 @@ TEST_F(TestParameter, bool_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, bool_variant_false.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(bool_variant_true, bool_variant_true); + EXPECT_NE(bool_variant_false, bool_variant_true); + EXPECT_EQ(bool_variant_true, from_msg_true); } TEST_F(TestParameter, integer_variant) { @@ -154,6 +173,9 @@ TEST_F(TestParameter, integer_variant) { EXPECT_THROW(integer_variant.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ("42", integer_variant.value_to_string()); + std::stringstream ss; + ss << integer_variant; + EXPECT_EQ("{\"name\": \"integer_param\", \"type\": \"integer\", \"value\": \"42\"}", ss.str()); rcl_interfaces::msg::Parameter integer_param = integer_variant.to_parameter_msg(); EXPECT_EQ("integer_param", integer_param.name); @@ -173,6 +195,11 @@ TEST_F(TestParameter, integer_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, from_msg.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(integer_variant, integer_variant); + EXPECT_NE(integer_variant, rclcpp::Parameter("integer_param", 41)); + EXPECT_EQ(integer_variant, from_msg); } TEST_F(TestParameter, long_integer_variant) { @@ -202,6 +229,12 @@ TEST_F(TestParameter, long_integer_variant) { EXPECT_THROW(long_variant.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ("9223372036854775807", long_variant.value_to_string()); + std::stringstream ss; + ss << long_variant; + EXPECT_EQ( + "{\"name\": \"long_integer_param\", \"type\": \"integer\", \"value\": " + "\"9223372036854775807\"}", + ss.str()); rcl_interfaces::msg::Parameter integer_param = long_variant.to_parameter_msg(); EXPECT_EQ("long_integer_param", integer_param.name); @@ -221,6 +254,12 @@ TEST_F(TestParameter, long_integer_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, from_msg.get_value_message().type); + + + // Check == and != operators work as expected + EXPECT_EQ(long_variant, long_variant); + EXPECT_NE(long_variant, rclcpp::Parameter("long_integer_param", 41)); + EXPECT_EQ(long_variant, from_msg); } TEST_F(TestParameter, float_variant) { @@ -250,6 +289,10 @@ TEST_F(TestParameter, float_variant) { EXPECT_THROW(float_variant.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ("42.000000", float_variant.value_to_string()); + std::stringstream ss; + ss << float_variant; + EXPECT_EQ( + "{\"name\": \"float_param\", \"type\": \"double\", \"value\": \"42.000000\"}", ss.str()); rcl_interfaces::msg::Parameter float_param = float_variant.to_parameter_msg(); EXPECT_EQ("float_param", float_param.name); @@ -269,6 +312,11 @@ TEST_F(TestParameter, float_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, from_msg.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(float_variant, float_variant); + EXPECT_NE(float_variant, rclcpp::Parameter("float_param", 41.0)); + EXPECT_EQ(float_variant, from_msg); } TEST_F(TestParameter, double_variant) { @@ -298,6 +346,10 @@ TEST_F(TestParameter, double_variant) { EXPECT_THROW(double_variant.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ("-42.100000", double_variant.value_to_string()); + std::stringstream ss; + ss << double_variant; + EXPECT_EQ( + "{\"name\": \"double_param\", \"type\": \"double\", \"value\": \"-42.100000\"}", ss.str()); rcl_interfaces::msg::Parameter double_param = double_variant.to_parameter_msg(); EXPECT_EQ("double_param", double_param.name); @@ -317,6 +369,11 @@ TEST_F(TestParameter, double_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, from_msg.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(double_variant, double_variant); + EXPECT_NE(double_variant, rclcpp::Parameter("double_param", -41.2)); + EXPECT_EQ(double_variant, from_msg); } TEST_F(TestParameter, string_variant) { @@ -346,6 +403,9 @@ TEST_F(TestParameter, string_variant) { EXPECT_THROW(string_variant.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ(TEST_VALUE, string_variant.value_to_string()); + std::stringstream ss; + ss << string_variant; + EXPECT_EQ("{\"name\": \"string_param\", \"type\": \"string\", \"value\": \"ROS2\"}", ss.str()); rcl_interfaces::msg::Parameter string_param = string_variant.to_parameter_msg(); EXPECT_EQ("string_param", string_param.name); @@ -363,6 +423,11 @@ TEST_F(TestParameter, string_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_STRING, from_msg.get_value_message().type); + + // Check == and != operators work as expected + EXPECT_EQ(string_variant, string_variant); + EXPECT_NE(string_variant, rclcpp::Parameter("string_param", "ROS1")); + EXPECT_EQ(string_variant, from_msg); } TEST_F(TestParameter, byte_array_variant) { @@ -392,6 +457,12 @@ TEST_F(TestParameter, byte_array_variant) { EXPECT_THROW(byte_array_variant.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ("[0x52, 0x4f, 0x53, 0x32]", byte_array_variant.value_to_string()); + std::stringstream ss; + ss << byte_array_variant; + EXPECT_EQ( + "{\"name\": \"byte_array_param\", \"type\": \"byte_array\", \"value\": " + "\"[0x52, 0x4f, 0x53, 0x32]\"}", + ss.str()); rcl_interfaces::msg::Parameter byte_array_param = byte_array_variant.to_parameter_msg(); EXPECT_EQ("byte_array_param", byte_array_param.name); @@ -411,6 +482,12 @@ TEST_F(TestParameter, byte_array_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2 {0x1, 0x2, 0x3, 0x4}; + EXPECT_EQ(byte_array_variant, byte_array_variant); + EXPECT_NE(byte_array_variant, rclcpp::Parameter("byte_array_param", TEST_VALUE2)); + EXPECT_EQ(byte_array_variant, from_msg); } TEST_F(TestParameter, bool_array_variant) { @@ -440,6 +517,12 @@ TEST_F(TestParameter, bool_array_variant) { EXPECT_THROW(bool_array_variant.as_string_array(), rclcpp::ParameterTypeException); EXPECT_EQ("[false, true, true, false, false, true]", bool_array_variant.value_to_string()); + std::stringstream ss; + ss << bool_array_variant; + EXPECT_EQ( + "{\"name\": \"bool_array_param\", \"type\": \"bool_array\", \"value\": " + "\"[false, true, true, false, false, true]\"}", + ss.str()); rcl_interfaces::msg::Parameter bool_array_param = bool_array_variant.to_parameter_msg(); EXPECT_EQ("bool_array_param", bool_array_param.name); @@ -459,6 +542,12 @@ TEST_F(TestParameter, bool_array_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2 {true, true, true}; + EXPECT_EQ(bool_array_variant, bool_array_variant); + EXPECT_NE(bool_array_variant, rclcpp::Parameter("bool_array_param", TEST_VALUE2)); + EXPECT_EQ(bool_array_variant, from_msg); } TEST_F(TestParameter, integer_array_variant) { @@ -506,6 +595,12 @@ TEST_F(TestParameter, integer_array_variant) { EXPECT_EQ( "[42, -99, 2147483647, -2147483648, 0]", integer_array_variant.value_to_string()); + std::stringstream ss; + ss << integer_array_variant; + EXPECT_EQ( + "{\"name\": \"integer_array_param\", \"type\": \"integer_array\", \"value\": " + "\"[42, -99, 2147483647, -2147483648, 0]\"}", + ss.str()); rcl_interfaces::msg::Parameter integer_array_param = integer_array_variant.to_parameter_msg(); EXPECT_EQ("integer_array_param", integer_array_param.name); @@ -538,6 +633,12 @@ TEST_F(TestParameter, integer_array_variant) { EXPECT_EQ( from_msg.get_value_message().type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{1, 2, 3, 4, 5}; + EXPECT_EQ(integer_array_variant, integer_array_variant); + EXPECT_NE(integer_array_variant, rclcpp::Parameter("integer_array_param", TEST_VALUE2)); + EXPECT_EQ(integer_array_variant, from_msg); } TEST_F(TestParameter, long_integer_array_variant) { @@ -571,6 +672,12 @@ TEST_F(TestParameter, long_integer_array_variant) { EXPECT_EQ( "[42, -99, 9223372036854775807, -9223372036854775808, 0]", long_array_variant.value_to_string()); + std::stringstream ss; + ss << long_array_variant; + EXPECT_EQ( + "{\"name\": \"long_integer_array_param\", \"type\": \"integer_array\", \"value\": " + "\"[42, -99, 9223372036854775807, -9223372036854775808, 0]\"}", + ss.str()); rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter_msg(); EXPECT_EQ("long_integer_array_param", integer_array_param.name); @@ -592,6 +699,12 @@ TEST_F(TestParameter, long_integer_array_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{1, 2, 3, 4, 5}; + EXPECT_EQ(long_array_variant, long_array_variant); + EXPECT_NE(long_array_variant, rclcpp::Parameter("long_integer_array_param", TEST_VALUE2)); + EXPECT_EQ(long_array_variant, from_msg); } TEST_F(TestParameter, float_array_variant) { @@ -639,6 +752,12 @@ TEST_F(TestParameter, float_array_variant) { EXPECT_EQ( "[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]", float_array_variant.value_to_string()); + std::stringstream ss; + ss << float_array_variant; + EXPECT_EQ( + "{\"name\": \"float_array_param\", \"type\": \"double_array\", \"value\": " + "\"[42.1, -99.1, 3.40282e+38, -3.40282e+38, 0.1]\"}", + ss.str()); rcl_interfaces::msg::Parameter float_array_param = float_array_variant.to_parameter_msg(); EXPECT_EQ("float_array_param", float_array_param.name); @@ -671,6 +790,12 @@ TEST_F(TestParameter, float_array_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{1.0, 2.0, 3.0, 4.0}; + EXPECT_EQ(float_array_variant, float_array_variant); + EXPECT_NE(float_array_variant, rclcpp::Parameter("float_array_param", TEST_VALUE2)); + EXPECT_EQ(float_array_variant, from_msg); } TEST_F(TestParameter, double_array_variant) { @@ -704,6 +829,12 @@ TEST_F(TestParameter, double_array_variant) { EXPECT_EQ( "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]", double_array_variant.value_to_string()); + std::stringstream ss; + ss << double_array_variant; + EXPECT_EQ( + "{\"name\": \"double_array_param\", \"type\": \"double_array\", \"value\": " + "\"[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]\"}", + ss.str()); rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter_msg(); EXPECT_EQ("double_array_param", double_array_param.name); @@ -725,6 +856,12 @@ TEST_F(TestParameter, double_array_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{1.0, 2.0, 3.0, 4.0}; + EXPECT_EQ(double_array_variant, double_array_variant); + EXPECT_NE(double_array_variant, rclcpp::Parameter("double_array_param", TEST_VALUE2)); + EXPECT_EQ(double_array_variant, from_msg); } TEST_F(TestParameter, string_array_variant) { @@ -756,6 +893,11 @@ TEST_F(TestParameter, string_array_variant) { EXPECT_THROW(string_array_variant.as_double_array(), rclcpp::ParameterTypeException); EXPECT_EQ("[R, O, S2]", string_array_variant.value_to_string()); + std::stringstream ss; + ss << string_array_variant; + EXPECT_EQ( + "{\"name\": \"string_array_param\", \"type\": \"string_array\", \"value\": \"[R, O, S2]\"}", + ss.str()); rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter_msg(); EXPECT_EQ("string_array_param", string_array_param.name); @@ -777,4 +919,31 @@ TEST_F(TestParameter, string_array_variant) { EXPECT_EQ( rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, from_msg.get_value_message().type); + + // Check == and != operators work as expected + const std::vector TEST_VALUE2{"R", "O", "S1"}; + EXPECT_EQ(string_array_variant, string_array_variant); + EXPECT_NE(string_array_variant, rclcpp::Parameter("string_array_param", TEST_VALUE2)); + EXPECT_EQ(string_array_variant, from_msg); +} + +TEST_F(TestParameter, parameter_vector_stringification) { + const std::vector parameters = { + rclcpp::Parameter(), + rclcpp::Parameter("bool_param", true), + rclcpp::Parameter("integer_param", 42), + rclcpp::Parameter("double_param", 3.14159), + rclcpp::Parameter("string_param", "I'm a string"), + }; + + std::stringstream ss; + ss << parameters; + EXPECT_EQ( + "{" + "\"\": {\"type\": \"not set\", \"value\": \"not set\"}, " + "\"bool_param\": {\"type\": \"bool\", \"value\": \"true\"}, " + "\"integer_param\": {\"type\": \"integer\", \"value\": \"42\"}, " + "\"double_param\": {\"type\": \"double\", \"value\": \"3.141590\"}, " + "\"string_param\": {\"type\": \"string\", \"value\": \"I'm a string\"}}", + ss.str()); } From 1cba93b9eacbd86ced9e838cf351bd49fd2a56d1 Mon Sep 17 00:00:00 2001 From: brawner Date: Tue, 29 Sep 2020 16:42:18 -0700 Subject: [PATCH 42/61] Test the remaining node public API (#1342) * Test the remaining node public API Signed-off-by: Stephen Brawner * Address PR feedback Signed-off-by: Stephen Brawner * Add comment Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_node.cpp | 139 ++++++++++++++++++++++++++++--- 2 files changed, 129 insertions(+), 12 deletions(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index d708473056..004979461c 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -188,7 +188,7 @@ if(TARGET test_node) "rosidl_typesupport_cpp" "test_msgs" ) - target_link_libraries(test_node ${PROJECT_NAME}) + target_link_libraries(test_node ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__get_node_interfaces diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index f5cd2a2ae9..2c52b5264d 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -26,7 +26,14 @@ #include "rclcpp/rclcpp.hpp" #include "rcpputils/filesystem_helper.hpp" + +#include "rmw/validate_namespace.h" + #include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/msg/empty.hpp" +#include "test_msgs/srv/empty.hpp" + +#include "../mocking_utils/patch.hpp" class TestNode : public ::testing::Test { @@ -50,7 +57,19 @@ class TestNode : public ::testing::Test TEST_F(TestNode, construction_and_destruction) { { auto node = std::make_shared("my_node", "/ns"); - (void)node; + EXPECT_NE(nullptr, node->get_node_base_interface()); + EXPECT_NE(nullptr, node->get_node_clock_interface()); + EXPECT_NE(nullptr, node->get_node_graph_interface()); + EXPECT_NE(nullptr, node->get_node_logging_interface()); + EXPECT_NE(nullptr, node->get_node_time_source_interface()); + EXPECT_NE(nullptr, node->get_node_timers_interface()); + EXPECT_NE(nullptr, node->get_node_topics_interface()); + EXPECT_NE(nullptr, node->get_node_services_interface()); + EXPECT_NE(nullptr, node->get_node_parameters_interface()); + EXPECT_NE(nullptr, node->get_node_waitables_interface()); + EXPECT_NE(nullptr, node->get_node_options().get_rcl_node_options()); + EXPECT_NE(nullptr, node->get_graph_event()); + EXPECT_NE(nullptr, node->get_clock()); } { @@ -279,8 +298,11 @@ TEST_F(TestNode, get_logger) { TEST_F(TestNode, get_clock) { auto node = std::make_shared("my_node", "/ns"); auto ros_clock = node->get_clock(); - EXPECT_TRUE(ros_clock != nullptr); + EXPECT_NE(nullptr, ros_clock); EXPECT_EQ(ros_clock->get_clock_type(), RCL_ROS_TIME); + + const rclcpp::Node & const_node = *node.get(); + EXPECT_NE(nullptr, const_node.get_clock()); } TEST_F(TestNode, now) { @@ -785,15 +807,24 @@ TEST_F(TestNode, undeclare_parameter) { TEST_F(TestNode, has_parameter) { auto node = std::make_shared("test_has_parameter_node"_unq); - { - // normal use - auto name = "parameter"_unq; - EXPECT_FALSE(node->has_parameter(name)); - node->declare_parameter(name); - EXPECT_TRUE(node->has_parameter(name)); - node->undeclare_parameter(name); - EXPECT_FALSE(node->has_parameter(name)); - } + // normal use + auto name = "parameter"_unq; + EXPECT_FALSE(node->has_parameter(name)); + node->declare_parameter(name); + EXPECT_TRUE(node->has_parameter(name)); + node->undeclare_parameter(name); + EXPECT_FALSE(node->has_parameter(name)); +} + +TEST_F(TestNode, list_parameters) { + auto node = std::make_shared("test_list_parameter_node"_unq); + // normal use + auto name = "parameter"_unq; + const size_t before_size = node->list_parameters({}, 1u).names.size(); + node->declare_parameter(name); + EXPECT_EQ(1u + before_size, node->list_parameters({}, 1u).names.size()); + node->undeclare_parameter(name); + EXPECT_EQ(before_size, node->list_parameters({}, 1u).names.size()); } TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { @@ -2655,3 +2686,89 @@ TEST_F(TestNode, get_publishers_subscriptions_info_by_topic) { subscription_list = node->get_subscriptions_info_by_topic("13"); }, rclcpp::exceptions::InvalidTopicNameError); } + +TEST_F(TestNode, callback_groups) { + auto node = std::make_shared("node", "ns"); + size_t num_callback_groups_in_basic_node = node->get_callback_groups().size(); + + auto group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + EXPECT_EQ(1u + num_callback_groups_in_basic_node, node->get_callback_groups().size()); + + auto group2 = node->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + EXPECT_EQ(2u + num_callback_groups_in_basic_node, node->get_callback_groups().size()); +} + +// This is tested more thoroughly in node_interfaces/test_node_graph +TEST_F(TestNode, get_entity_names) { + auto node = std::make_shared("node", "ns"); + const auto node_names = node->get_node_names(); + EXPECT_NE( + node_names.end(), + std::find(node_names.begin(), node_names.end(), node->get_fully_qualified_name())); + + const auto topic_names_and_types = node->get_topic_names_and_types(); + EXPECT_EQ(topic_names_and_types.end(), topic_names_and_types.find("topic")); + + EXPECT_EQ(0u, node->count_publishers("topic")); + EXPECT_EQ(0u, node->count_subscribers("topic")); + + const auto service_names_and_types = node->get_service_names_and_types(); + EXPECT_EQ(service_names_and_types.end(), service_names_and_types.find("service")); + + const auto service_names_and_types_by_node = + node->get_service_names_and_types_by_node("node", "/ns"); + EXPECT_EQ( + service_names_and_types_by_node.end(), + service_names_and_types_by_node.find("service")); +} + +TEST_F(TestNode, wait_for_graph_event) { + // Even though this node is only used in the std::thread below, it's here to ensure there is no + // race condition in its destruction and modification of the node_graph + auto node = std::make_shared("node", "ns"); + + constexpr std::chrono::seconds timeout(10); + auto thread_start = std::chrono::steady_clock::now(); + auto thread_completion = thread_start; + + // This runs until the graph is updated + std::thread graph_event_wait_thread([&thread_completion, node, timeout]() { + auto event = node->get_graph_event(); + EXPECT_NO_THROW(node->wait_for_graph_change(event, timeout)); + thread_completion = std::chrono::steady_clock::now(); + }); + + // Start creating nodes until at least one event triggers in graph_event_wait_thread or until 100 + // nodes have been created (at which point this is a failure) + std::vector> nodes; + while (thread_completion == thread_start && nodes.size() < 100) { + nodes.emplace_back(std::make_shared("node"_unq, "ns")); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + graph_event_wait_thread.join(); + // Nodes will probably only be of size 1 + EXPECT_LT(0u, nodes.size()); + EXPECT_GT(100u, nodes.size()); + EXPECT_NE(thread_start, thread_completion); + EXPECT_GT(timeout, thread_completion - thread_start); +} + +TEST_F(TestNode, create_sub_node_rmw_validate_namespace_error) { + auto node = std::make_shared("node", "ns"); + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT); + + // reset() is not necessary for this exception, but it handles unused return value warning + EXPECT_THROW( + node->create_sub_node("ns").reset(), + rclcpp::exceptions::RCLInvalidArgument); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR); + EXPECT_THROW( + node->create_sub_node("ns").reset(), + rclcpp::exceptions::RCLError); + } +} From a339d73235181a33930e8f30a93ef63436ea6482 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Sep 2020 08:17:01 -0400 Subject: [PATCH 43/61] Add in more tests for init_options coverage. (#1353) Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/init_options.cpp | 2 +- rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_init_options.cpp | 36 ++++++++++++++++++++++++ 3 files changed, 38 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/init_options.cpp b/rclcpp/src/rclcpp/init_options.cpp index f9fd6f5f60..f3b80fba1b 100644 --- a/rclcpp/src/rclcpp/init_options.cpp +++ b/rclcpp/src/rclcpp/init_options.cpp @@ -26,7 +26,7 @@ InitOptions::InitOptions(rcl_allocator_t allocator) *init_options_ = rcl_get_zero_initialized_init_options(); rcl_ret_t ret = rcl_init_options_init(init_options_.get(), allocator); if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialized rcl init options"); + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to initialize rcl init options"); } } diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 004979461c..5540a96459 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -282,7 +282,7 @@ endif() ament_add_gtest(test_init_options rclcpp/test_init_options.cpp) if(TARGET test_init_options) ament_target_dependencies(test_init_options "rcl") - target_link_libraries(test_init_options ${PROJECT_NAME}) + target_link_libraries(test_init_options ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_parameter_client rclcpp/test_parameter_client.cpp) if(TARGET test_parameter_client) diff --git a/rclcpp/test/rclcpp/test_init_options.cpp b/rclcpp/test/rclcpp/test_init_options.cpp index 3c0cae739a..1e02084c46 100644 --- a/rclcpp/test/rclcpp/test_init_options.cpp +++ b/rclcpp/test/rclcpp/test_init_options.cpp @@ -14,6 +14,7 @@ #include +#include #include #include @@ -22,6 +23,8 @@ #include "rclcpp/init_options.hpp" +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" TEST(TestInitOptions, test_construction) { rcl_allocator_t allocator = rcl_get_default_allocator(); @@ -61,3 +64,36 @@ TEST(TestInitOptions, test_initialize_logging) { EXPECT_FALSE(options.auto_initialize_logging()); } } + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST(TestInitOptions, constructor_rcl_init_options_init_failed) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init_options_init, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::InitOptions(), + std::runtime_error("failed to initialize rcl init options: error not set")); +} + +TEST(TestInitOptions, constructor_rcl_init_options_copy_failed) { + rcl_init_options_t rcl_opts; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init_options_copy, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + new rclcpp::InitOptions(rcl_opts), + std::runtime_error("failed to copy rcl init options: error not set")); +} + +TEST(TestInitOptions, copy_constructor_rcl_init_options_copy_failed) { + rclcpp::InitOptions options; + rclcpp::InitOptions options2; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init_options_copy, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + options2.operator=(options), + std::runtime_error("failed to copy rcl init options: error not set")); +} From 65b907f4f00492307f24372dbf2fdc4dd7123a25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 30 Sep 2020 16:15:35 +0200 Subject: [PATCH 44/61] Improved test_subscription_options (#1358) * Improved test_subscription_options Signed-off-by: ahcorde * used RCLCPP_EXPECT_THROW_EQ in test_subcription_options Signed-off-by: ahcorde * make linters happy Signed-off-by: ahcorde --- rclcpp/test/rclcpp/test_subscription_options.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/rclcpp/test/rclcpp/test_subscription_options.cpp b/rclcpp/test/rclcpp/test_subscription_options.cpp index ee24685dca..c4cfb6b4c4 100644 --- a/rclcpp/test/rclcpp/test_subscription_options.cpp +++ b/rclcpp/test/rclcpp/test_subscription_options.cpp @@ -23,6 +23,8 @@ #include "rclcpp/node_options.hpp" #include "rclcpp/subscription_options.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + using namespace std::chrono_literals; namespace @@ -85,4 +87,17 @@ TEST_F(TestSubscriptionOptions, topic_statistics_options_node_default_mode) { rclcpp::detail::resolve_enable_topic_statistics( subscription_options, *(node->get_node_base_interface()))); + + subscription_options.topic_stats_options.state = rclcpp::TopicStatisticsState::Disable; + EXPECT_FALSE( + rclcpp::detail::resolve_enable_topic_statistics( + subscription_options, + *(node->get_node_base_interface()))); + + subscription_options.topic_stats_options.state = static_cast(5); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::detail::resolve_enable_topic_statistics( + subscription_options, + *(node->get_node_base_interface())), + std::runtime_error("Unrecognized EnableTopicStatistics value")); } From 39a9d1d0d34dfcba38e52146a187146fb36050a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 30 Sep 2020 16:47:17 +0200 Subject: [PATCH 45/61] Covered resolve_use_intra_process (#1359) * Covered resolve_use_intra_process Signed-off-by: ahcorde * used RCLCPP_EXPECT_THROW_EQ in test_subscription_throws_intraprocess Signed-off-by: ahcorde --- rclcpp/test/rclcpp/test_subscription.cpp | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index ef626849a0..9559e69c4b 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -461,6 +462,29 @@ TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws) { } } +/* + Testing subscription with invalid use_intra_process_comm + */ +TEST_P(TestSubscriptionInvalidIntraprocessQos, test_subscription_throws_intraprocess) { + rclcpp::SubscriptionOptionsWithAllocator> options; + options.use_intra_process_comm = static_cast(5); + + initialize(); + rclcpp::QoS qos = GetParam().qos; + auto callback = std::bind( + &TestSubscriptionInvalidIntraprocessQos::OnMessage, + this, + std::placeholders::_1); + + RCLCPP_EXPECT_THROW_EQ( + {auto subscription = node->create_subscription( + "topic", + qos, + callback, + options);}, + std::runtime_error("Unrecognized IntraProcessSetting value")); +} + static std::vector invalid_qos_profiles() { std::vector parameters; From e2836303d65a3bbf3ccd2d07ed2b8c5f94a81294 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 30 Sep 2020 17:36:14 +0200 Subject: [PATCH 46/61] Improved test publisher - zero qos history depth value exception (#1360) Signed-off-by: ahcorde --- rclcpp/test/rclcpp/test_publisher.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 01a1ee2fde..eebf34ddef 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -375,6 +375,11 @@ TEST_F(TestPublisher, intra_process_publish_failures) { publisher->publish(std::move(loaned_msg)), std::runtime_error("loaned message is not valid")); } + RCLCPP_EXPECT_THROW_EQ( + node->create_publisher( + "topic", rclcpp::QoS(0), options), + std::invalid_argument( + "intraprocess communication is not allowed with a zero qos history depth value")); } TEST_F(TestPublisher, inter_process_publish_failures) { From 5cdff0135bdd8937ed48eed42d047c9f48955e9d Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Sep 2020 15:11:41 +0000 Subject: [PATCH 47/61] Minor fixes to the parameter_service.cpp file. Make sure to #include what is used, and also fix a typo in a test. Signed-off-by: Chris Lalancette --- rclcpp/src/rclcpp/parameter_client.cpp | 5 +++++ rclcpp/src/rclcpp/parameter_service.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 03a59c6618..c1c3d4eac7 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -15,7 +15,12 @@ #include "rclcpp/parameter_client.hpp" #include +#include +#include +#include +#include #include +#include #include #include diff --git a/rclcpp/src/rclcpp/parameter_service.cpp b/rclcpp/src/rclcpp/parameter_service.cpp index 33e460f2f9..0cbb9667e4 100644 --- a/rclcpp/src/rclcpp/parameter_service.cpp +++ b/rclcpp/src/rclcpp/parameter_service.cpp @@ -119,7 +119,7 @@ ParameterService::ParameterService( RCLCPP_WARN( rclcpp::get_logger("rclcpp"), "Failed to set parameters atomically: %s", ex.what()); response->result.successful = false; - response->result.reason = "One or more parameters wer not declared before setting"; + response->result.reason = "One or more parameters were not declared before setting"; } }, qos_profile, nullptr); From 3aeedf6616c03ffdccce72ea3e3ce42f9c10c23b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Sep 2020 15:12:04 +0000 Subject: [PATCH 48/61] Add in additional tests for parameter_client.cpp coverage. This gets us to 96% line coverage. Signed-off-by: Chris Lalancette --- rclcpp/test/rclcpp/test_parameter_client.cpp | 111 ++++++++++++++++++- 1 file changed, 110 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 523cd3fdce..9e5bcdd437 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -14,8 +14,13 @@ #include -#include +#include +#include +#include #include +#include +#include +#include #include "rclcpp/rclcpp.hpp" @@ -182,3 +187,107 @@ TEST_F(TestParameterClient, sync_parameter_is_ready) { EXPECT_TRUE(client.wait_for_service()); EXPECT_TRUE(client.service_is_ready()); } + +/* + Coverage for async get_parameter_types + */ +TEST_F(TestParameterClient, async_parameter_get_parameter_types) { + auto asynchronous_client = std::make_shared(node); + bool callback_called = false; + auto callback = [&callback_called](std::shared_future> result) + { + // We expect the result to be empty since we tried to get a parameter that didn't exist. + if (result.valid() && result.get().size() == 0) { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = + asynchronous_client->get_parameter_types(names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for async get_parameters + */ +TEST_F(TestParameterClient, async_parameter_get_parameters) { + auto asynchronous_client = std::make_shared(node); + bool callback_called = false; + auto callback = [&callback_called](std::shared_future> result) + { + if (result.valid() && result.get().size() == 1 && result.get()[0].get_name() == "foo") { + callback_called = true; + } + }; + std::vector names{"foo"}; + std::shared_future> future = asynchronous_client->get_parameters( + names, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for async set_parameters_atomically + */ +TEST_F(TestParameterClient, async_parameter_set_parameters_atomically) { + auto asynchronous_client = std::make_shared(node); + bool callback_called = false; + auto callback = + [&callback_called](std::shared_future result) + { + // We expect this to fail since we didn't declare the parameter first. + if (result.valid() && !result.get().successful && + result.get().reason == "One or more parameters were not declared before setting") + { + callback_called = true; + } + }; + std::vector parameters; + parameters.emplace_back(rclcpp::Parameter("foo")); + std::shared_future future = + asynchronous_client->set_parameters_atomically(parameters, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for async list_parameters + */ +TEST_F(TestParameterClient, async_parameter_list_parameters) { + auto asynchronous_client = std::make_shared(node); + bool callback_called = false; + auto callback = + [&callback_called](std::shared_future result) + { + if (result.valid() && result.get().names.size() == 0 && result.get().prefixes.size() == 0) { + callback_called = true; + } + }; + std::vector prefixes{"foo"}; + std::shared_future future = + asynchronous_client->list_parameters(prefixes, 0, callback); + auto return_code = rclcpp::spin_until_future_complete( + node, future, std::chrono::milliseconds(100)); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, return_code); + ASSERT_TRUE(callback_called); +} + +/* + Coverage for sync get_parameter_types + */ +TEST_F(TestParameterClient, sync_parameter_get_parameter_types) { + node->declare_parameter("foo", 4); + auto synchronous_client = std::make_shared(node); + std::vector names{"foo"}; + std::vector parameter_types = + synchronous_client->get_parameter_types(names); + ASSERT_EQ(1u, parameter_types.size()); + ASSERT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); +} From e1b86e46bbd3584058b19bf819050ede4c00b31b Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Wed, 30 Sep 2020 17:20:40 -0300 Subject: [PATCH 49/61] Add timer coverage tests (#1363) * Add missing tests API * Reformat style error throw * Add internal errors tests Signed-off-by: Jorge Perez --- rclcpp/src/rclcpp/timer.cpp | 27 ++++++------ rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_timer.cpp | 73 +++++++++++++++++++++++++++++++ 3 files changed, 87 insertions(+), 15 deletions(-) diff --git a/rclcpp/src/rclcpp/timer.cpp b/rclcpp/src/rclcpp/timer.cpp index 50df28baaa..70414f38c6 100644 --- a/rclcpp/src/rclcpp/timer.cpp +++ b/rclcpp/src/rclcpp/timer.cpp @@ -76,8 +76,9 @@ TimerBase::~TimerBase() void TimerBase::cancel() { - if (rcl_timer_cancel(timer_handle_.get()) != RCL_RET_OK) { - throw std::runtime_error(std::string("Couldn't cancel timer: ") + rcl_get_error_string().str); + rcl_ret_t ret = rcl_timer_cancel(timer_handle_.get()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't cancel timer"); } } @@ -95,8 +96,9 @@ TimerBase::is_canceled() void TimerBase::reset() { - if (rcl_timer_reset(timer_handle_.get()) != RCL_RET_OK) { - throw std::runtime_error(std::string("Couldn't reset timer: ") + rcl_get_error_string().str); + rcl_ret_t ret = rcl_timer_reset(timer_handle_.get()); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't reset timer"); } } @@ -104,8 +106,9 @@ bool TimerBase::is_ready() { bool ready = false; - if (rcl_timer_is_ready(timer_handle_.get(), &ready) != RCL_RET_OK) { - throw std::runtime_error(std::string("Failed to check timer: ") + rcl_get_error_string().str); + rcl_ret_t ret = rcl_timer_is_ready(timer_handle_.get(), &ready); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to check timer"); } return ready; } @@ -114,14 +117,10 @@ std::chrono::nanoseconds TimerBase::time_until_trigger() { int64_t time_until_next_call = 0; - if ( - rcl_timer_get_time_until_next_call( - timer_handle_.get(), - &time_until_next_call) != RCL_RET_OK) - { - throw std::runtime_error( - std::string( - "Timer could not get time until next call: ") + rcl_get_error_string().str); + rcl_ret_t ret = rcl_timer_get_time_until_next_call( + timer_handle_.get(), &time_until_next_call); + if (ret != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "Timer could not get time until next call"); } return std::chrono::nanoseconds(time_until_next_call); } diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 5540a96459..1d9099f631 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -484,7 +484,7 @@ ament_add_gtest(test_timer rclcpp/test_timer.cpp if(TARGET test_timer) ament_target_dependencies(test_timer "rcl") - target_link_libraries(test_timer ${PROJECT_NAME}) + target_link_libraries(test_timer ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_time_source rclcpp/test_time_source.cpp diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index cc568b06c5..0928fcaa5d 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -25,6 +25,9 @@ #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + using namespace std::chrono_literals; /// Timer testing bring up and teardown @@ -207,4 +210,74 @@ TEST_F(TestTimer, callback_with_timer) { executor->spin_once(std::chrono::milliseconds(10)); } EXPECT_EQ(timer.get(), timer_ptr); + EXPECT_LE(std::chrono::nanoseconds(0).count(), timer_ptr->time_until_trigger().count()); + EXPECT_FALSE(timer_ptr->is_ready()); +} + +TEST_F(TestTimer, callback_with_period_zero) { + rclcpp::TimerBase * timer_ptr = nullptr; + timer = test_node->create_wall_timer( + std::chrono::milliseconds(0), + [&timer_ptr](rclcpp::TimerBase & timer) { + timer_ptr = &timer; + }); + auto start = std::chrono::steady_clock::now(); + while (nullptr == timer_ptr && + (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) + { + executor->spin_once(std::chrono::milliseconds(10)); + } + ASSERT_EQ(timer.get(), timer_ptr); + EXPECT_GE(std::chrono::nanoseconds(0).count(), timer_ptr->time_until_trigger().count()); + EXPECT_TRUE(timer_ptr->is_ready()); +} + +/// Test internal failures using mocks +TEST_F(TestTimer, test_failures_with_exceptions) +{ + // expect clean state, don't run otherwise + test_initial_conditions(timer, has_timer_run); + { + std::shared_ptr timer_to_test_destructor; + // Test destructor failure, just logs a msg + auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_timer_fini, RCL_RET_ERROR); + EXPECT_NO_THROW( + { + timer_to_test_destructor = + test_node->create_wall_timer(std::chrono::milliseconds(0), [](void) {}); + timer_to_test_destructor.reset(); + }); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_cancel, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->cancel(), std::runtime_error("Couldn't cancel timer: error not set")); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_is_canceled, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->is_canceled(), + std::runtime_error("Couldn't get timer cancelled state: error not set")); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_reset, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->reset(), std::runtime_error("Couldn't reset timer: error not set")); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_is_ready, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->is_ready(), std::runtime_error("Failed to check timer: error not set")); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_timer_get_time_until_next_call, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + timer->time_until_trigger(), + std::runtime_error("Timer could not get time until next call: error not set")); + } } From 580d36be3a74fc159d61414b9501a918ab7b7295 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Wed, 30 Sep 2020 18:14:08 -0300 Subject: [PATCH 50/61] Add time API coverage tests (#1347) * Change value used as max representation * Add coverage tests time * Add call to detach clock * Add tests time * Add duration construction tests * Add const qualifier to constants * Add check clock stays the same * Make operator RCLCPP_PUBLIC * Add tests exceptions duration * Fix division by 0 on windows Signed-off-by: Jorge Perez --- rclcpp/include/rclcpp/time.hpp | 1 + rclcpp/test/rclcpp/test_duration.cpp | 24 +++++++ rclcpp/test/rclcpp/test_time.cpp | 92 +++++++++++++++++++++++++ rclcpp/test/rclcpp/test_time_source.cpp | 3 + 4 files changed, 120 insertions(+) diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index af41bef435..15533f39ef 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/rclcpp/include/rclcpp/time.hpp @@ -218,6 +218,7 @@ class Time /** * \throws std::overflow_error if addition leads to overflow */ +RCLCPP_PUBLIC Time operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs); diff --git a/rclcpp/test/rclcpp/test_duration.cpp b/rclcpp/test/rclcpp/test_duration.cpp index e09295a5bb..6e93ae019d 100644 --- a/rclcpp/test/rclcpp/test_duration.cpp +++ b/rclcpp/test/rclcpp/test_duration.cpp @@ -25,6 +25,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp/duration.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" using namespace std::chrono_literals; @@ -237,3 +238,26 @@ TEST_F(TestDuration, conversions) { EXPECT_EQ(chrono_duration.count(), -ONE_AND_HALF_SEC_IN_NS); } } + +TEST_F(TestDuration, test_some_constructors) { + builtin_interfaces::msg::Duration duration_msg; + duration_msg.sec = 1; + duration_msg.nanosec = 1000; + rclcpp::Duration duration_from_msg(duration_msg); + EXPECT_EQ(RCL_S_TO_NS(1) + 1000, duration_from_msg.nanoseconds()); + + rcl_duration_t duration_struct; + duration_struct.nanoseconds = 4000; + rclcpp::Duration duration_from_struct(duration_struct); + EXPECT_EQ(4000, duration_from_struct.nanoseconds()); +} + +TEST_F(TestDuration, test_some_exceptions) { + rclcpp::Duration test_duration(0u); + RCLCPP_EXPECT_THROW_EQ( + test_duration = rclcpp::Duration(INT64_MAX) - rclcpp::Duration(-1), + std::overflow_error("duration subtraction leads to int64_t overflow")); + RCLCPP_EXPECT_THROW_EQ( + test_duration = test_duration * (std::numeric_limits::infinity()), + std::runtime_error("abnormal scale in rclcpp::Duration")); +} diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index ac7f8e0a80..c399e51074 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -25,6 +25,8 @@ #include "rclcpp/time.hpp" #include "rclcpp/utilities.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + namespace { @@ -352,3 +354,93 @@ TEST_F(TestTime, seconds) { EXPECT_DOUBLE_EQ(4.5, rclcpp::Time(4, 500000000).seconds()); EXPECT_DOUBLE_EQ(2.5, rclcpp::Time(0, 2500000000).seconds()); } + +TEST_F(TestTime, test_max) { + const rclcpp::Time time_max = rclcpp::Time::max(); + const rclcpp::Time max_time(std::numeric_limits::max(), 999999999); + EXPECT_DOUBLE_EQ(max_time.seconds(), time_max.seconds()); + EXPECT_EQ(max_time.nanoseconds(), time_max.nanoseconds()); +} + +TEST_F(TestTime, test_constructor_from_rcl_time_point) { + const rcl_time_point_value_t test_nano_seconds = 555; + const rcl_clock_type_t test_clock_type = RCL_ROS_TIME; + rcl_time_point_t test_time_point; + test_time_point.nanoseconds = test_nano_seconds; + test_time_point.clock_type = test_clock_type; + + const rclcpp::Time time_max = rclcpp::Time(test_time_point); + + EXPECT_EQ(test_nano_seconds, time_max.nanoseconds()); + EXPECT_EQ(test_nano_seconds, test_time_point.nanoseconds); + EXPECT_EQ(test_clock_type, time_max.get_clock_type()); + EXPECT_EQ(test_clock_type, test_time_point.clock_type); +} + +TEST_F(TestTime, test_assignment_operator_from_builtin_msg_time) { + rclcpp::Clock ros_clock(RCL_ROS_TIME); + const builtin_interfaces::msg::Time ros_now = ros_clock.now(); + EXPECT_NE(0, ros_now.sec); + EXPECT_NE(0u, ros_now.nanosec); + + rclcpp::Time test_time(0u, RCL_CLOCK_UNINITIALIZED); + EXPECT_EQ(0u, test_time.nanoseconds()); + EXPECT_EQ(RCL_CLOCK_UNINITIALIZED, test_time.get_clock_type()); + + test_time = ros_now; + EXPECT_NE(0, test_time.nanoseconds()); + // The clock type is hardcoded internally + EXPECT_EQ(RCL_ROS_TIME, test_time.get_clock_type()); +} + +TEST_F(TestTime, test_sum_operator) { + const rclcpp::Duration one(1); + const rclcpp::Time test_time(0u); + EXPECT_EQ(0u, test_time.nanoseconds()); + + const rclcpp::Time new_time = one + test_time; + EXPECT_EQ(1, new_time.nanoseconds()); +} + +TEST_F(TestTime, test_overflow_underflow_throws) { + rclcpp::Time test_time(0u); + + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Time(INT64_MAX) + rclcpp::Duration(1), + std::overflow_error("addition leads to int64_t overflow")); + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Time(INT64_MIN) + rclcpp::Duration(-1), + std::underflow_error("addition leads to int64_t underflow")); + + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Time(INT64_MAX) - rclcpp::Duration(-1), + std::overflow_error("time subtraction leads to int64_t overflow")); + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Time(INT64_MIN) - rclcpp::Duration(1), + std::underflow_error("time subtraction leads to int64_t underflow")); + + test_time = rclcpp::Time(INT64_MAX); + RCLCPP_EXPECT_THROW_EQ( + test_time += rclcpp::Duration(1), + std::overflow_error("addition leads to int64_t overflow")); + test_time = rclcpp::Time(INT64_MIN); + RCLCPP_EXPECT_THROW_EQ( + test_time += rclcpp::Duration(-1), + std::underflow_error("addition leads to int64_t underflow")); + + test_time = rclcpp::Time(INT64_MAX); + RCLCPP_EXPECT_THROW_EQ( + test_time -= rclcpp::Duration(-1), + std::overflow_error("time subtraction leads to int64_t overflow")); + test_time = rclcpp::Time(INT64_MIN); + RCLCPP_EXPECT_THROW_EQ( + test_time -= rclcpp::Duration(1), + std::underflow_error("time subtraction leads to int64_t underflow")); + + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Duration(INT64_MAX) + rclcpp::Time(1), + std::overflow_error("addition leads to int64_t overflow")); + RCLCPP_EXPECT_THROW_EQ( + test_time = rclcpp::Duration(INT64_MIN) + rclcpp::Time(-1), + std::underflow_error("addition leads to int64_t underflow")); +} diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index ef624bd6e9..8901f6b2d4 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/rclcpp/test/rclcpp/test_time_source.cpp @@ -209,6 +209,9 @@ TEST_F(TestTimeSource, ROS_time_valid_attach_detach) { ts.attachNode(node); EXPECT_FALSE(ros_clock->ros_time_is_active()); + + ts.detachClock(ros_clock); + EXPECT_FALSE(ros_clock->ros_time_is_active()); } TEST_F(TestTimeSource, ROS_time_valid_wall_time) { From 1d67840fb7a39abc0c0bb696a8ef35eecbda9820 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 30 Sep 2020 14:30:02 -0700 Subject: [PATCH 51/61] Add test for ParameterService (#1355) * Add test for ParameterService Signed-off-by: Stephen Brawner * Address PR feedback Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 7 + rclcpp/test/rclcpp/test_parameter_service.cpp | 139 ++++++++++++++++++ 2 files changed, 146 insertions(+) create mode 100644 rclcpp/test/rclcpp/test_parameter_service.cpp diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 1d9099f631..1d41492de4 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -291,6 +291,13 @@ if(TARGET test_parameter_client) ) target_link_libraries(test_parameter_client ${PROJECT_NAME}) endif() +ament_add_gtest(test_parameter_service rclcpp/test_parameter_service.cpp) +if(TARGET test_parameter_service) + ament_target_dependencies(test_parameter_service + "rcl_interfaces" + ) + target_link_libraries(test_parameter_service ${PROJECT_NAME}) +endif() ament_add_gtest(test_parameter_events_filter rclcpp/test_parameter_events_filter.cpp) if(TARGET test_parameter_events_filter) ament_target_dependencies(test_parameter_events_filter diff --git a/rclcpp/test/rclcpp/test_parameter_service.cpp b/rclcpp/test/rclcpp/test_parameter_service.cpp new file mode 100644 index 0000000000..e9bbbba916 --- /dev/null +++ b/rclcpp/test/rclcpp/test_parameter_service.cpp @@ -0,0 +1,139 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "../../src/rclcpp/parameter_service_names.hpp" + +// This tests the ParameterService as it is included in an rclcpp::Node. Creating a separate +// ParameterService would interfere with the built-in one +class TestParameterService : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_parameter_service", "/ns"); + client = std::make_shared(node); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + } + + rclcpp::Node::SharedPtr node; + rclcpp::SyncParametersClient::SharedPtr client; +}; + +TEST_F(TestParameterService, get_parameters) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + EXPECT_EQ(42, client->get_parameter("parameter1", 0)); + + EXPECT_EQ(-42, client->get_parameter("undeclared_parameter", -42)); +} + +TEST_F(TestParameterService, get_parameter_types) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + + const std::vector declared_parameters = {"parameter1"}; + const auto parameter_types = client->get_parameter_types(declared_parameters); + ASSERT_EQ(1u, parameter_types.size()); + EXPECT_EQ(rclcpp::ParameterType::PARAMETER_INTEGER, parameter_types[0]); + + const std::vector undeclared_parameters = {"parameter2"}; + const auto undeclared_parameter_types = client->get_parameter_types(undeclared_parameters); + EXPECT_EQ(0u, undeclared_parameter_types.size()); +} + +TEST_F(TestParameterService, set_parameters) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + ASSERT_EQ(42, client->get_parameter("parameter1", 0)); + + const std::vector parameters = { + rclcpp::Parameter("parameter1", 0), + }; + client->set_parameters(parameters); + EXPECT_EQ(0, client->get_parameter("parameter1", 100)); +} + +TEST_F(TestParameterService, set_parameters_atomically) { + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + ASSERT_EQ(42, client->get_parameter("parameter1", 0)); + + const std::vector parameters = { + rclcpp::Parameter("parameter1", 0), + }; + client->set_parameters_atomically(parameters); + EXPECT_EQ(0, client->get_parameter("parameter1", 100)); +} + +TEST_F(TestParameterService, list_parameters) { + const size_t number_parameters_in_basic_node = client->list_parameters({}, 1).names.size(); + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + + const auto list_result = client->list_parameters({}, 1); + EXPECT_EQ(1u + number_parameters_in_basic_node, list_result.names.size()); +} + +TEST_F(TestParameterService, describe_parameters) { + // There is no current API in ParameterClient for calling the describe_parameters service + // Update this test when https://github.com/ros2/rclcpp/issues/1354 is resolved + + const std::string describe_parameters_service_name = + node->get_fully_qualified_name() + std::string("/") + + rclcpp::parameter_service_names::describe_parameters; + using ServiceT = rcl_interfaces::srv::DescribeParameters; + auto client = + node->create_client(describe_parameters_service_name); + + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + node->declare_parameter("parameter1", rclcpp::ParameterValue(42)); + + { + auto request = std::make_shared(); + request->names.push_back("parameter1"); + auto future = client->async_send_request(request); + + rclcpp::spin_until_future_complete(node, future, std::chrono::seconds(1)); + auto response = future.get(); + ASSERT_NE(nullptr, response); + EXPECT_EQ(1u, response->descriptors.size()); + + auto descriptor = response->descriptors[0]; + EXPECT_EQ("parameter1", descriptor.name); + EXPECT_EQ(rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, descriptor.type); + } + { + auto request = std::make_shared(); + request->names.push_back("undeclared_parameter"); + auto future = client->async_send_request(request); + + rclcpp::spin_until_future_complete(node, future, std::chrono::seconds(1)); + auto response = future.get(); + ASSERT_NE(nullptr, response); + EXPECT_EQ(0u, response->descriptors.size()); + } +} From 0164217cca55564aae46d19d98dd8d016e263722 Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 17 Sep 2020 13:57:44 -0700 Subject: [PATCH 52/61] Only exchange intra_process waitable if nonnull (#1317) Signed-off-by: Stephen Brawner --- rclcpp/include/rclcpp/wait_set_template.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 87e3974ac6..39c7dd3d4e 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -233,9 +233,9 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli } if (mask.include_intra_process_waitable) { auto local_waitable = inner_subscription->get_intra_process_waitable(); - inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false); if (nullptr != local_waitable) { - // This is the case when intra process is disabled for the subscription. + // This is the case when intra process is enabled for the subscription. + inner_subscription->exchange_in_use_by_wait_set_state(local_waitable.get(), false); this->storage_remove_waitable(std::move(local_waitable)); } } From c71f8b6594a5a714c9bdb371e484dabb9aa5932b Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 30 Sep 2020 18:32:56 -0400 Subject: [PATCH 53/61] Finish API coverage on executors. (#1364) In particular, add API coverage for spin_node_until_future_complete, spin_until_future_complete, and spin_node_once. Signed-off-by: Chris Lalancette --- .../test/rclcpp/executors/test_executors.cpp | 69 +++++++++++++++++++ rclcpp/test/rclcpp/test_executor.cpp | 34 +++++++++ 2 files changed, 103 insertions(+) diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 9128e17061..ac244594b8 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -335,3 +335,72 @@ TYPED_TEST(TestExecutorsStable, spinSome) { spinner.join(); } + +// Check spin_node_until_future_complete with node base pointer +TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodeBasePtr) { + using ExecutorType = TypeParam; + ExecutorType executor; + + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::executors::spin_node_until_future_complete( + executor, this->node->get_node_base_interface(), shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + +// Check spin_node_until_future_complete with node pointer +TYPED_TEST(TestExecutorsStable, testSpinNodeUntilFutureCompleteNodePtr) { + using ExecutorType = TypeParam; + ExecutorType executor; + + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::executors::spin_node_until_future_complete( + executor, this->node, shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); +} + +// Check spin_until_future_complete with node base pointer (instantiates its own executor) +TEST(TestExecutors, testSpinUntilFutureCompleteNodeBasePtr) { + rclcpp::init(0, nullptr); + + { + auto node = std::make_shared("node"); + + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + } + + rclcpp::shutdown(); +} + +// Check spin_until_future_complete with node pointer (instantiates its own executor) +TEST(TestExecutors, testSpinUntilFutureCompleteNodePtr) { + rclcpp::init(0, nullptr); + + { + auto node = std::make_shared("node"); + + std::promise promise; + std::future future = promise.get_future(); + promise.set_value(true); + + auto shared_future = future.share(); + auto ret = rclcpp::spin_until_future_complete(node, shared_future, 1s); + EXPECT_EQ(rclcpp::FutureReturnCode::SUCCESS, ret); + } + + rclcpp::shutdown(); +} diff --git a/rclcpp/test/rclcpp/test_executor.cpp b/rclcpp/test/rclcpp/test_executor.cpp index 49a836a054..d916ec95c8 100644 --- a/rclcpp/test/rclcpp/test_executor.cpp +++ b/rclcpp/test/rclcpp/test_executor.cpp @@ -321,3 +321,37 @@ TEST_F(TestExecutor, get_group_by_timer_with_deleted_group) { ASSERT_EQ(nullptr, dummy.local_get_group_by_timer(timer).get()); } + +TEST_F(TestExecutor, spin_node_once_base_interface) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_once(node->get_node_base_interface()); + EXPECT_TRUE(spin_called); +} + +TEST_F(TestExecutor, spin_node_once_node) { + DummyExecutor dummy; + auto node = std::make_shared("node", "ns"); + bool spin_called = false; + auto timer = + node->create_wall_timer( + std::chrono::milliseconds(1), [&]() { + spin_called = true; + }); + + // Wait for the wall timer to have expired. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + EXPECT_FALSE(spin_called); + dummy.spin_node_once(node); + EXPECT_TRUE(spin_called); +} From 85d809cdbd614e30828410c6af0dc5493a697496 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 30 Sep 2020 15:46:10 -0700 Subject: [PATCH 54/61] Finish coverage of publisher API (#1365) Signed-off-by: Stephen Brawner --- rclcpp/test/rclcpp/test_publisher.cpp | 29 +++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index eebf34ddef..c9f11520d3 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -266,6 +266,19 @@ TEST_F(TestPublisher, basic_getters) { } } +TEST_F(TestPublisher, serialized_message_publish) { + initialize(); + rclcpp::PublisherOptionsWithAllocator> options; + // This is the default, but it's also important for this test to succeed. + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto publisher = node->create_publisher("topic", 10, options); + + rclcpp::SerializedMessage serialized_msg; + EXPECT_NO_THROW(publisher->publish(serialized_msg)); + + EXPECT_NO_THROW(publisher->publish(serialized_msg.get_rcl_serialized_message())); +} + TEST_F(TestPublisher, rcl_publisher_init_error) { initialize(); auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_init, RCL_RET_ERROR); @@ -424,6 +437,11 @@ class TestPublisherProtectedMethods : public rclcpp::Publisherdo_loaned_message_publish(msg); } + + void call_default_incompatible_qos_callback(rclcpp::QOSOfferedIncompatibleQoSInfo & event) const + { + this->default_incompatible_qos_callback(event); + } }; TEST_F(TestPublisher, do_loaned_message_publish_error) { @@ -442,6 +460,17 @@ TEST_F(TestPublisher, do_loaned_message_publish_error) { } } +TEST_F(TestPublisher, default_incompatible_qos_callback) { + initialize(); + using PublisherT = TestPublisherProtectedMethods>; + auto publisher = + node->create_publisher, PublisherT>("topic", 10); + rclcpp::QOSOfferedIncompatibleQoSInfo event; + event.last_policy_kind = RMW_QOS_POLICY_INVALID; + // This message just logs an error message + EXPECT_NO_THROW(publisher->call_default_incompatible_qos_callback(event)); +} + TEST_F(TestPublisher, run_event_handlers) { initialize(); auto publisher = node->create_publisher("topic", 10); From 5706243170dd80ba253b821cba8974ba7a31ff98 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 30 Sep 2020 15:59:10 -0700 Subject: [PATCH 55/61] Add unit tests for qos and qos_event files (#1352) * Add unit tests for qos and qos_event files Signed-off-by: Stephen Brawner * PR Feedback Signed-off-by: Stephen Brawner * Address PR Feedback Signed-off-by: Stephen Brawner * Fix windows CI Signed-off-by: Stephen Brawner --- rclcpp/include/rclcpp/qos.hpp | 1 + rclcpp/test/CMakeLists.txt | 1 + rclcpp/test/rclcpp/test_qos.cpp | 121 ++++++++++++++++++++++++++ rclcpp/test/rclcpp/test_qos_event.cpp | 105 +++++++++++++++++++++- 4 files changed, 226 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/qos.hpp b/rclcpp/include/rclcpp/qos.hpp index ac016efec5..c59514a5a2 100644 --- a/rclcpp/include/rclcpp/qos.hpp +++ b/rclcpp/include/rclcpp/qos.hpp @@ -26,6 +26,7 @@ namespace rclcpp { +RCLCPP_PUBLIC std::string qos_policy_name_from_kind(rmw_qos_policy_kind_t policy_kind); /// QoS initialization values, cannot be created directly, use KeepAll or KeepLast instead. diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 1d41492de4..a2d8ec2f9e 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -361,6 +361,7 @@ if(TARGET test_qos_event) ) target_link_libraries(test_qos_event ${PROJECT_NAME} + mimick ) endif() ament_add_gtest(test_rate rclcpp/test_rate.cpp) diff --git a/rclcpp/test/rclcpp/test_qos.cpp b/rclcpp/test/rclcpp/test_qos.cpp index 7188119ceb..944cc1a8a3 100644 --- a/rclcpp/test/rclcpp/test_qos.cpp +++ b/rclcpp/test/rclcpp/test_qos.cpp @@ -16,6 +16,8 @@ #include "rclcpp/qos.hpp" +#include "rmw/types.h" + TEST(TestQoS, equality_history) { rclcpp::QoS a(10); rclcpp::QoS b(10); @@ -75,3 +77,122 @@ TEST(TestQoS, equality_namespace) { a.avoid_ros_namespace_conventions(true); EXPECT_NE(a, b); } + +TEST(TestQoS, setters) { + rclcpp::QoS qos(10); + + qos.keep_all(); + EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_ALL, qos.get_rmw_qos_profile().history); + + qos.keep_last(20); + EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_LAST, qos.get_rmw_qos_profile().history); + EXPECT_EQ(20u, qos.get_rmw_qos_profile().depth); + + qos.reliable(); + EXPECT_EQ(RMW_QOS_POLICY_RELIABILITY_RELIABLE, qos.get_rmw_qos_profile().reliability); + + qos.durability_volatile(); + EXPECT_EQ(RMW_QOS_POLICY_DURABILITY_VOLATILE, qos.get_rmw_qos_profile().durability); + + qos.transient_local(); + EXPECT_EQ(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, qos.get_rmw_qos_profile().durability); + + qos.history(RMW_QOS_POLICY_HISTORY_KEEP_ALL); + EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_ALL, qos.get_rmw_qos_profile().history); + + constexpr size_t duration_ns = 12345; + constexpr std::chrono::nanoseconds duration(duration_ns); + qos.deadline(duration); + EXPECT_EQ(0u, qos.get_rmw_qos_profile().deadline.sec); + EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().deadline.nsec); + + const rmw_time_t rmw_time {0, 54321}; + qos.deadline(rmw_time); + EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().deadline.sec); + EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().deadline.nsec); + + qos.lifespan(duration); + EXPECT_EQ(0u, qos.get_rmw_qos_profile().lifespan.sec); + EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().lifespan.nsec); + + qos.lifespan(rmw_time); + EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().lifespan.sec); + EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().lifespan.nsec); + + qos.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); + EXPECT_EQ(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, qos.get_rmw_qos_profile().liveliness); + + qos.liveliness_lease_duration(duration); + EXPECT_EQ(0u, qos.get_rmw_qos_profile().liveliness_lease_duration.sec); + EXPECT_EQ(duration_ns, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec); + + qos.liveliness_lease_duration(rmw_time); + EXPECT_EQ(rmw_time.sec, qos.get_rmw_qos_profile().liveliness_lease_duration.sec); + EXPECT_EQ(rmw_time.nsec, qos.get_rmw_qos_profile().liveliness_lease_duration.nsec); + + qos.avoid_ros_namespace_conventions(true); + EXPECT_TRUE(qos.get_rmw_qos_profile().avoid_ros_namespace_conventions); + qos.avoid_ros_namespace_conventions(false); + EXPECT_FALSE(qos.get_rmw_qos_profile().avoid_ros_namespace_conventions); +} + +bool operator==(const rmw_qos_profile_t & lhs, const rmw_qos_profile_t & rhs) +{ + if (lhs.history != rhs.history) { + return false; + } + switch (lhs.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + return true; + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + return lhs.depth == rhs.depth; + } + throw std::runtime_error("This line shouldn't be reached"); +} + +TEST(TestQoS, DerivedTypes) { + rclcpp::SensorDataQoS sensor_data_qos; + EXPECT_EQ(rmw_qos_profile_sensor_data, sensor_data_qos.get_rmw_qos_profile()); + + rclcpp::ParametersQoS parameter_qos; + EXPECT_EQ(rmw_qos_profile_parameters, parameter_qos.get_rmw_qos_profile()); + + rclcpp::ServicesQoS services_qos; + EXPECT_EQ(rmw_qos_profile_services_default, services_qos.get_rmw_qos_profile()); + + rclcpp::ParameterEventsQoS parameter_events_qos; + EXPECT_EQ(rmw_qos_profile_parameter_events, parameter_events_qos.get_rmw_qos_profile()); + + rclcpp::SystemDefaultsQoS system_default_qos; + const rclcpp::KeepLast expected_initialization(RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT); + const rclcpp::QoS expected_default(expected_initialization); + EXPECT_EQ(expected_default.get_rmw_qos_profile(), system_default_qos.get_rmw_qos_profile()); +} + +TEST(TestQoS, policy_name_from_kind) { + EXPECT_EQ( + "DURABILITY_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_DURABILITY)); + + EXPECT_EQ( + "DEADLINE_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_DEADLINE)); + + EXPECT_EQ( + "LIVELINESS_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIVELINESS)); + + EXPECT_EQ( + "RELIABILITY_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_RELIABILITY)); + + EXPECT_EQ( + "HISTORY_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_HISTORY)); + + EXPECT_EQ( + "LIFESPAN_QOS_POLICY", + rclcpp::qos_policy_name_from_kind(RMW_QOS_POLICY_LIFESPAN)); +} diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index c0f6076b16..baf5bd7778 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -25,6 +25,8 @@ #include "rmw/rmw.h" #include "test_msgs/msg/empty.hpp" +#include "../mocking_utils/patch.hpp" + class TestQosEvent : public ::testing::Test { protected: @@ -207,9 +209,14 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) rclcpp::executors::SingleThreadedExecutor ex; ex.add_node(node->get_node_base_interface()); - ex.spin_until_future_complete(log_msgs_future, std::chrono::seconds(10)); + // This future won't complete on fastrtps, so just timeout immediately + const auto timeout = (is_fastrtps) ? std::chrono::milliseconds(5) : std::chrono::seconds(10); + ex.spin_until_future_complete(log_msgs_future, timeout); - if (!is_fastrtps) { + if (is_fastrtps) { + EXPECT_EQ("", pub_log_msg); + EXPECT_EQ("", sub_log_msg); + } else { EXPECT_EQ( "New subscription discovered on this topic, requesting incompatible QoS. " "No messages will be sent to it. Last incompatible policy: DURABILITY_QOS_POLICY", @@ -222,3 +229,97 @@ TEST_F(TestQosEvent, test_default_incompatible_qos_callbacks) rcutils_logging_set_output_handler(original_output_handler); } + +TEST_F(TestQosEvent, construct_destruct_rcl_error) { + auto publisher = node->create_publisher(topic_name, 10); + auto rcl_handle = publisher->get_publisher_handle(); + ASSERT_NE(nullptr, rcl_handle); + + // This callback requires some type of parameter, but it could be anything + auto callback = [](int) {}; + const rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + + { + // Logs error and returns + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_event_init, RCL_RET_ERROR); + + auto throwing_statement = [callback, rcl_handle, event_type]() { + // reset() is not needed for the exception, but it handles unused return value warning + std::make_shared< + rclcpp::QOSEventHandler>>( + callback, rcl_publisher_event_init, rcl_handle, event_type).reset(); + }; + // This is done through a lambda because the compiler is having trouble parsing the templated + // function inside a macro. + EXPECT_THROW(throwing_statement(), rclcpp::exceptions::RCLError); + } + + { + // Logs error and returns + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_event_fini, RCL_RET_ERROR); + + auto throwing_statement = [callback, rcl_handle, event_type]() { + // reset() is needed for this exception + std::make_shared< + rclcpp::QOSEventHandler>>( + callback, rcl_publisher_event_init, rcl_handle, event_type).reset(); + }; + + // This is done through a lambda because the compiler is having trouble parsing the templated + // function inside a macro. + EXPECT_NO_THROW(throwing_statement()); + } +} + +TEST_F(TestQosEvent, execute) { + auto publisher = node->create_publisher(topic_name, 10); + auto rcl_handle = publisher->get_publisher_handle(); + + bool handler_callback_executed = false; + // This callback requires some type of parameter, but it could be anything + auto callback = [&handler_callback_executed](int) {handler_callback_executed = true;}; + rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + + rclcpp::QOSEventHandler handler( + callback, rcl_publisher_event_init, rcl_handle, event_type); + + EXPECT_NO_THROW(handler.execute()); + EXPECT_TRUE(handler_callback_executed); + + { + handler_callback_executed = false; + // Logs error and returns early + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_take_event, RCL_RET_ERROR); + EXPECT_NO_THROW(handler.execute()); + EXPECT_FALSE(handler_callback_executed); + } +} + +TEST_F(TestQosEvent, add_to_wait_set) { + auto publisher = node->create_publisher(topic_name, 10); + auto rcl_handle = publisher->get_publisher_handle(); + + // This callback requires some type of parameter, but it could be anything + auto callback = [](int) {}; + + rcl_publisher_event_type_t event_type = RCL_PUBLISHER_OFFERED_DEADLINE_MISSED; + rclcpp::QOSEventHandler handler( + callback, rcl_publisher_event_init, rcl_handle, event_type); + + EXPECT_EQ(1u, handler.get_number_of_ready_events()); + + { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_add_event, RCL_RET_OK); + EXPECT_TRUE(handler.add_to_wait_set(&wait_set)); + } + + { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_event, RCL_RET_ERROR); + EXPECT_THROW(handler.add_to_wait_set(&wait_set), rclcpp::exceptions::RCLError); + } +} From 01d4d5ba839cfe9bdaec6a2884ac74e592ef0175 Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 1 Oct 2020 11:51:13 -0700 Subject: [PATCH 56/61] Tests for LoanedMessage with mocked loaned message publisher (#1366) Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_loaned_message.cpp | 50 ++++++++++++++++++++++ 2 files changed, 51 insertions(+), 1 deletion(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index a2d8ec2f9e..69a04dcf78 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -164,7 +164,7 @@ ament_add_gtest(test_loaned_message rclcpp/test_loaned_message.cpp) ament_target_dependencies(test_loaned_message "test_msgs" ) -target_link_libraries(test_loaned_message ${PROJECT_NAME}) +target_link_libraries(test_loaned_message ${PROJECT_NAME} mimick) ament_add_gtest(test_memory_strategy rclcpp/test_memory_strategy.cpp) ament_target_dependencies(test_memory_strategy diff --git a/rclcpp/test/rclcpp/test_loaned_message.cpp b/rclcpp/test/rclcpp/test_loaned_message.cpp index 651b367020..b68024c772 100644 --- a/rclcpp/test/rclcpp/test_loaned_message.cpp +++ b/rclcpp/test/rclcpp/test_loaned_message.cpp @@ -20,6 +20,8 @@ #include "test_msgs/msg/basic_types.hpp" +#include "../mocking_utils/patch.hpp" + using MessageT = test_msgs::msg::BasicTypes; using LoanedMessageT = rclcpp::LoanedMessage; @@ -81,3 +83,51 @@ TEST_F(TestLoanedMessage, release) { SUCCEED(); } + +TEST_F(TestLoanedMessage, construct_with_loaned_message_publisher) { + auto node = std::make_shared("loaned_message_test_node"); + auto publisher = node->create_publisher("topic", 10); + std::allocator allocator; + + auto mock_can_loan = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_can_loan_messages, true); + + { + auto mock_borrow_loaned = mocking_utils::patch_and_return( + "self", rcl_borrow_loaned_message, RCL_RET_ERROR); + + EXPECT_THROW( + std::make_shared(*publisher.get(), allocator).reset(), + rclcpp::exceptions::RCLError); + } + + MessageT message; + auto borrow_loaned_message_callback = + [&message]( + const rcl_publisher_t *, const rosidl_message_type_support_t *, void ** ros_message) { + *ros_message = &message; + return RCL_RET_OK; + }; + auto mock_borrow_loaned = mocking_utils::patch( + "self", rcl_borrow_loaned_message, borrow_loaned_message_callback); + + { + auto mock_return_loaned = mocking_utils::patch_and_return( + "self", rcl_return_loaned_message_from_publisher, RCL_RET_OK); + + auto loaned_message = std::make_shared(*publisher.get(), allocator); + EXPECT_TRUE(loaned_message->is_valid()); + EXPECT_NO_THROW(loaned_message.reset()); + } + + { + auto loaned_message = std::make_shared(*publisher.get(), allocator); + EXPECT_TRUE(loaned_message->is_valid()); + + auto mock_return_loaned = mocking_utils::patch_and_return( + "self", rcl_return_loaned_message_from_publisher, RCL_RET_ERROR); + + // No exception, it just logs an error + EXPECT_NO_THROW(loaned_message.reset()); + } +} From 90f0a5e1da3446cc829d1944337fd28838f443eb Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 1 Oct 2020 13:10:07 -0700 Subject: [PATCH 57/61] Increase coverage of guard_condition.cpp to 100% (#1369) Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_guard_condition.cpp | 19 ++++++++++++++++++- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 69a04dcf78..5b42d31b40 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -567,7 +567,7 @@ endif() ament_add_gtest(test_guard_condition rclcpp/test_guard_condition.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_guard_condition) - target_link_libraries(test_guard_condition ${PROJECT_NAME}) + target_link_libraries(test_guard_condition ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_wait_set rclcpp/test_wait_set.cpp diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index 474d1d4bcc..8100cf1c9b 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -18,6 +18,8 @@ #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" + class TestGuardCondition : public ::testing::Test { protected: @@ -54,6 +56,14 @@ TEST_F(TestGuardCondition, construction_and_destruction) { (void)gc; }, rclcpp::exceptions::RCLInvalidArgument); } + + { + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + auto gc = std::make_shared(); + // This just logs an error on destruction + EXPECT_NO_THROW(gc.reset()); + } } /* @@ -82,6 +92,13 @@ TEST_F(TestGuardCondition, get_rcl_guard_condition) { TEST_F(TestGuardCondition, trigger) { { auto gc = std::make_shared(); - gc->trigger(); + EXPECT_NO_THROW(gc->trigger()); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + auto gc = std::make_shared(); + EXPECT_THROW(gc->trigger(), rclcpp::exceptions::RCLError); + } } } From 7b7c4910a42f39394328d004ecbe551988044db1 Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 1 Oct 2020 13:10:25 -0700 Subject: [PATCH 58/61] Increase coverage of WaitSetTemplate (#1368) * Increase coverage of WaitSetTemplate Signed-off-by: Stephen Brawner * PR fixup Signed-off-by: Stephen Brawner --- rclcpp/test/rclcpp/test_wait_set.cpp | 58 ++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index 7a5f7b3fbe..bbfc9383c8 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -266,6 +266,64 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { } } + +/* + * Testing adding each entity and waiting, and removing each entity and waiting + */ +TEST_F(TestWaitSet, add_remove_wait) { + rclcpp::WaitSet wait_set; + auto node = std::make_shared("add_remove_wait"); + + auto guard_condition = std::make_shared(); + guard_condition->trigger(); + + // For coverage reasons, this subscription should have event handlers + rclcpp::SubscriptionOptions subscription_options; + subscription_options.event_callbacks.deadline_callback = [](auto) {}; + subscription_options.event_callbacks.liveliness_callback = [](auto) {}; + auto do_nothing = [](const std::shared_ptr) {}; + auto sub = + node->create_subscription( + "~/test", 1, do_nothing, subscription_options); + + auto timer = node->create_wall_timer(std::chrono::milliseconds(1), []() {}); + + auto client = node->create_client("~/test"); + + auto srv_do_nothing = []( + const std::shared_ptr, + std::shared_ptr) {}; + auto service = + node->create_service("~/test", srv_do_nothing); + + rclcpp::PublisherOptions publisher_options; + publisher_options.event_callbacks.deadline_callback = + [](rclcpp::QOSDeadlineOfferedInfo &) {}; + auto pub = node->create_publisher( + "~/test", 1, publisher_options); + auto qos_event = pub->get_event_handlers()[0]; + + // Subscription mask is required here for coverage. + wait_set.add_subscription(sub, {true, true, true}); + wait_set.add_guard_condition(guard_condition); + wait_set.add_timer(timer); + wait_set.add_client(client); + wait_set.add_service(service); + wait_set.add_waitable(qos_event, pub); + + // At least timer or guard_condition should trigger + EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_set.wait(std::chrono::seconds(1)).kind()); + + wait_set.remove_subscription(sub, {true, true, true}); + wait_set.remove_guard_condition(guard_condition); + wait_set.remove_timer(timer); + wait_set.remove_client(client); + wait_set.remove_service(service); + wait_set.remove_waitable(qos_event); + + EXPECT_EQ(rclcpp::WaitResultKind::Empty, wait_set.wait(std::chrono::seconds(1)).kind()); +} + /* * Get wait_set from result. */ From a4b04422a86ea4b081f9ee3eedbd895ba0ecb636 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 5 Oct 2020 20:27:58 +0800 Subject: [PATCH 59/61] Make sure to clean the external client/service handle. (#1296) Signed-off-by: Chen Lihui --- rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index 70470ec17a..a992c26a15 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -83,6 +83,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test void TearDown() override { + allocator_memory_strategy_.reset(); rclcpp::shutdown(); } From 7060b8be9dc3c56217cc9d0bb35e2490723c4372 Mon Sep 17 00:00:00 2001 From: brawner Date: Wed, 14 Oct 2020 10:33:12 -0700 Subject: [PATCH 60/61] Increase test timeouts of slow running tests with rmw_connext_cpp (#1400) * Increase test timeouts of slow running tests with rmw_connext_cpp Signed-off-by: Stephen Brawner * Fix other issues with connext Signed-off-by: Stephen Brawner --- rclcpp/test/CMakeLists.txt | 12 +++++++----- rclcpp/test/rclcpp/test_publisher.cpp | 19 +++++++++++++++++-- rclcpp/test/rclcpp/test_subscription.cpp | 4 ++++ 3 files changed, 28 insertions(+), 7 deletions(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 5b42d31b40..873b9932b7 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -207,7 +207,8 @@ if(TARGET test_node_interfaces__node_clock) target_link_libraries(test_node_interfaces__node_clock ${PROJECT_NAME}) endif() ament_add_gtest(test_node_interfaces__node_graph - rclcpp/node_interfaces/test_node_graph.cpp) + rclcpp/node_interfaces/test_node_graph.cpp + TIMEOUT 120) if(TARGET test_node_interfaces__node_graph) ament_target_dependencies( test_node_interfaces__node_graph @@ -322,7 +323,7 @@ ament_add_gtest(test_parameter_map rclcpp/test_parameter_map.cpp) if(TARGET test_parameter_map) target_link_libraries(test_parameter_map ${PROJECT_NAME}) endif() -ament_add_gtest(test_publisher rclcpp/test_publisher.cpp) +ament_add_gtest(test_publisher rclcpp/test_publisher.cpp TIMEOUT 120) if(TARGET test_publisher) ament_target_dependencies(test_publisher "rcl" @@ -405,7 +406,8 @@ if(TARGET test_service) ) target_link_libraries(test_service ${PROJECT_NAME} mimick) endif() -ament_add_gtest(test_subscription rclcpp/test_subscription.cpp) +# Creating and destroying nodes is slow with Connext, so this needs larger timeout. +ament_add_gtest(test_subscription rclcpp/test_subscription.cpp TIMEOUT 120) if(TARGET test_subscription) ament_target_dependencies(test_subscription "rcl_interfaces" @@ -556,7 +558,7 @@ if(TARGET test_multi_threaded_executor) endif() ament_add_gtest(test_static_executor_entities_collector rclcpp/executors/test_static_executor_entities_collector.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") + APPEND_LIBRARY_DIRS "${append_library_dirs}" TIMEOUT 120) if(TARGET test_static_executor_entities_collector) ament_target_dependencies(test_static_executor_entities_collector "rcl" @@ -632,7 +634,7 @@ endif() ament_add_gtest(test_executor rclcpp/test_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}" -) + TIMEOUT 120) if(TARGET test_executor) ament_target_dependencies(test_executor "rcl") target_link_libraries(test_executor ${PROJECT_NAME} mimick) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index c9f11520d3..26aeb6975c 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -29,6 +29,10 @@ #include "test_msgs/msg/empty.hpp" +// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check +// that this test can complete fully, or adjust the timeout as necessary. +// See https://github.com/ros2/rmw_connext/issues/325 for resolution + class TestPublisher : public ::testing::Test { public: @@ -274,6 +278,9 @@ TEST_F(TestPublisher, serialized_message_publish) { auto publisher = node->create_publisher("topic", 10, options); rclcpp::SerializedMessage serialized_msg; + // Mock successful rcl publish because the serialized_msg above is poorly formed + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_serialized_message, RCL_RET_OK); EXPECT_NO_THROW(publisher->publish(serialized_msg)); EXPECT_NO_THROW(publisher->publish(serialized_msg.get_rcl_serialized_message())); @@ -411,14 +418,22 @@ TEST_F(TestPublisher, inter_process_publish_failures) { EXPECT_THROW(publisher->publish(msg), rclcpp::exceptions::RCLError); } - rclcpp::SerializedMessage serialized_msg; - EXPECT_NO_THROW(publisher->publish(serialized_msg)); + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_serialized_message` is entirely + // defined in a header. Also, this one requires mocking because the serialized_msg is poorly + // formed and this just tests rclcpp functionality. + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_serialized_message, RCL_RET_OK); + rclcpp::SerializedMessage serialized_msg; + EXPECT_NO_THROW(publisher->publish(serialized_msg)); + } { // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_serialized_message` is entirely // defined in a header auto mock = mocking_utils::patch_and_return( "self", rcl_publish_serialized_message, RCL_RET_ERROR); + rclcpp::SerializedMessage serialized_msg; EXPECT_THROW(publisher->publish(serialized_msg), rclcpp::exceptions::RCLError); } diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 9559e69c4b..ef43fc051c 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -29,6 +29,10 @@ #include "test_msgs/msg/empty.hpp" +// Note: This is a long running test with rmw_connext_cpp, if you change this file, please check +// that this test can complete fully, or adjust the timeout as necessary. +// See https://github.com/ros2/rmw_connext/issues/325 for resolution + using namespace std::chrono_literals; class TestSubscription : public ::testing::Test From 6bd52ae1712e849fb5af005a6d86426ef0bb3569 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 15 Oct 2020 20:05:01 +0800 Subject: [PATCH 61/61] Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency (#1303) * Clear members for StaticExecutorEntitiesCollector to avoid shared_ptr dependency Signed-off-by: Chen Lihui Signed-off-by: Chris Lalancette Co-authored-by: Chris Lalancette Signed-off-by: Stephen Brawner --- .../executors/static_executor_entities_collector.hpp | 5 +++++ .../rclcpp/executors/static_single_threaded_executor.hpp | 1 + .../executors/static_executor_entities_collector.cpp | 7 +++++++ .../rclcpp/executors/static_single_threaded_executor.cpp | 1 + rclcpp/test/rclcpp/executors/test_executors.cpp | 8 ++++++++ .../executors/test_static_executor_entities_collector.cpp | 3 +++ 6 files changed, 25 insertions(+) diff --git a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp index 0df25fde9d..604f20ddf9 100644 --- a/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp +++ b/rclcpp/include/rclcpp/executors/static_executor_entities_collector.hpp @@ -62,6 +62,11 @@ class StaticExecutorEntitiesCollector final rclcpp::memory_strategy::MemoryStrategy::SharedPtr & memory_strategy, rcl_guard_condition_t * executor_guard_condition); + /// Finalize StaticExecutorEntitiesCollector to clear resources + RCLCPP_PUBLIC + void + fini(); + RCLCPP_PUBLIC void execute() override; diff --git a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp index 9d39ba8e90..d007988f1f 100644 --- a/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp +++ b/rclcpp/include/rclcpp/executors/static_single_threaded_executor.hpp @@ -163,6 +163,7 @@ class StaticSingleThreadedExecutor : public rclcpp::Executor std::chrono::nanoseconds timeout_left = timeout_ns; entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + RCLCPP_SCOPE_EXIT(entities_collector_->fini()); while (rclcpp::ok(this->context_)) { // Do one set of work. diff --git a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp index 5461e665ab..7d514b2761 100644 --- a/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp +++ b/rclcpp/src/rclcpp/executors/static_executor_entities_collector.cpp @@ -62,6 +62,13 @@ StaticExecutorEntitiesCollector::init( execute(); } +void +StaticExecutorEntitiesCollector::fini() +{ + memory_strategy_->clear_handles(); + exec_list_.clear(); +} + void StaticExecutorEntitiesCollector::execute() { diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 728a0902a1..ced388023d 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -41,6 +41,7 @@ StaticSingleThreadedExecutor::spin() // Set memory_strategy_ and exec_list_ based on weak_nodes_ // Prepare wait_set_ based on memory_strategy_ entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_); + RCLCPP_SCOPE_EXIT(entities_collector_->fini()); while (rclcpp::ok(this->context_) && spinning.load()) { // Refresh wait set and wait for work diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index ac244594b8..e6beba61ad 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -259,6 +259,14 @@ class TestWaitable : public rclcpp::Waitable } } + ~TestWaitable() + { + rcl_ret_t ret = rcl_guard_condition_fini(&gc_); + if (RCL_RET_OK != ret) { + fprintf(stderr, "failed to call rcl_guard_condition_fini\n"); + } + } + bool add_to_wait_set(rcl_wait_set_t * wait_set) override { diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index 92ded7fe5a..6b2d0d1513 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -159,6 +159,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( expected_number_of_entities->subscriptions, entities_collector_->get_number_of_subscriptions()); @@ -206,6 +207,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { // Expect weak_node pointers to be cleaned up and used entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); EXPECT_EQ(0u, entities_collector_->get_number_of_services()); @@ -267,6 +269,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); + RCLCPP_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( 1u + expected_number_of_entities->subscriptions,