Skip to content

Commit

Permalink
Added a test to check if a custom subscriber was created correctly.
Browse files Browse the repository at this point in the history
  • Loading branch information
bks-ol committed Jan 19, 2025
1 parent 2dccd83 commit 270a517
Show file tree
Hide file tree
Showing 2 changed files with 80 additions and 0 deletions.
7 changes: 7 additions & 0 deletions rclcpp/test/rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
73 changes: 73 additions & 0 deletions rclcpp/test/rclcpp/test_create_custom_subscription.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,73 @@
#include <gtest/gtest.h>

#include <chrono>
#include <memory>
#include <type_traits>

#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<void>,
typename SubscribedT = typename rclcpp::TypeAdapter<MessageT>::custom_type,
typename ROSMessageT = typename rclcpp::TypeAdapter<MessageT>::ros_message_type,
typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy<
ROSMessageT,
AllocatorT
>>
class CustomSubscription : public rclcpp::Subscription<
MessageT, AllocatorT, SubscribedT, ROSMessageT, MessageMemoryStrategyT>
{
public:

template <typename... Args>
CustomSubscription(Args &&...args) : rclcpp::Subscription<
MessageT, AllocatorT, SubscribedT, ROSMessageT, MessageMemoryStrategyT>(
std::forward<Args>(args)...) {}
};

TEST_F(TestCreateSubscription, create) {
using MessageT = test_msgs::msg::Empty;

auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
const rclcpp::QoS qos(10);
auto options = rclcpp::SubscriptionOptions();
auto callback = [](MessageT::ConstSharedPtr) {};

using CallbackT = std::decay_t<decltype(callback)>;
using AllocatorT = std::allocator<void>;
using SubscriptionT = CustomSubscription<MessageT, AllocatorT>;
using CallbackMessageT =
typename rclcpp::subscription_traits::has_message_type<CallbackT>::type;
using MessageMemoryStrategyT =
rclcpp::message_memory_strategy::MessageMemoryStrategy<CallbackMessageT,
AllocatorT>;

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<std::decay_t<decltype(*subscription.get())>, SubscriptionT>);
}

0 comments on commit 270a517

Please sign in to comment.