diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 92796b42cc..b6d3ba517c 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -17,6 +17,25 @@ rosidl_generate_interfaces(${PROJECT_NAME}_test_msgs SKIP_INSTALL ) +ament_add_gtest( + test_allocator_common + rclcpp/allocator/test_allocator_common.cpp) +if(TARGET test_allocator_common) + target_link_libraries(test_allocator_common ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_allocator_deleter + rclcpp/allocator/test_allocator_deleter.cpp) +if(TARGET test_allocator_deleter) + target_link_libraries(test_allocator_deleter ${PROJECT_NAME}) +endif() +ament_add_gtest( + test_exceptions + rclcpp/exceptions/test_exceptions.cpp) +if(TARGET test_exceptions) + target_link_libraries(test_exceptions ${PROJECT_NAME} mimick) +endif() + # Increasing timeout because connext can take a long time to destroy nodes # TODO(brawner) remove when destroying Node for Connext is resolved. See: # https://github.com/ros2/rclcpp/issues/1250 @@ -102,6 +121,12 @@ if(TARGET test_function_traits) "rosidl_typesupport_cpp" ) endif() +ament_add_gtest( + test_future_return_code + rclcpp/test_future_return_code.cpp) +if(TARGET test_future_return_code) + target_link_libraries(test_future_return_code ${PROJECT_NAME}) +endif() ament_add_gmock(test_intra_process_manager rclcpp/test_intra_process_manager.cpp) if(TARGET test_intra_process_manager) ament_target_dependencies(test_intra_process_manager @@ -173,7 +198,7 @@ endif() ament_add_gtest(test_node_interfaces__node_base rclcpp/node_interfaces/test_node_base.cpp) if(TARGET test_node_interfaces__node_base) - target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_base ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_clock rclcpp/node_interfaces/test_node_clock.cpp) @@ -186,22 +211,22 @@ if(TARGET test_node_interfaces__node_graph) ament_target_dependencies( test_node_interfaces__node_graph "test_msgs") - target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_graph ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_parameters rclcpp/node_interfaces/test_node_parameters.cpp) if(TARGET test_node_interfaces__node_parameters) - target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_parameters ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_services rclcpp/node_interfaces/test_node_services.cpp) if(TARGET test_node_interfaces__node_services) - target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_services ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_timers rclcpp/node_interfaces/test_node_timers.cpp) if(TARGET test_node_interfaces__node_timers) - target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_timers ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_topics rclcpp/node_interfaces/test_node_topics.cpp) @@ -209,12 +234,12 @@ if(TARGET test_node_interfaces__node_topics) ament_target_dependencies( test_node_interfaces__node_topics "test_msgs") - target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_topics ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_node_interfaces__node_waitables rclcpp/node_interfaces/test_node_waitables.cpp) if(TARGET test_node_interfaces__node_waitables) - target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME}) + target_link_libraries(test_node_interfaces__node_waitables ${PROJECT_NAME} mimick) endif() # TODO(wjwwood): reenable these build failure tests when I can get Jenkins to ignore their output @@ -292,12 +317,13 @@ endif() ament_add_gtest(test_publisher rclcpp/test_publisher.cpp) if(TARGET test_publisher) ament_target_dependencies(test_publisher + "rcl" "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" "test_msgs" ) - target_link_libraries(test_publisher ${PROJECT_NAME}) + target_link_libraries(test_publisher ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_publisher_subscription_count_api rclcpp/test_publisher_subscription_count_api.cpp) if(TARGET test_publisher_subscription_count_api) @@ -379,7 +405,7 @@ if(TARGET test_subscription) "rosidl_typesupport_cpp" "test_msgs" ) - target_link_libraries(test_subscription ${PROJECT_NAME}) + target_link_libraries(test_subscription ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_subscription_publisher_count_api rclcpp/test_subscription_publisher_count_api.cpp) if(TARGET test_subscription_publisher_count_api) @@ -473,7 +499,7 @@ ament_add_gtest(test_utilities rclcpp/test_utilities.cpp if(TARGET test_utilities) ament_target_dependencies(test_utilities "rcl") - target_link_libraries(test_utilities ${PROJECT_NAME}) + target_link_libraries(test_utilities ${PROJECT_NAME} mimick) endif() ament_add_gtest(test_init rclcpp/test_init.cpp diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp new file mode 100644 index 0000000000..d3270e8e12 --- /dev/null +++ b/rclcpp/test/rclcpp/allocator/test_allocator_common.cpp @@ -0,0 +1,67 @@ +// 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/allocator/allocator_common.hpp" + +TEST(TestAllocatorCommon, retyped_allocate) { + std::allocator allocator; + void * untyped_allocator = &allocator; + void * allocated_mem = + rclcpp::allocator::retyped_allocate>(1u, untyped_allocator); + ASSERT_NE(nullptr, allocated_mem); + + auto code = [&untyped_allocator, allocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + allocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code()); + + allocated_mem = allocator.allocate(1); + ASSERT_NE(nullptr, allocated_mem); + void * reallocated_mem = + rclcpp::allocator::retyped_reallocate>( + allocated_mem, 2u, untyped_allocator); + ASSERT_NE(nullptr, reallocated_mem); + + auto code2 = [&untyped_allocator, reallocated_mem]() { + rclcpp::allocator::retyped_deallocate>( + reallocated_mem, untyped_allocator); + }; + EXPECT_NO_THROW(code2()); +} + +TEST(TestAllocatorCommon, get_rcl_allocator) { + std::allocator allocator; + auto rcl_allocator = rclcpp::allocator::get_rcl_allocator(allocator); + EXPECT_NE(nullptr, rcl_allocator.allocate); + EXPECT_NE(nullptr, rcl_allocator.deallocate); + EXPECT_NE(nullptr, rcl_allocator.reallocate); + EXPECT_NE(nullptr, rcl_allocator.zero_allocate); + // Not testing state as that may or may not be null depending on platform +} + +TEST(TestAllocatorCommon, get_void_rcl_allocator) { + std::allocator allocator; + auto rcl_allocator = + rclcpp::allocator::get_rcl_allocator>(allocator); + EXPECT_NE(nullptr, rcl_allocator.allocate); + EXPECT_NE(nullptr, rcl_allocator.deallocate); + EXPECT_NE(nullptr, rcl_allocator.reallocate); + EXPECT_NE(nullptr, rcl_allocator.zero_allocate); + // Not testing state as that may or may not be null depending on platform +} diff --git a/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp b/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp new file mode 100644 index 0000000000..ae963e9160 --- /dev/null +++ b/rclcpp/test/rclcpp/allocator/test_allocator_deleter.cpp @@ -0,0 +1,101 @@ +// 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/allocator/allocator_deleter.hpp" + +#include "../../utils/rclcpp_gtest_macros.hpp" + +TEST(TestAllocatorDeleter, construct_destruct) { + std::allocator allocator; + + rclcpp::allocator::AllocatorDeleter> deleter; + EXPECT_EQ(nullptr, deleter.get_allocator()); + deleter.set_allocator(&allocator); + EXPECT_EQ(&allocator, deleter.get_allocator()); + + rclcpp::allocator::AllocatorDeleter> deleter2(&allocator); + EXPECT_EQ(&allocator, deleter2.get_allocator()); + + rclcpp::allocator::AllocatorDeleter> deleter3(deleter2); + EXPECT_EQ(&allocator, deleter3.get_allocator()); +} + +TEST(TestAllocatorDeleter, delete) { + std::allocator allocator; + int * some_mem = allocator.allocate(1u); + ASSERT_NE(nullptr, some_mem); + + rclcpp::allocator::AllocatorDeleter> deleter(&allocator); + EXPECT_NO_THROW(deleter(some_mem)); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_AllocatorDeleter) { + using AllocatorT = std::allocator; + using DeleterT = rclcpp::allocator::AllocatorDeleter; + AllocatorT allocator; + DeleterT deleter(&allocator); + + std::allocator allocator2; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator2); + EXPECT_EQ(&allocator2, deleter.get_allocator()); + + auto throwing_statement = [&allocator]() { + DeleterT * null_del_ptr = nullptr; + rclcpp::allocator::set_allocator_for_deleter( + null_del_ptr, &allocator); + }; + RCLCPP_EXPECT_THROW_EQ( + throwing_statement(), + std::invalid_argument("Argument was NULL to set_allocator_for_deleter")); + + auto throwing_statement2 = [&deleter]() { + AllocatorT * null_alloc_ptr = nullptr; + rclcpp::allocator::set_allocator_for_deleter( + &deleter, null_alloc_ptr); + }; + + RCLCPP_EXPECT_THROW_EQ( + throwing_statement2(), + std::invalid_argument("Argument was NULL to set_allocator_for_deleter")); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_std_default_delete) { + using AllocatorT = std::allocator; + using DeleterT = std::default_delete; + auto not_throwing_statement = []() { + AllocatorT allocator; + DeleterT deleter; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator); + }; + EXPECT_NO_THROW(not_throwing_statement()); +} + +TEST(TestAllocatorDeleter, set_allocator_for_deleter_unexpected_template) { + class SomeAllocatorClass {}; + class SomeDeleterClass {}; + using AllocatorT = SomeAllocatorClass; + using DeleterT = SomeDeleterClass; + auto throwing_statement = []() { + DeleterT deleter; + AllocatorT allocator; + rclcpp::allocator::set_allocator_for_deleter(&deleter, &allocator); + }; + RCLCPP_EXPECT_THROW_EQ( + throwing_statement(), + std::runtime_error("Reached unexpected template specialization")); +} diff --git a/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp b/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp new file mode 100644 index 0000000000..184dfc7a06 --- /dev/null +++ b/rclcpp/test/rclcpp/exceptions/test_exceptions.cpp @@ -0,0 +1,58 @@ +// 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 "rclcpp/exceptions/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +TEST(TestExceptions, throw_from_rcl_error) { + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_BAD_ALLOC, ""), + rclcpp::exceptions::RCLBadAlloc); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, ""), + rclcpp::exceptions::RCLInvalidArgument); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_INVALID_ROS_ARGS, ""), + rclcpp::exceptions::RCLInvalidROSArgsError); + + EXPECT_THROW( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""), + rclcpp::exceptions::RCLError); + + RCLCPP_EXPECT_THROW_EQ( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_OK, ""), + std::invalid_argument("ret is RCL_RET_OK")); + + { + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_get_error_state, nullptr); + RCLCPP_EXPECT_THROW_EQ( + rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR, ""), + std::runtime_error("rcl error state is not set")); + } +} + +TEST(TestExceptions, basic_constructors) { + EXPECT_STREQ("node is invalid", rclcpp::exceptions::InvalidNodeError().what()); + rcl_error_state_t error_state{"error", __FILE__, __LINE__}; + EXPECT_STREQ( + "prefix: error not set", + rclcpp::exceptions::RCLInvalidROSArgsError(RCL_RET_ERROR, &error_state, "prefix: ").what()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp index cfe42d7819..a40bc712bd 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_base.cpp @@ -21,6 +21,11 @@ #include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_base.hpp" #include "rclcpp/rclcpp.hpp" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" class TestNodeBase : public ::testing::Test { @@ -36,6 +41,12 @@ class TestNodeBase : public ::testing::Test } }; +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) + TEST_F(TestNodeBase, construct_from_node) { std::shared_ptr node = std::make_shared("node", "ns"); @@ -58,3 +69,125 @@ TEST_F(TestNodeBase, construct_from_node) EXPECT_NE(nullptr, const_node_base->get_rcl_node_handle()); EXPECT_NE(nullptr, const_node_base->get_shared_rcl_node_handle()); } + +TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_init_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_error) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_ERROR); + + // This function is called only if rcl_node_init fails, so both mocked functions are required + // This just logs an error, so behavior shouldn't change + auto mock_guard_condition_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock_validate_node_name = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_invalid_argument) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock_validate_node_name = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLInvalidArgument); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_node_name_valid_rmw_node_name) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAME); + + // `rmw_validate_node_name` is only called if `rcl_node_init` returns INVALID_NAME + auto mock = mocking_utils::patch( + "lib:rclcpp", rmw_validate_node_name, [](const char *, int * validation_result, size_t *) + { + *validation_result = RMW_NODE_NAME_VALID; + return RMW_RET_OK; + }); + + RCLCPP_EXPECT_THROW_EQ( + std::make_shared("node", "ns").reset(), + std::runtime_error("valid rmw node name but invalid rcl node name")); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock_validate_namespace = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_ERROR); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_rmw_invalid_argument) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock_validate_namespace = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_validate_namespace, RMW_RET_INVALID_ARGUMENT); + + EXPECT_THROW( + std::make_shared("node", "ns").reset(), + rclcpp::exceptions::RCLInvalidArgument); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_rcl_invalid_namespace_valid_rmw_namespace) { + auto mock_node_init = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_node_init, RCL_RET_NODE_INVALID_NAMESPACE); + + // `rmw_validate_namespace` is only called if `rcl_node_init` returns INVALID_NAMESPACE + auto mock = mocking_utils::patch( + "lib:rclcpp", rmw_validate_namespace, [](const char *, int * validation_result, size_t *) + { + *validation_result = RMW_NAMESPACE_VALID; + return RMW_RET_OK; + }); + + RCLCPP_EXPECT_THROW_EQ( + std::make_shared("node", "ns").reset(), + std::runtime_error("valid rmw node namespace but invalid rcl node namespace")); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_node_init_fini_error) { + auto mock_node_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_node_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW(std::make_shared("node", "ns").reset()); +} + +TEST_F(TestNodeBase, construct_destruct_rcl_guard_condition_fini_error) { + auto mock_node_fini = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + + EXPECT_NO_THROW(std::make_shared("node", "ns").reset()); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp index a1922d56af..6a10366f8b 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_graph.cpp @@ -15,105 +15,172 @@ #include #include +#include #include +#include "rcl/graph.h" #include "rcl/node_options.h" +#include "rcl/remap.h" #include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_base.hpp" #include "rclcpp/node_interfaces/node_graph.hpp" #include "rclcpp/rclcpp.hpp" +#include "rcutils/strdup.h" #include "test_msgs/msg/empty.hpp" #include "test_msgs/srv/empty.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + +namespace +{ + +constexpr char node_name[] = "node"; +constexpr char node_namespace[] = "ns"; +constexpr char absolute_namespace[] = "/ns"; + +} // namespace + class TestNodeGraph : public ::testing::Test { public: void SetUp() { rclcpp::init(0, nullptr); + node_ = std::make_shared(node_name, node_namespace); + + // This dynamic cast is not necessary for the unittests, but instead is used to ensure + // the proper type is being tested and covered. + node_graph_ = + dynamic_cast(node_->get_node_graph_interface().get()); + ASSERT_NE(nullptr, node_graph_); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node() {return node_;} + + const rclcpp::node_interfaces::NodeGraph * node_graph() const {return node_graph_;} + +private: + std::shared_ptr node_; + rclcpp::node_interfaces::NodeGraph * node_graph_; }; TEST_F(TestNodeGraph, construct_from_node) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); - - auto topic_names_and_types = node_graph->get_topic_names_and_types(false); + auto topic_names_and_types = node_graph()->get_topic_names_and_types(false); EXPECT_LT(0u, topic_names_and_types.size()); - auto service_names_and_types = node_graph->get_service_names_and_types(); + auto service_names_and_types = node_graph()->get_service_names_and_types(); EXPECT_LT(0u, service_names_and_types.size()); - auto names = node_graph->get_node_names(); + auto names = node_graph()->get_node_names(); EXPECT_EQ(1u, names.size()); - auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); + auto names_and_namespaces = node_graph()->get_node_names_and_namespaces(); EXPECT_EQ(1u, names_and_namespaces.size()); - EXPECT_EQ(0u, node_graph->count_publishers("not_a_topic")); - EXPECT_EQ(0u, node_graph->count_subscribers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_publishers("not_a_topic")); + EXPECT_EQ(0u, node_graph()->count_subscribers("not_a_topic")); + EXPECT_NE(nullptr, node_graph()->get_graph_guard_condition()); + + // get_graph_event is non-const + EXPECT_NE(nullptr, node()->get_node_graph_interface()->get_graph_event()); + EXPECT_EQ(1u, node()->get_node_graph_interface()->count_graph_users()); } TEST_F(TestNodeGraph, get_topic_names_and_types) { - auto node = std::make_shared("node2", "ns"); - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); - auto topic_names_and_types = node_graph->get_topic_names_and_types(); + auto topic_names_and_types = node_graph()->get_topic_names_and_types(); EXPECT_LT(0u, topic_names_and_types.size()); } +TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_error) +{ + auto mock_get_topic_names = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_topic_names_and_types, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_topic_names_and_types(), + std::runtime_error( + "failed to get topic names and types: error not set, failed also to cleanup topic names and" + " types, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, get_topic_names_and_types_rcl_names_and_types_fini_error) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_topic_names_and_types(), + std::runtime_error("could not destroy topic names and types: error not set")); +} + TEST_F(TestNodeGraph, get_service_names_and_types) { - auto node = std::make_shared("node2", "ns"); - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); - auto service_names_and_types = node_graph->get_service_names_and_types(); + auto service_names_and_types = node_graph()->get_service_names_and_types(); EXPECT_LT(0u, service_names_and_types.size()); } +TEST_F(TestNodeGraph, get_service_names_and_types_rcl_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_service_names_and_types, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types(), + std::runtime_error( + "failed to get service names and types: error not set, failed also to cleanup service names" + " and types, leaking memory: error not set")); +} + +TEST_F(TestNodeGraph, get_service_names_and_types_rcl_names_and_types_fini) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types(), + std::runtime_error("could not destroy service names and types: error not set")); +} + TEST_F(TestNodeGraph, get_service_names_and_types_by_node) { - auto node1 = std::make_shared("node1", "ns"); auto callback = []( const test_msgs::srv::Empty::Request::SharedPtr, test_msgs::srv::Empty::Response::SharedPtr) {}; auto service = - node1->create_service("node1_service", std::move(callback)); - auto node2 = std::make_shared("node2", "ns"); - const auto * node_graph = - dynamic_cast(node1->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + node()->create_service("node1_service", std::move(callback)); + + const std::string node2_name = "node2"; + auto node2 = std::make_shared(node2_name, node_namespace); // rcl_get_service_names_and_types_by_node() expects the node to exist, otherwise it fails EXPECT_THROW( - node_graph->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), + node_graph()->get_service_names_and_types_by_node("not_a_node", "not_absolute_namespace"), std::runtime_error); // Check that node1_service exists for node1 but not node2. This shouldn't exercise graph // discovery as node_graph belongs to node1 anyway. This is just to test the API itself. auto services_of_node1 = - node_graph->get_service_names_and_types_by_node("node1", "/ns"); + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace); auto services_of_node2 = - node_graph->get_service_names_and_types_by_node("node2", "/ns"); + node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace); auto start = std::chrono::steady_clock::now(); while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { - services_of_node1 = node_graph->get_service_names_and_types_by_node("node1", "/ns"); - services_of_node2 = node_graph->get_service_names_and_types_by_node("node2", "/ns"); + services_of_node1 = + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace); + services_of_node2 = + node_graph()->get_service_names_and_types_by_node(node2_name, absolute_namespace); if (services_of_node1.find("/ns/node1_service") != services_of_node1.end()) { break; } @@ -123,70 +190,134 @@ TEST_F(TestNodeGraph, get_service_names_and_types_by_node) EXPECT_FALSE(services_of_node2.find("/ns/node1_service") != services_of_node2.end()); } -TEST_F(TestNodeGraph, get_node_names_and_namespaces) +TEST_F(TestNodeGraph, get_service_names_and_types_by_node_rcl_errors) { - auto node = std::make_shared("node", "ns"); - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node()->create_service("node1_service", std::move(callback)); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_service_names_and_types_by_node, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_service_names_and_types_by_node(node_name, node_namespace), + std::runtime_error( + "failed to get service names and types by node: error not set, failed also to cleanup" + " service names and types, leaking memory: error not set")); +} - auto names_and_namespaces = node_graph->get_node_names_and_namespaces(); +TEST_F(TestNodeGraph, get_service_names_and_types_by_node_names_and_types_fini_error) +{ + auto callback = []( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {}; + auto service = + node()->create_service("node1_service", std::move(callback)); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_names_and_types_fini, RCL_RET_ERROR); + + EXPECT_THROW( + node_graph()->get_service_names_and_types_by_node(node_name, absolute_namespace), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_node_names_and_namespaces) +{ + auto names_and_namespaces = node_graph()->get_node_names_and_namespaces(); EXPECT_EQ(1u, names_and_namespaces.size()); } -TEST_F(TestNodeGraph, notify_shutdown) +TEST_F(TestNodeGraph, get_node_names_and_namespaces_rcl_errors) { - auto node = std::make_shared("node", "ns"); - auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_node_names, RCL_RET_ERROR); + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_and_namespaces(), + std::runtime_error( + "failed to get node names: error not set, failed also to cleanup node names, leaking memory:" + " error not set, failed also to cleanup node namespaces, leaking memory: error not set")); +} - EXPECT_NO_THROW(node_graph->notify_shutdown()); +TEST_F(TestNodeGraph, get_node_names_and_namespaces_fini_errors) +{ + auto mock_names_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcutils_string_array_fini, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_node_names_and_namespaces(), + std::runtime_error("could not destroy node names, could not destroy node namespaces")); } -TEST_F(TestNodeGraph, wait_for_graph_change) +TEST_F(TestNodeGraph, count_publishers_rcl_error) { - std::shared_ptr node = std::make_shared("node", "ns"); - auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_publishers, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_publishers("topic"), + std::runtime_error("could not count publishers: error not set")); +} + +TEST_F(TestNodeGraph, count_subscribers_rcl_error) +{ + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_count_subscribers, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->count_subscribers("topic"), + std::runtime_error("could not count subscribers: error not set")); +} + +TEST_F(TestNodeGraph, notify_shutdown) +{ + EXPECT_NO_THROW(node()->get_node_graph_interface()->notify_shutdown()); +} - EXPECT_NO_THROW(node_graph->notify_graph_change()); +TEST_F(TestNodeGraph, wait_for_graph_change) +{ + auto node_graph_interface = node()->get_node_graph_interface(); + EXPECT_NO_THROW(node_graph_interface->notify_graph_change()); EXPECT_THROW( - node_graph->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), + node_graph_interface->wait_for_graph_change(nullptr, std::chrono::milliseconds(1)), rclcpp::exceptions::InvalidEventError); auto event = std::make_shared(); EXPECT_THROW( - node_graph->wait_for_graph_change(event, std::chrono::milliseconds(0)), + node_graph_interface->wait_for_graph_change(event, std::chrono::milliseconds(0)), rclcpp::exceptions::EventNotRegisteredError); } +TEST_F(TestNodeGraph, notify_graph_change_rcl_error) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + EXPECT_THROW( + node()->get_node_graph_interface()->notify_graph_change(), + rclcpp::exceptions::RCLError); +} + TEST_F(TestNodeGraph, get_info_by_topic) { - std::shared_ptr node = std::make_shared("node", "ns"); const rclcpp::QoS publisher_qos(1); - auto publisher = node->create_publisher("topic", publisher_qos); + auto publisher = node()->create_publisher("topic", publisher_qos); auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; const rclcpp::QoS subscriber_qos(10); auto subscription = - node->create_subscription( + node()->create_subscription( "topic", subscriber_qos, std::move(callback)); - const auto * node_graph = - dynamic_cast(node->get_node_graph_interface().get()); - ASSERT_NE(nullptr, node_graph); + EXPECT_EQ(0u, node_graph()->get_publishers_info_by_topic("topic", true).size()); - auto publishers = node_graph->get_publishers_info_by_topic("topic", false); + auto publishers = node_graph()->get_publishers_info_by_topic("topic", false); ASSERT_EQ(1u, publishers.size()); auto publisher_endpoint_info = publishers[0]; const auto const_publisher_endpoint_info = publisher_endpoint_info; - EXPECT_STREQ("node", publisher_endpoint_info.node_name().c_str()); - EXPECT_STREQ("node", const_publisher_endpoint_info.node_name().c_str()); - EXPECT_STREQ("/ns", publisher_endpoint_info.node_namespace().c_str()); - EXPECT_STREQ("/ns", const_publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ(node_name, publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ(node_name, const_publisher_endpoint_info.node_name().c_str()); + EXPECT_STREQ(absolute_namespace, publisher_endpoint_info.node_namespace().c_str()); + EXPECT_STREQ(absolute_namespace, const_publisher_endpoint_info.node_namespace().c_str()); EXPECT_STREQ("test_msgs/msg/Empty", publisher_endpoint_info.topic_type().c_str()); EXPECT_STREQ("test_msgs/msg/Empty", const_publisher_endpoint_info.topic_type().c_str()); EXPECT_EQ(rclcpp::EndpointType::Publisher, publisher_endpoint_info.endpoint_type()); @@ -225,3 +356,90 @@ TEST_F(TestNodeGraph, get_info_by_topic) } EXPECT_FALSE(endpoint_gid_is_all_zeros); } + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_node_get_options_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_node_get_options, nullptr); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_publishers_info_by_topic("topic", false), + std::runtime_error("Need valid node options in get_info_by_topic()")); +} + +// Required for mocking_utils below +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_remap_topic_name, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_graph()->get_publishers_info_by_topic("topic", false), + std::runtime_error("Failed to remap topic name /ns/topic: error not set")); +} + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_remap_topic_name_nullptr) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + // Should be cleaned up by get_info_by_topic + char * some_string = rcutils_strdup("", rcl_get_default_allocator()); + ASSERT_NE(nullptr, some_string); + auto mock = + mocking_utils::patch( + "lib:rclcpp", rcl_remap_topic_name, [&some_string]( + const rcl_arguments_t *, const rcl_arguments_t *, const char *, const char *, const char *, + rcl_allocator_t, char ** output_name) + { + *output_name = some_string; + return RCL_RET_OK; + }); + EXPECT_NO_THROW(node_graph()->get_publishers_info_by_topic("topic", false)); +} + +TEST_F(TestNodeGraph, get_info_by_topic_rcl_errors) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_ERROR); + auto mock_info_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_info_by_topic_unsupported) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_get_publishers_info_by_topic, RCL_RET_UNSUPPORTED); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestNodeGraph, get_info_by_topic_endpoint_info_array_fini_error) +{ + const rclcpp::QoS publisher_qos(1); + auto publisher = node()->create_publisher("topic", publisher_qos); + + auto mock_info_array_fini = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_topic_endpoint_info_array_fini, RCL_RET_ERROR); + EXPECT_THROW( + node_graph()->get_publishers_info_by_topic("topic", false), + rclcpp::exceptions::RCLError); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp index 68a7546c83..786422d0d2 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp @@ -28,31 +28,47 @@ #include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_parameters.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + class TestNodeParameters : public ::testing::Test { public: void SetUp() { rclcpp::init(0, nullptr); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + node = std::make_shared("node", "ns", options); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_parameters = + dynamic_cast( + node->get_node_parameters_interface().get()); + ASSERT_NE(nullptr, node_parameters); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeParameters * node_parameters; }; +TEST_F(TestNodeParameters, construct_destruct_rcl_errors) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_arguments_get_param_overrides, RCL_RET_ERROR); + EXPECT_THROW( + std::make_shared("node2", "ns").reset(), + rclcpp::exceptions::RCLError); +} + TEST_F(TestNodeParameters, list_parameters) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_parameters = - dynamic_cast( - node->get_node_parameters_interface().get()); - ASSERT_NE(nullptr, node_parameters); - std::vector prefixes; const auto list_result = node_parameters->list_parameters(prefixes, 1u); @@ -73,17 +89,113 @@ TEST_F(TestNodeParameters, list_parameters) EXPECT_NE( std::find(list_result2.names.begin(), list_result2.names.end(), parameter_name), list_result2.names.end()); + + // Check prefixes + const std::string parameter_name2 = "prefix.new_parameter"; + const rclcpp::ParameterValue value2(true); + const rcl_interfaces::msg::ParameterDescriptor descriptor2; + const auto added_parameter_value2 = + node_parameters->declare_parameter(parameter_name2, value2, descriptor2, false); + EXPECT_EQ(value.get(), added_parameter_value.get()); + prefixes = {"prefix"}; + auto list_result3 = node_parameters->list_parameters(prefixes, 2u); + EXPECT_EQ(1u, list_result3.names.size()); + EXPECT_NE( + std::find(list_result3.names.begin(), list_result3.names.end(), parameter_name2), + list_result3.names.end()); + + // Check if prefix equals parameter name + prefixes = {"new_parameter"}; + auto list_result4 = node_parameters->list_parameters(prefixes, 2u); + EXPECT_EQ(1u, list_result4.names.size()); + EXPECT_NE( + std::find(list_result4.names.begin(), list_result4.names.end(), parameter_name), + list_result4.names.end()); } TEST_F(TestNodeParameters, parameter_overrides) { - std::shared_ptr node = std::make_shared("node", "ns"); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + node_options.append_parameter_override("param1", true); + node_options.append_parameter_override("param2", 42); + + std::shared_ptr node2 = std::make_shared("node2", "ns", node_options); - auto * node_parameters = + auto * node_parameters_interface = dynamic_cast( - node->get_node_parameters_interface().get()); + node2->get_node_parameters_interface().get()); ASSERT_NE(nullptr, node_parameters); - const auto & parameter_overrides = node_parameters->get_parameter_overrides(); - EXPECT_EQ(0u, parameter_overrides.size()); + const auto & parameter_overrides = node_parameters_interface->get_parameter_overrides(); + EXPECT_EQ(2u, parameter_overrides.size()); +} + +TEST_F(TestNodeParameters, set_parameters) { + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true); + + rcl_interfaces::msg::ParameterDescriptor bool_descriptor; + bool_descriptor.name = "bool_parameter"; + bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + bool_descriptor.read_only = false; + node_parameters->declare_parameter( + "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false); + + rcl_interfaces::msg::ParameterDescriptor read_only_descriptor; + read_only_descriptor.name = "read_only_parameter"; + read_only_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + read_only_descriptor.read_only = true; + node_parameters->declare_parameter( + "read_only_parameter", rclcpp::ParameterValue(42), read_only_descriptor, false); + + const std::vector parameters = { + rclcpp::Parameter("bool_parameter", true), + rclcpp::Parameter("read_only_parameter", 42), + }; + auto result = node_parameters->set_parameters(parameters); + ASSERT_EQ(parameters.size(), result.size()); + EXPECT_TRUE(result[0].successful); + EXPECT_FALSE(result[1].successful); + EXPECT_STREQ( + "parameter 'read_only_parameter' cannot be set because it is read-only", + result[1].reason.c_str()); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->set_parameters({rclcpp::Parameter("", true)}), + rclcpp::exceptions::InvalidParametersException("parameter name must not be empty")); + + result = node_parameters->set_parameters({rclcpp::Parameter("undeclared_parameter", 3.14159)}); + ASSERT_EQ(1u, result.size()); + EXPECT_TRUE(result[0].successful); +} + +TEST_F(TestNodeParameters, add_remove_parameters_callback) { + rcl_interfaces::msg::ParameterDescriptor bool_descriptor; + bool_descriptor.name = "bool_parameter"; + bool_descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + bool_descriptor.read_only = false; + node_parameters->declare_parameter( + "bool_parameter", rclcpp::ParameterValue(false), bool_descriptor, false); + const std::vector parameters = {rclcpp::Parameter("bool_parameter", true)}; + + const std::string reason = "some totally not made up reason"; + auto callback = [reason](const std::vector &) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = reason; + return result; + }; + + auto handle = node_parameters->add_on_set_parameters_callback(callback); + auto result = node_parameters->set_parameters(parameters); + ASSERT_EQ(1u, result.size()); + EXPECT_FALSE(result[0].successful); + EXPECT_EQ(reason, result[0].reason); + + EXPECT_NO_THROW(node_parameters->remove_on_set_parameters_callback(handle.get())); + + RCLCPP_EXPECT_THROW_EQ( + node_parameters->remove_on_set_parameters_callback(handle.get()), + std::runtime_error("Callback doesn't exist")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp index 893340d33a..a48c50254a 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_services.cpp @@ -22,6 +22,9 @@ #include "rclcpp/node_interfaces/node_services.hpp" #include "rclcpp/rclcpp.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + class TestService : public rclcpp::ServiceBase { public: @@ -51,25 +54,29 @@ class TestNodeService : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + + node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_services = + dynamic_cast( + node->get_node_services_interface().get()); + ASSERT_NE(nullptr, node_services); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeServices * node_services; }; TEST_F(TestNodeService, add_service) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_services = - dynamic_cast( - node->get_node_services_interface().get()); - ASSERT_NE(nullptr, node_services); - auto service = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW( @@ -80,22 +87,24 @@ TEST_F(TestNodeService, add_service) auto callback_group_in_different_node = different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node_services->add_service(service, callback_group_in_different_node), - std::runtime_error); + std::runtime_error("Cannot create service, group not in node.")); } -TEST_F(TestNodeService, add_client) +TEST_F(TestNodeService, add_service_rcl_trigger_guard_condition_error) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_services = - dynamic_cast( - node->get_node_services_interface().get()); - ASSERT_NE(nullptr, node_services); + auto service = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_services->add_service(service, callback_group), + std::runtime_error("Failed to notify wait set on service creation: error not set")); +} +TEST_F(TestNodeService, add_client) +{ auto client = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW(node_services->add_client(client, callback_group)); @@ -105,7 +114,18 @@ TEST_F(TestNodeService, add_client) auto callback_group_in_different_node = different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node_services->add_client(client, callback_group_in_different_node), - std::runtime_error); + std::runtime_error("Cannot create client, group not in node.")); +} + +TEST_F(TestNodeService, add_client_rcl_trigger_guard_condition_error) +{ + auto client = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_services->add_client(client, callback_group), + std::runtime_error("Failed to notify wait set on client creation: error not set")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp index af340d292e..e206e75b6c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_timers.cpp @@ -22,6 +22,9 @@ #include "rclcpp/node_interfaces/node_timers.hpp" #include "rclcpp/rclcpp.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + class TestTimer : public rclcpp::TimerBase { public: @@ -39,23 +42,27 @@ class TestNodeTimers : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_timers = + dynamic_cast(node->get_node_timers_interface().get()); + ASSERT_NE(nullptr, node_timers); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeTimers * node_timers; }; TEST_F(TestNodeTimers, add_timer) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto node_timers = - dynamic_cast(node->get_node_timers_interface().get()); - ASSERT_NE(nullptr, node_timers); auto timer = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW(node_timers->add_timer(timer, callback_group)); @@ -65,7 +72,19 @@ TEST_F(TestNodeTimers, add_timer) auto callback_group_in_different_node = different_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node_timers->add_timer(timer, callback_group_in_different_node), - std::runtime_error); + std::runtime_error("Cannot create timer, group not in node.")); +} + +TEST_F(TestNodeTimers, add_timer_rcl_trigger_guard_condition_error) +{ + auto timer = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_timers->add_timer(timer, callback_group), + std::runtime_error("Failed to notify wait set on timer creation: error not set")); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp index 86b4e72e36..9b1d4f374e 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_topics.cpp @@ -24,6 +24,9 @@ #include "rclcpp/rclcpp.hpp" #include "test_msgs/msg/empty.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + namespace { @@ -77,23 +80,27 @@ class TestNodeTopics : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure + // the proper type is being tested and covered. + node_topics = + dynamic_cast(node->get_node_topics_interface().get()); + ASSERT_NE(nullptr, node_topics); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeTopics * node_topics; }; TEST_F(TestNodeTopics, add_publisher) { - std::shared_ptr node = std::make_shared("node", "ns"); - - // This dynamic cast is not necessary for the unittest itself, but instead is used to ensure - // the proper type is being tested and covered. - auto * node_topics = - dynamic_cast(node->get_node_topics_interface().get()); - ASSERT_NE(nullptr, node_topics); auto publisher = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW(node_topics->add_publisher(publisher, callback_group)); @@ -108,12 +115,20 @@ TEST_F(TestNodeTopics, add_publisher) std::runtime_error); } +TEST_F(TestNodeTopics, add_publisher_rcl_trigger_guard_condition_error) +{ + auto publisher = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_topics->add_publisher(publisher, callback_group), + std::runtime_error("Failed to notify wait set on publisher creation: error not set")); +} + TEST_F(TestNodeTopics, add_subscription) { - std::shared_ptr node = std::make_shared("node", "ns"); - auto * node_topics = - dynamic_cast(node->get_node_topics_interface().get()); - ASSERT_NE(nullptr, node_topics); auto subscription = std::make_shared(node.get()); auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); EXPECT_NO_THROW(node_topics->add_subscription(subscription, callback_group)); @@ -127,3 +142,15 @@ TEST_F(TestNodeTopics, add_subscription) node_topics->add_subscription(subscription, callback_group_in_different_node), std::runtime_error); } + +TEST_F(TestNodeTopics, add_subscription_rcl_trigger_guard_condition_error) +{ + auto subscription = std::make_shared(node.get()); + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_topics->add_subscription(subscription, callback_group), + std::runtime_error("failed to notify wait set on subscription creation: error not set")); +} diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index 19cc63fe07..b20aff864c 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -22,6 +22,9 @@ #include "rclcpp/node_interfaces/node_waitables.hpp" #include "rclcpp/rclcpp.hpp" +#include "../../mocking_utils/patch.hpp" +#include "../../utils/rclcpp_gtest_macros.hpp" + class TestWaitable : public rclcpp::Waitable { public: @@ -36,23 +39,26 @@ class TestNodeWaitables : public ::testing::Test void SetUp() { rclcpp::init(0, nullptr); + node = std::make_shared("node", "ns"); + + node_waitables = + dynamic_cast( + node->get_node_waitables_interface().get()); + ASSERT_NE(nullptr, node_waitables); } void TearDown() { rclcpp::shutdown(); } + +protected: + std::shared_ptr node; + rclcpp::node_interfaces::NodeWaitables * node_waitables; }; TEST_F(TestNodeWaitables, add_remove_waitable) { - std::shared_ptr node = std::make_shared("node", "ns"); - - auto * node_waitables = - dynamic_cast( - node->get_node_waitables_interface().get()); - ASSERT_NE(nullptr, node_waitables); - std::shared_ptr node2 = std::make_shared("node2", "ns"); auto callback_group1 = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -60,9 +66,25 @@ TEST_F(TestNodeWaitables, add_remove_waitable) auto waitable = std::make_shared(); EXPECT_NO_THROW( node_waitables->add_waitable(waitable, callback_group1)); - EXPECT_THROW( + RCLCPP_EXPECT_THROW_EQ( node_waitables->add_waitable(waitable, callback_group2), - std::runtime_error); + std::runtime_error("Cannot create waitable, group not in node.")); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group1)); EXPECT_NO_THROW(node_waitables->remove_waitable(waitable, callback_group2)); + + auto waitable2 = std::make_shared(); + EXPECT_NO_THROW(node_waitables->add_waitable(waitable2, nullptr)); + EXPECT_NO_THROW(node_waitables->remove_waitable(waitable2, nullptr)); +} + +TEST_F(TestNodeWaitables, add_waitable_rcl_trigger_guard_condition_error) +{ + auto callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto waitable = std::make_shared(); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node_waitables->add_waitable(waitable, callback_group), + std::runtime_error("Failed to notify wait set on waitable creation: error not set")); } diff --git a/rclcpp/test/rclcpp/test_future_return_code.cpp b/rclcpp/test/rclcpp/test_future_return_code.cpp new file mode 100644 index 0000000000..f4647b6e29 --- /dev/null +++ b/rclcpp/test/rclcpp/test_future_return_code.cpp @@ -0,0 +1,34 @@ +// Copyright 2015 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/future_return_code.hpp" + +TEST(TestFutureReturnCode, to_string) { + EXPECT_EQ( + "Unknown enum value (-1)", rclcpp::to_string(rclcpp::FutureReturnCode(-1))); + EXPECT_EQ( + "SUCCESS (0)", rclcpp::to_string(rclcpp::FutureReturnCode::SUCCESS)); + EXPECT_EQ( + "INTERRUPTED (1)", rclcpp::to_string(rclcpp::FutureReturnCode::INTERRUPTED)); + EXPECT_EQ( + "TIMEOUT (2)", rclcpp::to_string(rclcpp::FutureReturnCode::TIMEOUT)); + EXPECT_EQ( + "Unknown enum value (3)", rclcpp::to_string(rclcpp::FutureReturnCode(3))); + EXPECT_EQ( + "Unknown enum value (100)", rclcpp::to_string(rclcpp::FutureReturnCode(100))); +} diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 8543d9b975..d522b72b54 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/rclcpp/test/rclcpp/test_node_options.cpp @@ -198,3 +198,12 @@ TEST(TestNodeOptions, copy) { rcl_arguments_get_count_unparsed(&rcl_options->arguments)); } } + +TEST(TestNodeOptions, append_parameter_override) { + std::vector expected_args{"--unknown-flag", "arg"}; + auto options = rclcpp::NodeOptions().arguments(expected_args).use_global_arguments(false); + rclcpp::Parameter parameter("some_parameter", 10); + options.append_parameter_override("some_parameter", 10); + EXPECT_EQ(1u, options.parameter_overrides().size()); + EXPECT_EQ(std::string("some_parameter"), options.parameter_overrides()[0].get_name()); +} diff --git a/rclcpp/test/rclcpp/test_parameter.cpp b/rclcpp/test/rclcpp/test_parameter.cpp index f7244e4488..0c93204dcb 100644 --- a/rclcpp/test/rclcpp/test_parameter.cpp +++ b/rclcpp/test/rclcpp/test_parameter.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -32,6 +33,12 @@ class TestParameter : public ::testing::Test } }; +TEST_F(TestParameter, construct_destruct) { + EXPECT_NO_THROW(std::make_shared().reset()); + EXPECT_NO_THROW(std::make_shared("some_parameter").reset()); + EXPECT_NO_THROW(std::make_shared("some_parameter", 10).reset()); +} + TEST_F(TestParameter, not_set_variant) { // Direct instantiation rclcpp::Parameter not_set_variant; @@ -56,6 +63,10 @@ TEST_F(TestParameter, not_set_variant) { EXPECT_EQ( rclcpp::ParameterType::PARAMETER_NOT_SET, rclcpp::Parameter::from_parameter_msg(not_set_param).get_type()); + + EXPECT_THROW( + not_set_variant.get_value(), + rclcpp::exceptions::InvalidParameterTypeException); } TEST_F(TestParameter, bool_variant) { diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 17b44d6490..523cd3fdce 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -19,6 +19,8 @@ #include "rclcpp/rclcpp.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + #include "rcl_interfaces/msg/parameter_event.hpp" class TestParameterClient : public ::testing::Test @@ -159,3 +161,24 @@ TEST_F(TestParameterClient, sync_parameter_event_subscription) { (void)event_sub; } } + +/* + Coverage for simple get_parameter methods + */ +TEST_F(TestParameterClient, sync_parameter_get_parameter) { + rclcpp::SyncParametersClient client(node); + EXPECT_EQ(10, client.get_parameter("not_a_parameter", 10)); + + RCLCPP_EXPECT_THROW_EQ( + client.get_parameter("not_a_parameter"), + std::runtime_error("Parameter 'not_a_parameter' is not set")); +} + +/* + Coverage for async waiting/is_ready + */ +TEST_F(TestParameterClient, sync_parameter_is_ready) { + rclcpp::SyncParametersClient client(node); + EXPECT_TRUE(client.wait_for_service()); + EXPECT_TRUE(client.service_is_ready()); +} diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 0ae98cc74f..01a1ee2fde 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -16,11 +16,17 @@ #include #include +#include #include +#include "rcl/publisher.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + #include "test_msgs/msg/empty.hpp" class TestPublisher : public ::testing::Test @@ -259,3 +265,182 @@ TEST_F(TestPublisher, basic_getters) { EXPECT_EQ(publisher, publisher_rmw_gid); } } + +TEST_F(TestPublisher, rcl_publisher_init_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_init, RCL_RET_ERROR); + EXPECT_THROW( + node->create_publisher("topic", 10).reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestPublisher, rcl_publisher_get_rmw_handle_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_get_rmw_handle, nullptr); + RCLCPP_EXPECT_THROW_EQ( + node->create_publisher("topic", 10), + std::runtime_error("failed to get rmw handle: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_get_gid_for_publisher_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_get_gid_for_publisher, RMW_RET_ERROR); + RCLCPP_EXPECT_THROW_EQ( + node->create_publisher("topic", 10), + std::runtime_error("failed to get publisher gid: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_fini_error) { + initialize(); + auto mock = mocking_utils::inject_on_return("lib:rclcpp", rcl_publisher_fini, RCL_RET_ERROR); + auto publisher = node->create_publisher("topic", 10); + ASSERT_EQ(1, publisher.use_count()); + // Failure in rcl_publisher_fini should just log error + EXPECT_NO_THROW(publisher.reset()); +} + +TEST_F(TestPublisher, rcl_publisher_get_options_error) { + initialize(); + auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_publisher_get_options, nullptr); + auto publisher = node->create_publisher("topic", 10); + RCLCPP_EXPECT_THROW_EQ( + publisher->get_queue_size(), + std::runtime_error("failed to get publisher options: error not set")); +} + +TEST_F(TestPublisher, rcl_publisher_get_subscription_count_publisher_invalid) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_subscription_count, RCL_RET_PUBLISHER_INVALID); + auto publisher = node->create_publisher("topic", 10); + EXPECT_THROW( + publisher->get_subscription_count(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestPublisher, rcl_publisher_get_actual_qos_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publisher_get_actual_qos, nullptr); + auto publisher = node->create_publisher("topic", 10); + RCLCPP_EXPECT_THROW_EQ( + publisher->get_actual_qos(), + std::runtime_error("failed to get qos settings: error not set")); +} + +TEST_F(TestPublisher, publishers_equal_rmw_compare_gids_error) { + initialize(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rmw_compare_gids_equal, RMW_RET_ERROR); + const auto publisher = node->create_publisher("topic", 10); + const rmw_gid_t * gid = nullptr; + auto throwing_fn = [publisher, gid]() + { + // The == operator is expected to throw here, but this lambda avoids unused result warning + return (*publisher.get() == gid) ? true : false; + }; + + RCLCPP_EXPECT_THROW_EQ( + throwing_fn(), + std::runtime_error("failed to compare gids: error not set")); +} + +TEST_F(TestPublisher, intra_process_publish_failures) { + initialize(); + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + auto publisher = node->create_publisher("topic", 10, options); + + auto msg_unique = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_unique))); + + rclcpp::SerializedMessage serialized_msg; + RCLCPP_EXPECT_THROW_EQ( + publisher->publish(serialized_msg), + std::runtime_error("storing serialized messages in intra process is not supported yet")); + + std::allocator allocator; + { + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + RCLCPP_EXPECT_THROW_EQ( + publisher->publish(std::move(loaned_msg)), + std::runtime_error("storing loaned messages in intra process is not supported yet")); + } + + { + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + loaned_msg.release(); + RCLCPP_EXPECT_THROW_EQ( + publisher->publish(std::move(loaned_msg)), + std::runtime_error("loaned message is not valid")); + } +} + +TEST_F(TestPublisher, inter_process_publish_failures) { + initialize(); + rclcpp::PublisherOptionsWithAllocator> options; + options.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + auto publisher = node->create_publisher("topic", 10, options); + + auto msg_unique = std::make_unique(); + EXPECT_NO_THROW(publisher->publish(std::move(msg_unique))); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_publish, RCL_RET_PUBLISHER_INVALID); + test_msgs::msg::Empty msg; + EXPECT_THROW(publisher->publish(msg), rclcpp::exceptions::RCLError); + } + + rclcpp::SerializedMessage serialized_msg; + EXPECT_NO_THROW(publisher->publish(serialized_msg)); + + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_serialized_message` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_serialized_message, RCL_RET_ERROR); + EXPECT_THROW(publisher->publish(serialized_msg), rclcpp::exceptions::RCLError); + } + + std::allocator allocator; + rclcpp::LoanedMessage loaned_msg(*publisher, allocator); + EXPECT_NO_THROW(publisher->publish(std::move(loaned_msg))); +} + +template> +class TestPublisherProtectedMethods : public rclcpp::Publisher +{ +public: + using rclcpp::Publisher::Publisher; + + void publish_loaned_message(MessageT * msg) + { + this->do_loaned_message_publish(msg); + } +}; + +TEST_F(TestPublisher, do_loaned_message_publish_error) { + initialize(); + using PublisherT = TestPublisherProtectedMethods>; + auto publisher = + node->create_publisher, PublisherT>("topic", 10); + + auto msg = std::make_shared(); + { + // Using 'self' instead of 'lib:rclcpp' because `rcl_publish_loaned_message` is entirely + // defined in a header + auto mock = mocking_utils::patch_and_return( + "self", rcl_publish_loaned_message, RCL_RET_PUBLISHER_INVALID); + EXPECT_THROW(publisher->publish_loaned_message(msg.get()), rclcpp::exceptions::RCLError); + } +} + +TEST_F(TestPublisher, run_event_handlers) { + initialize(); + auto publisher = node->create_publisher("topic", 10); + for (const auto & handler : publisher->get_event_handlers()) { + EXPECT_NO_THROW(handler->execute()); + } +} diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 5833e02b21..d6608d59f6 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -29,6 +29,7 @@ TEST(TestRate, rate_basics) { auto start = std::chrono::system_clock::now(); rclcpp::Rate r(period); + EXPECT_EQ(period, r.period()); ASSERT_FALSE(r.is_steady()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::system_clock::now(); @@ -68,6 +69,7 @@ TEST(TestRate, wall_rate_basics) { auto start = std::chrono::steady_clock::now(); rclcpp::WallRate r(period); + EXPECT_EQ(period, r.period()); ASSERT_TRUE(r.is_steady()); ASSERT_TRUE(r.sleep()); auto one = std::chrono::steady_clock::now(); @@ -98,3 +100,22 @@ TEST(TestRate, wall_rate_basics) { delta = five - four; EXPECT_GT(epsilon, delta); } + +TEST(TestRate, from_double) { + { + rclcpp::WallRate rate(1.0); + EXPECT_EQ(std::chrono::seconds(1), rate.period()); + } + { + rclcpp::WallRate rate(2.0); + EXPECT_EQ(std::chrono::milliseconds(500), rate.period()); + } + { + rclcpp::WallRate rate(0.5); + EXPECT_EQ(std::chrono::seconds(2), rate.period()); + } + { + rclcpp::WallRate rate(4.0); + EXPECT_EQ(std::chrono::milliseconds(250), rate.period()); + } +} diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index a265f05e17..ef626849a0 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -23,6 +23,9 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + #include "test_msgs/msg/empty.hpp" using namespace std::chrono_literals; @@ -150,7 +153,19 @@ TEST_F(TestSubscription, construction_and_destruction) { (void)msg; }; { - auto sub = node->create_subscription("topic", 10, callback); + constexpr size_t depth = 10u; + auto sub = node->create_subscription("topic", depth, callback); + + EXPECT_NE(nullptr, sub->get_subscription_handle()); + // Converting to base class was necessary for the compiler to choose the const version of + // get_subscription_handle() + const rclcpp::SubscriptionBase * const_sub = sub.get(); + EXPECT_NE(nullptr, const_sub->get_subscription_handle()); + EXPECT_FALSE(sub->use_take_shared_method()); + + EXPECT_NE(nullptr, sub->get_message_type_support_handle().typesupport_identifier); + EXPECT_NE(nullptr, sub->get_message_type_support_handle().data); + EXPECT_EQ(depth, sub->get_actual_qos().get_rmw_qos_profile().depth); } { @@ -229,6 +244,21 @@ TEST_F(TestSubscription, various_creation_signatures) { node, "topic", 42, cb, rclcpp::SubscriptionOptions()); (void)sub; } + { + rclcpp::SubscriptionOptionsWithAllocator> options; + options.allocator = std::make_shared>(); + EXPECT_NE(nullptr, options.get_allocator()); + auto sub = rclcpp::create_subscription( + node, "topic", 42, cb, options); + (void)sub; + } + { + rclcpp::SubscriptionOptionsBase options_base; + rclcpp::SubscriptionOptionsWithAllocator> options(options_base); + auto sub = rclcpp::create_subscription( + node, "topic", 42, cb, options); + (void)sub; + } } /* @@ -329,6 +359,86 @@ TEST_F(TestSubscription, take_serialized) { } } +TEST_F(TestSubscription, rcl_subscription_init_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_init, RCL_RET_TOPIC_NAME_INVALID); + + // reset() is not needed for triggering exception, just to avoid an unused return value warning + EXPECT_THROW( + node->create_subscription("topic", 10, callback).reset(), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_subscription_fini_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_subscription_fini, RCL_RET_ERROR); + + // Cleanup just fails, no exception expected + EXPECT_NO_THROW( + node->create_subscription("topic", 10, callback).reset()); +} + +TEST_F(TestSubscription, rcl_subscription_get_actual_qos_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_actual_qos, nullptr); + + auto sub = node->create_subscription("topic", 10, callback); + RCLCPP_EXPECT_THROW_EQ( + sub->get_actual_qos(), std::runtime_error("failed to get qos settings: error not set")); +} + +TEST_F(TestSubscription, rcl_take_type_erased_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take, RCL_RET_ERROR); + + auto sub = node->create_subscription("topic", 10, callback); + test_msgs::msg::Empty msg; + rclcpp::MessageInfo message_info; + + EXPECT_THROW(sub->take_type_erased(&msg, message_info), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_take_serialized_message_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_take_serialized_message, RCL_RET_ERROR); + + auto sub = node->create_subscription("topic", 10, callback); + rclcpp::SerializedMessage msg; + rclcpp::MessageInfo message_info; + + EXPECT_THROW(sub->take_serialized(msg, message_info), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, rcl_subscription_get_publisher_count_error) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_publisher_count, RCL_RET_ERROR); + + auto sub = node->create_subscription("topic", 10, callback); + EXPECT_THROW(sub->get_publisher_count(), rclcpp::exceptions::RCLError); +} + +TEST_F(TestSubscription, handle_loaned_message) { + initialize(); + auto callback = [](std::shared_ptr) {}; + auto sub = node->create_subscription("topic", 10, callback); + + test_msgs::msg::Empty msg; + rclcpp::MessageInfo message_info; + EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info)); +} + /* Testing subscription with intraprocess enabled and invalid QoS */ diff --git a/rclcpp/test/rclcpp/test_timer.cpp b/rclcpp/test/rclcpp/test_timer.cpp index 7f85a1c4c5..cc568b06c5 100644 --- a/rclcpp/test/rclcpp/test_timer.cpp +++ b/rclcpp/test/rclcpp/test_timer.cpp @@ -54,6 +54,7 @@ class TestTimer : public ::testing::Test this->executor->cancel(); } ); + EXPECT_TRUE(timer->is_steady()); executor->add_node(test_node); // don't start spinning, let the test dictate when @@ -191,3 +192,19 @@ TEST_F(TestTimer, test_bad_arguments) { rclcpp::GenericTimer(unitialized_clock, 1us, []() {}, context), rclcpp::exceptions::RCLError); } + +TEST_F(TestTimer, callback_with_timer) { + rclcpp::TimerBase * timer_ptr = nullptr; + timer = test_node->create_wall_timer( + std::chrono::milliseconds(1), + [&timer_ptr](rclcpp::TimerBase & timer) { + timer_ptr = &timer; + }); + auto start = std::chrono::steady_clock::now(); + while (nullptr == timer_ptr && + (std::chrono::steady_clock::now() - start) < std::chrono::milliseconds(100)) + { + executor->spin_once(std::chrono::milliseconds(10)); + } + EXPECT_EQ(timer.get(), timer_ptr); +} diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index d0e05f9bbb..905ff1a4f0 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -17,9 +17,15 @@ #include #include +#include "rcl/init.h" +#include "rcl/logging.h" + #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/utilities.hpp" +#include "rclcpp/scope_exit.hpp" + +#include "../mocking_utils/patch.hpp" TEST(TestUtilities, remove_ros_arguments) { const char * const argv[] = { @@ -78,3 +84,128 @@ TEST(TestUtilities, multi_init) { EXPECT_FALSE(rclcpp::ok(context1)); EXPECT_FALSE(rclcpp::ok(context2)); } + +TEST(TestUtilities, test_context_basic_access) { + auto context1 = std::make_shared(); + EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); + EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + EXPECT_EQ(std::string{""}, context1->shutdown_reason()); +} + +TEST(TestUtilities, test_context_basic_access_const_methods) { + auto context1 = std::make_shared(); + + EXPECT_NE(nullptr, context1->get_init_options().get_rcl_init_options()); + EXPECT_EQ(0u, context1->get_on_shutdown_callbacks().size()); + // EXPECT_EQ(std::string{""}, context1->shutdown_reason()); not available for const +} + +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcl_guard_condition_options_t, <) + +TEST(TestUtilities, test_context_release_interrupt_guard_condition) { + auto context1 = std::make_shared(); + context1->init(0, nullptr); + RCLCPP_SCOPE_EXIT(rclcpp::shutdown(context1);); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, 0, 2, 0, 0, 0, 0, context1->get_rcl_context().get(), + rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret); + + // Expected usage + rcl_guard_condition_t * interrupt_guard_condition = + context1->get_interrupt_guard_condition(&wait_set); + EXPECT_NE(nullptr, interrupt_guard_condition); + EXPECT_NO_THROW(context1->release_interrupt_guard_condition(&wait_set)); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_guard_condition_init, RCL_RET_ERROR); + EXPECT_THROW( + {interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set);}, + rclcpp::exceptions::RCLError); + } + + { + interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + EXPECT_THROW( + {context1->release_interrupt_guard_condition(&wait_set);}, + rclcpp::exceptions::RCLError); + } + + { + interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_guard_condition_fini, RCL_RET_ERROR); + EXPECT_NO_THROW({context1->release_interrupt_guard_condition(&wait_set, std::nothrow);}); + } + + { + EXPECT_THROW( + context1->release_interrupt_guard_condition(nullptr), + std::runtime_error); + } + + // Test it works after restore mocks + interrupt_guard_condition = context1->get_interrupt_guard_condition(&wait_set); + EXPECT_NE(nullptr, interrupt_guard_condition); + EXPECT_NO_THROW(context1->release_interrupt_guard_condition(&wait_set)); +} + + +TEST(TestUtilities, test_context_init_shutdown_fails) { + { + auto context = std::make_shared(); + auto context_fail_init = std::make_shared(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_init, RCL_RET_ERROR); + EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError); + } + + { + auto context_fail_init = std::make_shared(); + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_logging_configure_with_output_handler, RCL_RET_ERROR); + EXPECT_THROW(context_fail_init->init(0, nullptr), rclcpp::exceptions::RCLError); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW(context->shutdown("")); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_shutdown, RCL_RET_ERROR); + EXPECT_THROW(context->shutdown(""), rclcpp::exceptions::RCLError); + } + + { + auto context = std::make_shared(); + context->init(0, nullptr); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_logging_fini, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW(context->shutdown("")); + } + + { + auto context_to_destroy = std::make_shared(); + auto mock = mocking_utils::inject_on_return( + "lib:rclcpp", rcl_trigger_guard_condition, RCL_RET_ERROR); + // This will log a message, no throw expected + EXPECT_NO_THROW({context_to_destroy.reset();}); + } +}