diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 12812e6999..62f53da5cd 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -373,6 +373,7 @@ if(TARGET test_service) "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" + "test_msgs" ) target_link_libraries(test_service ${PROJECT_NAME}) endif() diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index dce0b8caa8..0ae98cc74f 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -203,3 +203,59 @@ TEST_F(TestPublisherSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidTopicNameError); } } + +// Auxiliary class used to test getter for const PublisherBase +const rosidl_message_type_support_t EmptyTypeSupport() +{ + return *rosidl_typesupport_cpp::get_message_type_support_handle(); +} + +const rcl_publisher_options_t PublisherOptions() +{ + return rclcpp::PublisherOptionsWithAllocator>().template + to_rcl_publisher_options(rclcpp::QoS(10)); +} + +class TestPublisherBase : public rclcpp::PublisherBase +{ +public: + explicit TestPublisherBase(rclcpp::Node * node) + : rclcpp::PublisherBase( + node->get_node_base_interface().get(), "topic", EmptyTypeSupport(), PublisherOptions()) {} +}; + +/* + Testing some publisher getters + */ +TEST_F(TestPublisher, basic_getters) { + initialize(); + using test_msgs::msg::Empty; + { + using rclcpp::QoS; + using rclcpp::KeepLast; + const size_t qos_depth_size = 10u; + auto publisher = node->create_publisher("topic", QoS(KeepLast(qos_depth_size))); + + size_t publisher_queue_size = publisher->get_queue_size(); + EXPECT_EQ(qos_depth_size, publisher_queue_size); + + const rmw_gid_t & publisher_rmw_gid = publisher->get_gid(); + EXPECT_NE(nullptr, publisher_rmw_gid.implementation_identifier); + + std::shared_ptr publisher_handle = publisher->get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + + EXPECT_TRUE(publisher->assert_liveliness()); + } + { + const TestPublisherBase publisher = TestPublisherBase(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + + const rmw_gid_t & publisher_rmw_gid = publisher.get_gid(); + EXPECT_NE(nullptr, publisher_rmw_gid.implementation_identifier); + + // Test == operator of publisher with rmw_gid_t + EXPECT_EQ(publisher, publisher_rmw_gid); + } +} diff --git a/rclcpp/test/rclcpp/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp index 4e28106a88..e9f201fc0c 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -140,6 +140,20 @@ TEST(TestSerializedMessage, release) { EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&released_handle)); } +TEST(TestSerializedMessage, reserve) { + rclcpp::SerializedMessage serialized_msg(13); + EXPECT_EQ(13u, serialized_msg.capacity()); + + // Resize using reserve method + serialized_msg.reserve(15); + EXPECT_EQ(15u, serialized_msg.capacity()); + + // Use invalid value throws exception + EXPECT_THROW( + {serialized_msg.reserve(-1);}, + rclcpp::exceptions::RCLBadAlloc); +} + TEST(TestSerializedMessage, serialization) { using MessageT = test_msgs::msg::BasicTypes; @@ -159,6 +173,85 @@ TEST(TestSerializedMessage, serialization) { } } +TEST(TestSerializedMessage, assignment_operators) { + const std::string content = "Hello World"; + const auto content_size = content.size() + 1; // accounting for null terminator + auto default_allocator = rcl_get_default_allocator(); + auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator); + ASSERT_EQ(RCL_RET_OK, ret); + + // manually copy some content + std::memcpy(rcl_serialized_msg.buffer, content.c_str(), content_size); + rcl_serialized_msg.buffer_length = content_size; + EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); + rclcpp::SerializedMessage serialized_message_to_assign(rcl_serialized_msg); + EXPECT_EQ(13u, serialized_message_to_assign.capacity()); + EXPECT_EQ(content_size, serialized_message_to_assign.size()); + + // Test copy assignment with = operator, on another rclcpp::SerializedMessage + rclcpp::SerializedMessage serialized_msg_copy(2); + EXPECT_EQ(2u, serialized_msg_copy.capacity()); + EXPECT_EQ(0u, serialized_msg_copy.size()); + serialized_msg_copy = serialized_message_to_assign; + EXPECT_EQ(13u, serialized_msg_copy.capacity()); + EXPECT_EQ(content_size, serialized_msg_copy.size()); + + // Test copy assignment with = operator, with a rcl_serialized_message_t + rclcpp::SerializedMessage serialized_msg_copy_rcl(2); + EXPECT_EQ(2u, serialized_msg_copy_rcl.capacity()); + EXPECT_EQ(0u, serialized_msg_copy_rcl.size()); + serialized_msg_copy_rcl = rcl_serialized_msg; + EXPECT_EQ(13u, serialized_msg_copy_rcl.capacity()); + EXPECT_EQ(content_size, serialized_msg_copy_rcl.size()); + + // Test move assignment with = operator + rclcpp::SerializedMessage serialized_msg_move(2); + EXPECT_EQ(2u, serialized_msg_move.capacity()); + EXPECT_EQ(0u, serialized_msg_move.size()); + serialized_msg_move = std::move(serialized_message_to_assign); + EXPECT_EQ(13u, serialized_msg_move.capacity()); + EXPECT_EQ(content_size, serialized_msg_move.size()); + EXPECT_EQ(0u, serialized_message_to_assign.capacity()); + EXPECT_EQ(0u, serialized_message_to_assign.size()); + + // Test move assignment with = operator, with a rcl_serialized_message_t + rclcpp::SerializedMessage serialized_msg_move_rcl(2); + EXPECT_EQ(2u, serialized_msg_move_rcl.capacity()); + EXPECT_EQ(0u, serialized_msg_move_rcl.size()); + serialized_msg_move_rcl = std::move(rcl_serialized_msg); + EXPECT_EQ(13u, serialized_msg_move_rcl.capacity()); + EXPECT_EQ(content_size, serialized_msg_move_rcl.size()); + EXPECT_EQ(0u, rcl_serialized_msg.buffer_capacity); + + // Error because it was moved + EXPECT_EQ(RCUTILS_RET_INVALID_ARGUMENT, rmw_serialized_message_fini(&rcl_serialized_msg)); +} + +TEST(TestSerializedMessage, failed_init_throws) { + rclcpp::SerializedMessage serialized_msg(13); + EXPECT_EQ(13u, serialized_msg.capacity()); + + // Constructor with invalid size throws exception + EXPECT_THROW( + {rclcpp::SerializedMessage serialized_msg_fail(-1);}, + rclcpp::exceptions::RCLBadAlloc); + + // Constructor copy with rmw_serialized bad msg throws + auto default_allocator = rcl_get_default_allocator(); + auto rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto ret = rmw_serialized_message_init(&rcl_serialized_msg, 13, &default_allocator); + ASSERT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); + rcl_serialized_msg.buffer_capacity = -1; + EXPECT_THROW( + {rclcpp::SerializedMessage serialized_msg_fail_2(rcl_serialized_msg);}, + rclcpp::exceptions::RCLBadAlloc); + + rcl_serialized_msg.buffer_capacity = 13; + EXPECT_EQ(RCL_RET_OK, rmw_serialized_message_fini(&rcl_serialized_msg)); +} + void serialize_default_ros_msg() { using MessageT = test_msgs::msg::BasicTypes; diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 9ab90e1208..8043178b7e 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -21,6 +21,8 @@ #include "rclcpp/rclcpp.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/empty.h" class TestService : public ::testing::Test { @@ -105,3 +107,39 @@ TEST_F(TestServiceSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidServiceNameError); } } + +/* Testing basic getters */ +TEST_F(TestService, basic_public_getters) { + using rcl_interfaces::srv::ListParameters; + auto callback = + [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { + }; + auto service = node->create_service("service", callback); + EXPECT_STREQ(service->get_service_name(), "/ns/service"); + std::shared_ptr service_handle = service->get_service_handle(); + EXPECT_NE(nullptr, service_handle); + + { + // Create a extern defined const service + auto node_handle_int = rclcpp::Node::make_shared("base_node"); + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + const rosidl_service_type_support_t * ts = + rosidl_typesupport_cpp::get_service_type_support_handle(); + rcl_ret_t ret = rcl_service_init( + &service_handle, + node_handle_int->get_node_base_interface()->get_rcl_node_handle(), + ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + rclcpp::AnyServiceCallback cb; + const rclcpp::Service base( + node_handle_int->get_node_base_interface()->get_shared_rcl_node_handle(), + &service_handle, cb); + // Use get_service_handle specific to const service + std::shared_ptr const_service_handle = base.get_service_handle(); + EXPECT_NE(nullptr, const_service_handle); + } +}