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 e9e70f8063..92796b42cc 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() @@ -399,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 @@ -490,7 +501,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() @@ -554,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/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 035c154586..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; @@ -41,33 +41,43 @@ 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)); - auto callback = [this](std_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; + 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](test_msgs::msg::Empty::SharedPtr) {this->callback_count++;}; subscription = - node->create_subscription( - topic_name.str(), rclcpp::QoS(10), std::move(callback)); + node->create_subscription( + 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; - rclcpp::Publisher::SharedPtr publisher; - rclcpp::Subscription::SharedPtr subscription; + rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; int callback_count; }; @@ -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; }); @@ -311,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); } 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; 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..7ad25e71e5 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,44 @@ 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); + + 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)); +} 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/topic_statistics/test_subscription_topic_statistics.cpp b/rclcpp/test/rclcpp/topic_statistics/test_subscription_topic_statistics.cpp index dc4e1fa86e..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", - 2); + kNumExpectedMessages); auto empty_subscriber = std::make_shared( kTestSubNodeName, @@ -301,49 +385,37 @@ 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(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(2u, 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. + // 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; - } - 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); } } } @@ -361,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", - 2); + kNumExpectedMessages); auto msg_with_header_subscriber = std::make_shared( kTestSubNodeName, @@ -378,44 +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(2, 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(2u, 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. for (const auto & msg : received_messages) { - 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); } } 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 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()); + } +}