From 07f410ecc8cb90e3dc3dbd2d0ad74c3cdb080231 Mon Sep 17 00:00:00 2001 From: olesya Date: Sun, 19 Jan 2025 16:22:15 +0000 Subject: [PATCH] Added a test to check if a custom subscriber was created correctly. Signed-off-by: olesya --- rclcpp/test/rclcpp/CMakeLists.txt | 7 ++ .../test_create_custom_subscription.cpp | 72 +++++++++++++++++++ 2 files changed, 79 insertions(+) create mode 100644 rclcpp/test/rclcpp/test_create_custom_subscription.cpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 0bdb0d931e..e2e356cb34 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -100,6 +100,13 @@ if(TARGET test_create_subscription) "test_msgs" ) endif() +ament_add_gtest(test_create_custom_subscription test_create_custom_subscription.cpp) +if(TARGET test_create_custom_subscription) + target_link_libraries(test_create_custom_subscription ${PROJECT_NAME}) + ament_target_dependencies(test_create_custom_subscription + "test_msgs" + ) +endif() function(test_add_callback_groups_to_executor_for_rmw_implementation) set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) ament_add_gmock(test_add_callback_groups_to_executor${target_suffix} test_add_callback_groups_to_executor.cpp diff --git a/rclcpp/test/rclcpp/test_create_custom_subscription.cpp b/rclcpp/test/rclcpp/test_create_custom_subscription.cpp new file mode 100644 index 0000000000..dfea57f0a4 --- /dev/null +++ b/rclcpp/test/rclcpp/test_create_custom_subscription.cpp @@ -0,0 +1,72 @@ +#include + +#include +#include +#include + +#include "rclcpp/subscription.hpp" +#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(); + } +}; + +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename SubscribedT = typename rclcpp::TypeAdapter::custom_type, + typename ROSMessageT = typename rclcpp::TypeAdapter::ros_message_type, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + ROSMessageT, + AllocatorT + >> +class CustomSubscription : public rclcpp::Subscription< + MessageT, AllocatorT, SubscribedT, ROSMessageT, MessageMemoryStrategyT> +{ +public: + + template + CustomSubscription(Args &&...args) : rclcpp::Subscription< + MessageT, AllocatorT, SubscribedT, ROSMessageT, MessageMemoryStrategyT>( + std::forward(args)...) {} +}; + +TEST_F(TestCreateSubscription, create) { + using MessageT = test_msgs::msg::Empty; + + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](MessageT::ConstSharedPtr) {}; + + using CallbackT = std::decay_t; + using AllocatorT = std::allocator; + using SubscriptionT = CustomSubscription; + using CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type; + using MessageMemoryStrategyT = + rclcpp::message_memory_strategy::MessageMemoryStrategy; + + auto subscription = rclcpp::create_subscription< + MessageT, CallbackT, AllocatorT, SubscriptionT, MessageMemoryStrategyT>( + node, "topic_name", qos, std::move(callback), options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); + static_assert(std::is_same_v, SubscriptionT>); +} \ No newline at end of file