From 580c0f2801a4e8e393717ec5fdbe04b3e9318b3a Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Fri, 4 Sep 2020 16:24:18 -0300 Subject: [PATCH 1/8] Add tests serialize functions Signed-off-by: Jorge Perez --- .../test/rclcpp/test_serialized_message.cpp | 88 +++++++++++++++++++ 1 file changed, 88 insertions(+) diff --git a/rclcpp/test/rclcpp/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp index 4e28106a88..e79c4ea229 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -140,6 +140,18 @@ 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_ANY_THROW(serialized_msg.reserve(-1)); +} + TEST(TestSerializedMessage, serialization) { using MessageT = test_msgs::msg::BasicTypes; @@ -159,6 +171,82 @@ 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[content.size()] = '\0'; + 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_ANY_THROW(rclcpp::SerializedMessage serialized_msg_fail(-1);); + + // 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_ANY_THROW(rclcpp::SerializedMessage serialized_msg_fail_2(rcl_serialized_msg);); + + 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; From 0a3fb5ba9359808ae07d22675802c8ba7fa7c8d8 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 7 Sep 2020 18:58:21 -0300 Subject: [PATCH 2/8] Add test getter const get_service_handle Signed-off-by: Jorge Perez --- rclcpp/test/CMakeLists.txt | 1 + rclcpp/test/rclcpp/test_service.cpp | 38 +++++++++++++++++++++++++++++ 2 files changed, 39 insertions(+) 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_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 9ab90e1208..8043178b7e 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -21,6 +21,8 @@ #include "rclcpp/rclcpp.hpp" #include "rcl_interfaces/srv/list_parameters.hpp" +#include "test_msgs/srv/empty.hpp" +#include "test_msgs/srv/empty.h" class TestService : public ::testing::Test { @@ -105,3 +107,39 @@ TEST_F(TestServiceSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidServiceNameError); } } + +/* Testing basic getters */ +TEST_F(TestService, basic_public_getters) { + using rcl_interfaces::srv::ListParameters; + auto callback = + [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) { + }; + auto service = node->create_service("service", callback); + EXPECT_STREQ(service->get_service_name(), "/ns/service"); + std::shared_ptr service_handle = service->get_service_handle(); + EXPECT_NE(nullptr, service_handle); + + { + // Create a extern defined const service + auto node_handle_int = rclcpp::Node::make_shared("base_node"); + rcl_service_t service_handle = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + const rosidl_service_type_support_t * ts = + rosidl_typesupport_cpp::get_service_type_support_handle(); + rcl_ret_t ret = rcl_service_init( + &service_handle, + node_handle_int->get_node_base_interface()->get_rcl_node_handle(), + ts, "base_node_service", &service_options); + if (ret != RCL_RET_OK) { + FAIL(); + return; + } + rclcpp::AnyServiceCallback cb; + const rclcpp::Service base( + node_handle_int->get_node_base_interface()->get_shared_rcl_node_handle(), + &service_handle, cb); + // Use get_service_handle specific to const service + std::shared_ptr const_service_handle = base.get_service_handle(); + EXPECT_NE(nullptr, const_service_handle); + } +} From 588f74a969c4f78c1b59ab0183742d6a1f3d0f41 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 8 Sep 2020 14:22:56 -0300 Subject: [PATCH 3/8] Add basic tests getters publisher Signed-off-by: Jorge Perez --- rclcpp/test/rclcpp/test_publisher.cpp | 46 +++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index dce0b8caa8..7984e7e58a 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -203,3 +203,49 @@ TEST_F(TestPublisherSub, construction_and_destruction) { }, rclcpp::exceptions::InvalidTopicNameError); } } + +// Auxiliar 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; + { + auto publisher = node->create_publisher("topic", 42); + + size_t publisher_queue_size = publisher->get_queue_size(); + // TODO(blast545): get default rmw qos options to compare here + EXPECT_NE(800u, publisher_queue_size); + + const rmw_gid_t & publisher_rmw_gid_t = publisher->get_gid(); + EXPECT_NE(nullptr, publisher_rmw_gid_t.implementation_identifier); + + std::shared_ptr publisher_handle = publisher->get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } + { + const TestPublisherBase publisher = TestPublisherBase(node.get()); + std::shared_ptr publisher_handle = publisher.get_publisher_handle(); + EXPECT_NE(nullptr, publisher_handle); + } +} From 8338652398c2a1adcd94665d3050e20ef4f87a06 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 8 Sep 2020 17:22:45 -0300 Subject: [PATCH 4/8] Add == operator tests Signed-off-by: Jorge Perez --- rclcpp/test/rclcpp/test_publisher.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 7984e7e58a..c687d5ce1b 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -237,15 +237,23 @@ TEST_F(TestPublisher, basic_getters) { // TODO(blast545): get default rmw qos options to compare here EXPECT_NE(800u, publisher_queue_size); - const rmw_gid_t & publisher_rmw_gid_t = publisher->get_gid(); - EXPECT_NE(nullptr, publisher_rmw_gid_t.implementation_identifier); + 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); } } From 41fcb80477a0b6679949ab20dda30e8e2e66cdca Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Wed, 9 Sep 2020 12:35:44 -0300 Subject: [PATCH 5/8] Improve check on QOS depth Signed-off-by: Jorge Perez --- rclcpp/test/rclcpp/test_publisher.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index c687d5ce1b..d6ef8dacfa 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -231,11 +231,13 @@ TEST_F(TestPublisher, basic_getters) { initialize(); using test_msgs::msg::Empty; { - auto publisher = node->create_publisher("topic", 42); + 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(); - // TODO(blast545): get default rmw qos options to compare here - EXPECT_NE(800u, publisher_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); From 15c1b5b7044432a804434b2b1c40512f811fd1d6 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Wed, 9 Sep 2020 12:46:58 -0300 Subject: [PATCH 6/8] Remove extra line, copy directly string Signed-off-by: Jorge Perez --- rclcpp/test/rclcpp/test_serialized_message.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp index e79c4ea229..7078d8f454 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -180,8 +180,7 @@ TEST(TestSerializedMessage, assignment_operators) { 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[content.size()] = '\0'; + 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); From a85d6c3e6c5f60626ccbd957cafa3a2bf43bfef0 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Wed, 9 Sep 2020 13:24:37 -0300 Subject: [PATCH 7/8] Expect specific error throws Signed-off-by: Jorge Perez --- rclcpp/test/rclcpp/test_serialized_message.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/rclcpp/test_serialized_message.cpp b/rclcpp/test/rclcpp/test_serialized_message.cpp index 7078d8f454..e9f201fc0c 100644 --- a/rclcpp/test/rclcpp/test_serialized_message.cpp +++ b/rclcpp/test/rclcpp/test_serialized_message.cpp @@ -149,7 +149,9 @@ TEST(TestSerializedMessage, reserve) { EXPECT_EQ(15u, serialized_msg.capacity()); // Use invalid value throws exception - EXPECT_ANY_THROW(serialized_msg.reserve(-1)); + EXPECT_THROW( + {serialized_msg.reserve(-1);}, + rclcpp::exceptions::RCLBadAlloc); } TEST(TestSerializedMessage, serialization) { @@ -231,7 +233,9 @@ TEST(TestSerializedMessage, failed_init_throws) { EXPECT_EQ(13u, serialized_msg.capacity()); // Constructor with invalid size throws exception - EXPECT_ANY_THROW(rclcpp::SerializedMessage serialized_msg_fail(-1);); + 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(); @@ -240,7 +244,9 @@ TEST(TestSerializedMessage, failed_init_throws) { ASSERT_EQ(RCL_RET_OK, ret); EXPECT_EQ(13u, rcl_serialized_msg.buffer_capacity); rcl_serialized_msg.buffer_capacity = -1; - EXPECT_ANY_THROW(rclcpp::SerializedMessage serialized_msg_fail_2(rcl_serialized_msg);); + 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)); From b04a1b0528b9c2c36ebd7ff200df08e9a94b51aa Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Wed, 9 Sep 2020 18:16:02 -0300 Subject: [PATCH 8/8] Improve comment, peer review Signed-off-by: Jorge Perez --- rclcpp/test/rclcpp/test_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index d6ef8dacfa..0ae98cc74f 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -204,7 +204,7 @@ TEST_F(TestPublisherSub, construction_and_destruction) { } } -// Auxiliar class used to test getter for const PublisherBase +// 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();