From b786cea3489c829987238a29afdd80ae70bf27ed Mon Sep 17 00:00:00 2001 From: brawner Date: Thu, 28 May 2020 16:02:35 -0700 Subject: [PATCH] Increase test coverage of rcl_action (#663) * Add action_server validity check, and remove dead code Signed-off-by: Stephen Brawner * Increase test coverage of rcl_action Signed-off-by: Stephen Brawner * Reorganizing impl structs into private header Signed-off-by: Stephen Brawner * Addressing PR Feedback Signed-off-by: Stephen Brawner * Addressing memory error and PR feedback Signed-off-by: Stephen Brawner Co-authored-by: Stephen Brawner --- rcl_action/CMakeLists.txt | 26 +- rcl_action/src/rcl_action/action_client.c | 18 +- .../src/rcl_action/action_client_impl.h | 39 + rcl_action/src/rcl_action/action_server.c | 26 +- .../src/rcl_action/action_server_impl.h | 45 + .../src/rcl_action/goal_state_machine.c | 31 +- .../test/rcl_action/test_action_client.cpp | 109 +++ .../test/rcl_action/test_action_server.cpp | 288 ++++++- .../test/rcl_action/test_goal_handle.cpp | 37 + rcl_action/test/rcl_action/test_graph.cpp | 14 + rcl_action/test/rcl_action/test_names.cpp | 21 + rcl_action/test/rcl_action/test_types.cpp | 97 ++- rcl_action/test/rcl_action/test_wait.cpp | 813 ++++++++++++++++++ 13 files changed, 1496 insertions(+), 68 deletions(-) create mode 100644 rcl_action/src/rcl_action/action_client_impl.h create mode 100644 rcl_action/src/rcl_action/action_server_impl.h create mode 100644 rcl_action/test/rcl_action/test_wait.cpp diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt index 99f5537..783ee73 100644 --- a/rcl_action/CMakeLists.txt +++ b/rcl_action/CMakeLists.txt @@ -85,6 +85,7 @@ if(BUILD_TESTING) if(TARGET test_action_client) target_include_directories(test_action_client PUBLIC include + src ) target_link_libraries(test_action_client ${PROJECT_NAME} @@ -146,6 +147,7 @@ if(BUILD_TESTING) if(TARGET test_action_server) target_include_directories(test_action_server PUBLIC include + src ) target_link_libraries(test_action_server ${PROJECT_NAME} @@ -166,7 +168,7 @@ if(BUILD_TESTING) target_link_libraries(test_goal_handle ${PROJECT_NAME} ) - ament_target_dependencies(test_action_server "rcl") + ament_target_dependencies(test_goal_handle "rcl") endif() ament_add_gtest(test_goal_state_machine test/rcl_action/test_goal_state_machine.cpp @@ -175,7 +177,7 @@ if(BUILD_TESTING) target_link_libraries(test_goal_state_machine ${PROJECT_NAME} ) - ament_target_dependencies(test_action_server "rcl") + ament_target_dependencies(test_goal_state_machine "osrf_testing_tools_cpp" "rcl") endif() ament_add_gtest(test_types test/rcl_action/test_types.cpp @@ -187,7 +189,7 @@ if(BUILD_TESTING) target_link_libraries(test_types ${PROJECT_NAME} ) - ament_target_dependencies(test_action_server "rcl") + ament_target_dependencies(test_types "rcl") endif() ament_add_gtest(test_names test/rcl_action/test_names.cpp @@ -196,7 +198,23 @@ if(BUILD_TESTING) target_link_libraries(test_names ${PROJECT_NAME} ) - ament_target_dependencies(test_action_server "rcl") + ament_target_dependencies(test_names "rcl") + endif() + ament_add_gtest(test_wait + test/rcl_action/test_wait.cpp + ) + if(TARGET test_wait) + target_link_libraries(test_wait + ${PROJECT_NAME} + ) + target_include_directories(test_wait PUBLIC + src + ) + ament_target_dependencies(test_wait + "osrf_testing_tools_cpp" + "rcl" + "test_msgs" + ) endif() endif() diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 03b7b85..7551eab 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -18,6 +18,7 @@ extern "C" #endif #include "rcl_action/action_client.h" +#include "./action_client_impl.h" #include "rcl_action/default_qos.h" #include "rcl_action/names.h" @@ -38,23 +39,6 @@ extern "C" #include "rmw/types.h" -typedef struct rcl_action_client_impl_t -{ - rcl_client_t goal_client; - rcl_client_t cancel_client; - rcl_client_t result_client; - rcl_subscription_t feedback_subscription; - rcl_subscription_t status_subscription; - rcl_action_client_options_t options; - char * action_name; - // Wait set records - size_t wait_set_goal_client_index; - size_t wait_set_cancel_client_index; - size_t wait_set_result_client_index; - size_t wait_set_feedback_subscription_index; - size_t wait_set_status_subscription_index; -} rcl_action_client_impl_t; - rcl_action_client_t rcl_action_get_zero_initialized_client(void) { diff --git a/rcl_action/src/rcl_action/action_client_impl.h b/rcl_action/src/rcl_action/action_client_impl.h new file mode 100644 index 0000000..00cdd61 --- /dev/null +++ b/rcl_action/src/rcl_action/action_client_impl.h @@ -0,0 +1,39 @@ +// 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. + +#ifndef RCL_ACTION__ACTION_CLIENT_IMPL_H_ +#define RCL_ACTION__ACTION_CLIENT_IMPL_H_ + +#include "rcl_action/types.h" +#include "rcl/rcl.h" + +typedef struct rcl_action_client_impl_t +{ + rcl_client_t goal_client; + rcl_client_t cancel_client; + rcl_client_t result_client; + rcl_subscription_t feedback_subscription; + rcl_subscription_t status_subscription; + rcl_action_client_options_t options; + char * action_name; + // Wait set records + size_t wait_set_goal_client_index; + size_t wait_set_cancel_client_index; + size_t wait_set_result_client_index; + size_t wait_set_feedback_subscription_index; + size_t wait_set_status_subscription_index; +} rcl_action_client_impl_t; + + +#endif // RCL_ACTION__ACTION_CLIENT_IMPL_H_ diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 9d17d17..626101e 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -18,6 +18,7 @@ extern "C" #endif #include "rcl_action/action_server.h" +#include "./action_server_impl.h" #include "rcl_action/default_qos.h" #include "rcl_action/goal_handle.h" @@ -34,28 +35,6 @@ extern "C" #include "rmw/rmw.h" -/// Internal rcl_action implementation struct. -typedef struct rcl_action_server_impl_t -{ - rcl_service_t goal_service; - rcl_service_t cancel_service; - rcl_service_t result_service; - rcl_publisher_t feedback_publisher; - rcl_publisher_t status_publisher; - rcl_timer_t expire_timer; - char * action_name; - rcl_action_server_options_t options; - // Array of goal handles - rcl_action_goal_handle_t ** goal_handles; - size_t num_goal_handles; - // Clock - rcl_clock_t clock; - // Wait set records - size_t wait_set_goal_service_index; - size_t wait_set_cancel_service_index; - size_t wait_set_result_service_index; - size_t wait_set_expire_timer_index; -} rcl_action_server_impl_t; rcl_action_server_t rcl_action_get_zero_initialized_server(void) @@ -693,6 +672,9 @@ rcl_ret_t rcl_action_notify_goal_done( const rcl_action_server_t * action_server) { + if (!rcl_action_server_is_valid(action_server)) { + return RCL_RET_ACTION_SERVER_INVALID; + } return _recalculate_expire_timer( &action_server->impl->expire_timer, action_server->impl->options.result_timeout.nanoseconds, diff --git a/rcl_action/src/rcl_action/action_server_impl.h b/rcl_action/src/rcl_action/action_server_impl.h new file mode 100644 index 0000000..af718a1 --- /dev/null +++ b/rcl_action/src/rcl_action/action_server_impl.h @@ -0,0 +1,45 @@ +// 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. + +#ifndef RCL_ACTION__ACTION_SERVER_IMPL_H_ +#define RCL_ACTION__ACTION_SERVER_IMPL_H_ + +#include "rcl_action/types.h" +#include "rcl/rcl.h" + +/// Internal rcl_action implementation struct. +typedef struct rcl_action_server_impl_t +{ + rcl_service_t goal_service; + rcl_service_t cancel_service; + rcl_service_t result_service; + rcl_publisher_t feedback_publisher; + rcl_publisher_t status_publisher; + rcl_timer_t expire_timer; + char * action_name; + rcl_action_server_options_t options; + // Array of goal handles + rcl_action_goal_handle_t ** goal_handles; + size_t num_goal_handles; + // Clock + rcl_clock_t clock; + // Wait set records + size_t wait_set_goal_service_index; + size_t wait_set_cancel_service_index; + size_t wait_set_result_service_index; + size_t wait_set_expire_timer_index; +} rcl_action_server_impl_t; + + +#endif // RCL_ACTION__ACTION_SERVER_IMPL_H_ diff --git a/rcl_action/src/rcl_action/goal_state_machine.c b/rcl_action/src/rcl_action/goal_state_machine.c index 9bbc3c6..a82b259 100644 --- a/rcl_action/src/rcl_action/goal_state_machine.c +++ b/rcl_action/src/rcl_action/goal_state_machine.c @@ -26,51 +26,40 @@ typedef rcl_action_goal_state_t rcl_action_goal_state_t _execute_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { - if (GOAL_STATE_ACCEPTED != state || GOAL_EVENT_EXECUTE != event) { - return GOAL_STATE_UNKNOWN; - } + assert(GOAL_STATE_ACCEPTED == state); + assert(GOAL_EVENT_EXECUTE == event); return GOAL_STATE_EXECUTING; } rcl_action_goal_state_t _cancel_goal_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { - if ((GOAL_STATE_ACCEPTED != state && GOAL_STATE_EXECUTING != state) || - GOAL_EVENT_CANCEL_GOAL != event) - { - return GOAL_STATE_UNKNOWN; - } + assert(GOAL_STATE_ACCEPTED == state || GOAL_STATE_EXECUTING == state); + assert(GOAL_EVENT_CANCEL_GOAL == event); return GOAL_STATE_CANCELING; } rcl_action_goal_state_t _succeed_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { - if ((GOAL_STATE_EXECUTING != state && GOAL_STATE_CANCELING != state) || - GOAL_EVENT_SUCCEED != event) - { - return GOAL_STATE_UNKNOWN; - } + assert(GOAL_STATE_EXECUTING == state || GOAL_STATE_CANCELING == state); + assert(GOAL_EVENT_SUCCEED == event); return GOAL_STATE_SUCCEEDED; } rcl_action_goal_state_t _abort_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { - if ((GOAL_STATE_EXECUTING != state && GOAL_STATE_CANCELING != state) || - GOAL_EVENT_ABORT != event) - { - return GOAL_STATE_UNKNOWN; - } + assert(GOAL_STATE_EXECUTING == state || GOAL_STATE_CANCELING == state); + assert(GOAL_EVENT_ABORT == event); return GOAL_STATE_ABORTED; } rcl_action_goal_state_t _canceled_event_handler(rcl_action_goal_state_t state, rcl_action_goal_event_t event) { - if (GOAL_STATE_CANCELING != state || GOAL_EVENT_CANCELED != event) { - return GOAL_STATE_UNKNOWN; - } + assert(GOAL_STATE_CANCELING == state); + assert(GOAL_EVENT_CANCELED == event); return GOAL_STATE_CANCELED; } diff --git a/rcl_action/test/rcl_action/test_action_client.cpp b/rcl_action/test/rcl_action/test_action_client.cpp index 72a7fda..66e91eb 100644 --- a/rcl_action/test/rcl_action/test_action_client.cpp +++ b/rcl_action/test/rcl_action/test_action_client.cpp @@ -15,6 +15,7 @@ #include #include "rcl_action/action_client.h" +#include "rcl_action/action_client_impl.h" #include "rcl/error_handling.h" #include "rcl/rcl.h" @@ -22,6 +23,24 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "test_msgs/action/fibonacci.h" +struct __time_bomb_allocator_state +{ + int malloc_count_until_failure; +}; + +void * time_bomb_malloc(size_t size, void * state) +{ + __time_bomb_allocator_state * time_bomb_state = + reinterpret_cast<__time_bomb_allocator_state *>(state); + if (time_bomb_state->malloc_count_until_failure >= 0 && + time_bomb_state->malloc_count_until_failure-- == 0) + { + printf("Malloc time bomb countdown reached 0, returning nullptr\n"); + return nullptr; + } + return rcutils_get_default_allocator().allocate(size, rcutils_get_default_allocator().state); +} + class TestActionClientBaseFixture : public ::testing::Test { protected: @@ -122,6 +141,33 @@ TEST_F(TestActionClientBaseFixture, test_action_client_init_fini) { EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); + // Fail allocating for implement struct + invalid_action_client_options.allocator = + rcl_get_default_allocator(); + __time_bomb_allocator_state time_bomb_state = {0}; + invalid_action_client_options.allocator.state = + reinterpret_cast(&time_bomb_state); + invalid_action_client_options.allocator.allocate = time_bomb_malloc; + ret = rcl_action_client_init( + &action_client, &this->node, action_typesupport, + action_name, &invalid_action_client_options); + EXPECT_EQ(ret, RCL_RET_BAD_ALLOC) << rcl_get_error_string().str; + rcl_reset_error(); + + // Fail copying action_name string, this will also fail to cleanup and will return generic error + time_bomb_state.malloc_count_until_failure = 1; + invalid_action_client_options.allocator.state = &time_bomb_state; + ret = rcl_action_client_init( + &action_client, &this->node, action_typesupport, + action_name, &invalid_action_client_options); + EXPECT_EQ(ret, RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); + + // Manually cleanup to setup for next test + action_client_options.allocator.deallocate( + action_client.impl, action_client_options.allocator.state); + action_client.impl = NULL; + ret = rcl_action_client_init( &action_client, &this->node, action_typesupport, action_name, &action_client_options); @@ -185,6 +231,28 @@ protected: rcl_action_client_t action_client; }; +TEST_F(TestActionClientFixture, test_action_server_is_available) { + bool is_available = false; + rcl_ret_t ret = rcl_action_server_is_available(nullptr, &this->action_client, &is_available); + EXPECT_EQ(ret, RCL_RET_NODE_INVALID); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_action_server_is_available(&this->node, nullptr, &is_available); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_action_server_is_available(&this->node, &this->action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_action_server_is_available(&this->node, &this->action_client, &is_available); + EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_FALSE(is_available); +} + TEST_F(TestActionClientFixture, test_action_client_is_valid) { bool is_valid = rcl_action_client_is_valid(nullptr); EXPECT_FALSE(is_valid) << rcl_get_error_string().str; @@ -194,6 +262,47 @@ TEST_F(TestActionClientFixture, test_action_client_is_valid) { EXPECT_FALSE(is_valid) << rcl_get_error_string().str; rcl_reset_error(); + rcl_client_impl_t * tmp_client = this->action_client.impl->goal_client.impl; + this->action_client.impl->goal_client.impl = nullptr; + is_valid = rcl_action_client_is_valid(&this->action_client); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_client.impl->goal_client.impl = tmp_client; + + tmp_client = this->action_client.impl->cancel_client.impl; + this->action_client.impl->cancel_client.impl = nullptr; + is_valid = rcl_action_client_is_valid(&this->action_client); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_client.impl->cancel_client.impl = tmp_client; + + tmp_client = this->action_client.impl->result_client.impl; + this->action_client.impl->result_client.impl = nullptr; + is_valid = rcl_action_client_is_valid(&this->action_client); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_client.impl->result_client.impl = tmp_client; + + rcl_subscription_impl_t * tmp_subscription = + this->action_client.impl->feedback_subscription.impl; + this->action_client.impl->feedback_subscription.impl = nullptr; + is_valid = rcl_action_client_is_valid(&this->action_client); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_client.impl->feedback_subscription.impl = tmp_subscription; + + tmp_subscription = this->action_client.impl->status_subscription.impl; + this->action_client.impl->status_subscription.impl = nullptr; + is_valid = rcl_action_client_is_valid(&this->action_client); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_client.impl->status_subscription.impl = tmp_subscription; + is_valid = rcl_action_client_is_valid(&this->action_client); EXPECT_TRUE(is_valid) << rcl_get_error_string().str; rcl_reset_error(); diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp index 1b31caf..b116e3a 100644 --- a/rcl_action/test/rcl_action/test_action_server.cpp +++ b/rcl_action/test/rcl_action/test_action_server.cpp @@ -22,12 +22,29 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl_action/action_server.h" +#include "rcl_action/action_server_impl.h" #include "rcl/error_handling.h" #include "rcl/rcl.h" #include "test_msgs/action/fibonacci.h" +void * bad_malloc(size_t, void *) +{ + return nullptr; +} + +void * bad_realloc(void *, size_t, void *) +{ + return nullptr; +} + +void * bad_calloc(size_t, size_t, void *) +{ + printf("Returning null ptr\n"); + return nullptr; +} + TEST(TestActionServerInitFini, test_action_server_init_fini) { rcl_allocator_t allocator = rcl_get_default_allocator(); @@ -54,6 +71,7 @@ TEST(TestActionServerInitFini, test_action_server_init_fini) // Initialize with a null action server ret = rcl_action_server_init(nullptr, &node, &clock, ts, action_name, &options); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); // Initialize with a null node @@ -106,6 +124,13 @@ TEST(TestActionServerInitFini, test_action_server_init_fini) EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); + // Initialize with an invalid timeout + rcl_action_server_options_t bad_options = rcl_action_server_get_default_options(); + bad_options.result_timeout.nanoseconds = -1; + ret = rcl_action_server_init(&action_server, &node, &clock, ts, action_name, &bad_options); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + // Initialize with valid arguments ret = rcl_action_server_init(&action_server, &node, &clock, ts, action_name, &options); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -240,6 +265,104 @@ TEST_F(TestActionServer, test_action_server_is_valid) // Check valid action server is_valid = rcl_action_server_is_valid(&this->action_server); EXPECT_TRUE(is_valid) << rcl_get_error_string().str; + + rcl_service_impl_t * tmp_service = this->action_server.impl->goal_service.impl; + this->action_server.impl->goal_service.impl = nullptr; + is_valid = rcl_action_server_is_valid(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->goal_service.impl = tmp_service; + + tmp_service = this->action_server.impl->cancel_service.impl; + this->action_server.impl->cancel_service.impl = nullptr; + is_valid = rcl_action_server_is_valid(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->cancel_service.impl = tmp_service; + + tmp_service = this->action_server.impl->result_service.impl; + this->action_server.impl->result_service.impl = nullptr; + is_valid = rcl_action_server_is_valid(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->result_service.impl = tmp_service; + + rcl_publisher_impl_t * tmp_publisher = this->action_server.impl->feedback_publisher.impl; + this->action_server.impl->feedback_publisher.impl = nullptr; + is_valid = rcl_action_server_is_valid(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->feedback_publisher.impl = tmp_publisher; + + tmp_publisher = this->action_server.impl->status_publisher.impl; + this->action_server.impl->status_publisher.impl = nullptr; + is_valid = rcl_action_server_is_valid(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->status_publisher.impl = tmp_publisher; +} + +TEST_F(TestActionServer, test_action_server_is_valid_except_context) +{ + // Check with null pointer + bool is_valid = rcl_action_server_is_valid_except_context(nullptr); + EXPECT_FALSE(is_valid) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check with uninitialized action server + rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); + is_valid = rcl_action_server_is_valid_except_context(&invalid_action_server); + EXPECT_FALSE(is_valid) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check valid action server + is_valid = rcl_action_server_is_valid_except_context(&this->action_server); + EXPECT_TRUE(is_valid) << rcl_get_error_string().str; + + rcl_service_impl_t * tmp_service = this->action_server.impl->goal_service.impl; + this->action_server.impl->goal_service.impl = nullptr; + is_valid = rcl_action_server_is_valid_except_context(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->goal_service.impl = tmp_service; + + tmp_service = this->action_server.impl->cancel_service.impl; + this->action_server.impl->cancel_service.impl = nullptr; + is_valid = rcl_action_server_is_valid_except_context(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->cancel_service.impl = tmp_service; + + tmp_service = this->action_server.impl->result_service.impl; + this->action_server.impl->result_service.impl = nullptr; + is_valid = rcl_action_server_is_valid_except_context(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->result_service.impl = tmp_service; + + rcl_publisher_impl_t * tmp_publisher = this->action_server.impl->feedback_publisher.impl; + this->action_server.impl->feedback_publisher.impl = nullptr; + is_valid = rcl_action_server_is_valid_except_context(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->feedback_publisher.impl = tmp_publisher; + + tmp_publisher = this->action_server.impl->status_publisher.impl; + this->action_server.impl->status_publisher.impl = nullptr; + is_valid = rcl_action_server_is_valid_except_context(&this->action_server); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + this->action_server.impl->status_publisher.impl = tmp_publisher; } TEST_F(TestActionServer, test_action_accept_new_goal) @@ -251,17 +374,38 @@ TEST_F(TestActionServer, test_action_accept_new_goal) // Accept goal with a null action server rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal(nullptr, &goal_info_in); EXPECT_EQ(goal_handle, nullptr); + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); // Accept goal with null goal info goal_handle = rcl_action_accept_new_goal(&this->action_server, nullptr); EXPECT_EQ(goal_handle, nullptr); + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); // Accept goal with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); goal_handle = rcl_action_accept_new_goal(&invalid_action_server, &goal_info_in); EXPECT_EQ(goal_handle, nullptr); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Check failing allocation of goal_handle + this->action_server.impl->options.allocator.allocate = bad_malloc; + goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in); + EXPECT_EQ(goal_handle, nullptr); + this->action_server.impl->options.allocator.allocate = + rcl_get_default_allocator().allocate; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Check failing reallocate of goal_handles + this->action_server.impl->options.allocator.reallocate = bad_realloc; + goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in); + EXPECT_EQ(goal_handle, nullptr); + this->action_server.impl->options.allocator.reallocate = + rcl_get_default_allocator().reallocate; + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); std::vector handles; @@ -276,6 +420,13 @@ TEST_F(TestActionServer, test_action_accept_new_goal) EXPECT_TRUE(uuidcmp(goal_info_out.goal_id.uuid, goal_info_in.goal_id.uuid)); size_t num_goals = 0u; rcl_action_goal_handle_t ** goal_handle_array = {nullptr}; + + // Check invalid action server + ret = rcl_action_server_get_goal_handles(nullptr, &goal_handle_array, &num_goals); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_action_server_get_goal_handles(&this->action_server, &goal_handle_array, &num_goals); ASSERT_EQ(ret, RCL_RET_OK); EXPECT_EQ(num_goals, 1u); @@ -285,6 +436,7 @@ TEST_F(TestActionServer, test_action_accept_new_goal) // Accept with the same goal ID goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in); EXPECT_EQ(goal_handle, nullptr); + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); // Accept a different goal @@ -305,14 +457,84 @@ TEST_F(TestActionServer, test_action_accept_new_goal) for (auto & handle : handles) { EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&handle)); + EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str; } } +TEST_F(TestActionServer, test_action_server_goal_exists) { + rcl_action_goal_info_t goal_info_out = rcl_action_get_zero_initialized_goal_info(); + EXPECT_FALSE(rcl_action_server_goal_exists(nullptr, &goal_info_out)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_FALSE(rcl_action_server_goal_exists(&this->action_server, nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Initialize a goal info + rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info(); + init_test_uuid0(goal_info_in.goal_id.uuid); + + // Add new goal + rcl_action_goal_handle_t * goal_handle = + rcl_action_accept_new_goal(&this->action_server, &goal_info_in); + EXPECT_NE(goal_handle, nullptr) << rcl_get_error_string().str; + + // Check exists + EXPECT_TRUE(rcl_action_server_goal_exists(&this->action_server, &goal_info_in)); + + rcl_action_goal_info_t different_goal = rcl_action_get_zero_initialized_goal_info(); + init_test_uuid1(goal_info_in.goal_id.uuid); + + // Check doesn't exist + EXPECT_FALSE(rcl_action_server_goal_exists(&this->action_server, &different_goal)); + EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str; + + // Check corrupted goal_handles + rcl_get_default_allocator().deallocate(goal_handle, rcl_get_default_allocator().state); + this->action_server.impl->goal_handles[this->action_server.impl->num_goal_handles - 1] = nullptr; + EXPECT_FALSE(rcl_action_server_goal_exists(&this->action_server, &different_goal)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Reset for teardown + this->action_server.impl->num_goal_handles--; +} + +TEST_F(TestActionServer, test_action_server_notify_goal_done) { + // Invalid action server + EXPECT_EQ(RCL_RET_ACTION_SERVER_INVALID, rcl_action_notify_goal_done(nullptr)); + rcl_reset_error(); + + // No goals yet, should be ok + EXPECT_EQ(RCL_RET_OK, rcl_action_notify_goal_done(&action_server)); + + rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info(); + init_test_uuid0(goal_info_in.goal_id.uuid); + + // Add new goal + rcl_action_goal_handle_t * goal_handle = + rcl_action_accept_new_goal(&this->action_server, &goal_info_in); + EXPECT_NE(goal_handle, nullptr) << rcl_get_error_string().str; + + // One goal, should be able to notify + EXPECT_EQ(RCL_RET_OK, rcl_action_notify_goal_done(&action_server)); + + // Invalid goal handle + rcl_get_default_allocator().deallocate(goal_handle, rcl_get_default_allocator().state); + this->action_server.impl->goal_handles[this->action_server.impl->num_goal_handles - 1] = nullptr; + EXPECT_EQ(RCL_RET_ERROR, rcl_action_notify_goal_done(&action_server)); + rcl_reset_error(); + + // Reset for teardown + this->action_server.impl->num_goal_handles--; +} + TEST_F(TestActionServer, test_action_clear_expired_goals) { const size_t capacity = 1u; rcl_action_goal_info_t expired_goals[1u]; - size_t num_expired = 1u; + size_t num_expired = 42u; // Clear expired goals with null action server rcl_ret_t ret = rcl_action_expire_goals(nullptr, expired_goals, capacity, &num_expired); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; @@ -324,6 +546,24 @@ TEST_F(TestActionServer, test_action_clear_expired_goals) EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); + // Clear with invalid arguments + ret = rcl_action_expire_goals(&this->action_server, nullptr, capacity, &num_expired); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + EXPECT_EQ(num_expired, 42u); + rcl_reset_error(); + + // Clear with invalid arguments + ret = rcl_action_expire_goals(&this->action_server, expired_goals, 0u, &num_expired); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + EXPECT_EQ(num_expired, 42u); + rcl_reset_error(); + + // Clear with invalid arguments + ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + EXPECT_EQ(num_expired, 42u); + rcl_reset_error(); + // Clear with valid arguments ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, &num_expired); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -339,6 +579,7 @@ TEST_F(TestActionServer, test_action_clear_expired_goals) // Set ROS time ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&this->clock)); ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(1))); + // Accept a goal to create a new handle rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info(); init_test_uuid1(goal_info_in.goal_id.uuid); @@ -346,11 +587,14 @@ TEST_F(TestActionServer, test_action_clear_expired_goals) rcl_action_accept_new_goal(&this->action_server, &goal_info_in); ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str; handles.push_back(*goal_handle); + // Transition executing to aborted ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_EXECUTE)); ASSERT_EQ(RCL_RET_OK, rcl_action_update_goal_state(goal_handle, GOAL_EVENT_ABORT)); + // Set time to something far in the future ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, RCUTILS_S_TO_NS(99999))); + // Clear with valid arguments ret = rcl_action_expire_goals(&this->action_server, expired_goals, capacity, &num_expired); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -382,6 +626,13 @@ TEST_F(TestActionServer, test_action_process_cancel_request) EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); rcl_reset_error(); + this->action_server.impl->options.allocator.allocate = bad_malloc; + // Process cancel request with bad allocator + ret = rcl_action_process_cancel_request(&this->action_server, &cancel_request, &cancel_response); + EXPECT_EQ(ret, RCL_RET_BAD_ALLOC); + rcl_reset_error(); + this->action_server.impl->options.allocator = rcl_get_default_allocator(); + // Process cancel request with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); ret = rcl_action_process_cancel_request( @@ -397,6 +648,24 @@ TEST_F(TestActionServer, test_action_process_cancel_request) EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); // A zero request means "cancel all goals", which succeeds if there's nothing to cancel EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE); + + // Number of goals is not 0, but goal handle is null, for case with request_nanosec == 0 + size_t num_goal_handles = 1u; + rcl_allocator_t allocator = this->action_server.impl->options.allocator; + this->action_server.impl->num_goal_handles = num_goal_handles; + this->action_server.impl->goal_handles = reinterpret_cast( + allocator.zero_allocate(num_goal_handles, sizeof(rcl_action_goal_handle_t *), allocator.state)); + ret = rcl_action_process_cancel_request( + &this->action_server, &cancel_request, &cancel_response); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcl_reset_error(); + + // Number of goals is not 0, but goal handle is null, for case with request_nanosec > 0 + cancel_request.goal_info.stamp.nanosec = 1; + ret = rcl_action_process_cancel_request( + &this->action_server, &cancel_request, &cancel_response); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcl_reset_error(); } TEST_F(TestActionServer, test_action_server_get_goal_status_array) @@ -419,6 +688,23 @@ TEST_F(TestActionServer, test_action_server_get_goal_status_array) EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); rcl_reset_error(); + // Check goal_status_array_init fails + this->action_server.impl->num_goal_handles = 1u; + this->action_server.impl->options.allocator.zero_allocate = bad_calloc; + ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); + EXPECT_EQ(ret, RCL_RET_BAD_ALLOC); + rcl_reset_error(); + this->action_server.impl->options.allocator = rcl_get_default_allocator(); + + // Check status_message is already inited + this->action_server.impl->num_goal_handles = 1u; + status_array.msg.status_list.size = 1; + ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcl_reset_error(); + status_array.msg.status_list.size = 0; + this->action_server.impl->num_goal_handles = 0u; + // Get with valid arguments (but not goals being tracked) ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; diff --git a/rcl_action/test/rcl_action/test_goal_handle.cpp b/rcl_action/test/rcl_action/test_goal_handle.cpp index d28ee4f..e7659aa 100644 --- a/rcl_action/test/rcl_action/test_goal_handle.cpp +++ b/rcl_action/test/rcl_action/test_goal_handle.cpp @@ -23,6 +23,11 @@ #include "rcl/error_handling.h" +void * bad_malloc(size_t, void *) +{ + return nullptr; +} + TEST(TestGoalHandle, test_goal_handle_init_fini) { rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); @@ -45,6 +50,13 @@ TEST(TestGoalHandle, test_goal_handle_init_fini) EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); + // Initialize with a failing allocator + rcl_allocator_t bad_allocator = rcl_get_default_allocator(); + bad_allocator.allocate = bad_malloc; + ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, bad_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcl_reset_error(); + // Initialize with valid goal handle and allocator ret = rcl_action_goal_handle_init(&goal_handle, &goal_info, rcl_get_default_allocator()); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -154,6 +166,7 @@ TEST(TestGoalHandle, test_goal_handle_update_state_invalid) ret = rcl_action_update_goal_state(&goal_handle, GOAL_EVENT_NUM_EVENTS); EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_EVENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_EQ(RCL_RET_OK, rcl_action_goal_handle_fini(&goal_handle)); } @@ -216,6 +229,28 @@ TEST_P(TestGoalHandleStateTransitionSequence, test_goal_handle_state_transitions // Goal handle starts in state ACCEPTED expect_state_eq(GOAL_STATE_ACCEPTED); + // Test bad goal handles + rcl_action_goal_state_t status; + EXPECT_EQ( + RCL_RET_ACTION_GOAL_HANDLE_INVALID, + rcl_action_goal_handle_get_status(nullptr, &status)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_action_goal_handle_get_status(&this->goal_handle, nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_FALSE(rcl_action_goal_handle_is_active(nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_FALSE(rcl_action_goal_handle_is_cancelable(nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Walk through state transitions rcl_ret_t ret; for (const EventStateActiveCancelableTuple & event_state : this->test_sequence) { @@ -228,6 +263,8 @@ TEST_P(TestGoalHandleStateTransitionSequence, test_goal_handle_state_transitions ret = rcl_action_update_goal_state(&this->goal_handle, goal_event); if (GOAL_STATE_UNKNOWN == expected_goal_state) { EXPECT_EQ(ret, RCL_RET_ACTION_GOAL_EVENT_INVALID); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); continue; } EXPECT_EQ(ret, RCL_RET_OK); diff --git a/rcl_action/test/rcl_action/test_graph.cpp b/rcl_action/test/rcl_action/test_graph.cpp index 0f94beb..95fb64b 100644 --- a/rcl_action/test/rcl_action/test_graph.cpp +++ b/rcl_action/test/rcl_action/test_graph.cpp @@ -36,6 +36,11 @@ # define CLASSNAME(NAME, SUFFIX) NAME #endif +void * bad_malloc(size_t, void *) +{ + return NULL; +} + class CLASSNAME (TestActionGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test { public: @@ -144,6 +149,15 @@ TEST_F( &this->node, &this->allocator, this->test_graph_node_name, "", nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + + // Failing allocator + rcl_allocator_t bad_allocator = rcl_get_default_allocator(); + bad_allocator.allocate = bad_malloc; + ret = rcl_action_get_client_names_and_types_by_node( + &this->node, &bad_allocator, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Valid call ret = rcl_action_get_client_names_and_types_by_node( &this->node, &this->allocator, this->test_graph_node_name, "", &nat); diff --git a/rcl_action/test/rcl_action/test_names.cpp b/rcl_action/test/rcl_action/test_names.cpp index 2e42a23..9620a83 100644 --- a/rcl_action/test/rcl_action/test_names.cpp +++ b/rcl_action/test/rcl_action/test_names.cpp @@ -30,6 +30,11 @@ struct ActionDerivedNameTestSubject const char * subject_name; }; +void * bad_malloc(size_t, void *) +{ + return NULL; +} + std::ostream & operator<<(std::ostream & os, const ActionDerivedNameTestSubject & test_subject) { return os << test_subject.subject_name; @@ -57,6 +62,8 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter) null_action_name, default_allocator, &action_derived_name); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); action_derived_name = NULL; const char * const invalid_action_name = ""; @@ -64,6 +71,7 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter) invalid_action_name, default_allocator, &action_derived_name); EXPECT_EQ(RCL_RET_ACTION_NAME_INVALID, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); action_derived_name = NULL; @@ -73,6 +81,7 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter) test_subject.action_name, invalid_allocator, &action_derived_name); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); action_derived_name = NULL; @@ -81,6 +90,7 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter) test_subject.action_name, default_allocator, invalid_ptr_to_action_derived_name); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); char dummy_char = '\0'; @@ -89,6 +99,7 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter) test_subject.action_name, default_allocator, &action_derived_name); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); rcl_reset_error(); action_derived_name = NULL; @@ -98,6 +109,16 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter) ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_STREQ(test_subject.expected_action_derived_name, action_derived_name); default_allocator.deallocate(action_derived_name, default_allocator.state); + + rcl_allocator_t failing_allocator = rcl_get_default_allocator(); + failing_allocator.allocate = bad_malloc; + action_derived_name = NULL; + ret = test_subject.get_action_derived_name( + test_subject.action_name, failing_allocator, + &action_derived_name); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); } const ActionDerivedNameTestSubject action_service_and_topic_subjects[] = { diff --git a/rcl_action/test/rcl_action/test_types.cpp b/rcl_action/test/rcl_action/test_types.cpp index b31c110..c0aae72 100644 --- a/rcl_action/test/rcl_action/test_types.cpp +++ b/rcl_action/test/rcl_action/test_types.cpp @@ -13,8 +13,19 @@ // limitations under the License. #include +#include "rcl/error_handling.h" #include "rcl_action/types.h" +void * bad_realloc(void *, size_t, void *) +{ + return nullptr; +} + +void * bad_calloc(size_t, size_t, void *) +{ + return nullptr; +} + TEST(TestActionTypes, test_get_zero_inititalized_goal_info) { rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); @@ -79,6 +90,8 @@ TEST(TestActionTypes, test_init_fini_goal_status_array) num_status, rcl_get_default_allocator()); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); // Initialize with invalid allocator rcl_allocator_t invalid_allocator = rcl_get_default_allocator(); @@ -90,6 +103,22 @@ TEST(TestActionTypes, test_init_fini_goal_status_array) EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); EXPECT_EQ(status_array.msg.status_list.size, 0u); EXPECT_EQ(status_array.msg.status_list.data, nullptr); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Initialize with a failing allocator + rcl_allocator_t failing_allocator = rcl_get_default_allocator(); + failing_allocator.zero_allocate = bad_calloc; + status_array = rcl_action_get_zero_initialized_goal_status_array(); + ASSERT_EQ(status_array.msg.status_list.size, 0u); + ret = rcl_action_goal_status_array_init(&status_array, num_status, failing_allocator); + EXPECT_EQ(ret, RCL_RET_BAD_ALLOC); + EXPECT_EQ(status_array.msg.status_list.size, 0u); + EXPECT_EQ(status_array.msg.status_list.data, nullptr); + // Doesn't set error + EXPECT_FALSE(rcl_error_is_set()); + rcl_reset_error(); + // Initialize with zero size status_array = rcl_action_get_zero_initialized_goal_status_array(); ASSERT_EQ(status_array.msg.status_list.size, 0u); @@ -97,18 +126,33 @@ TEST(TestActionTypes, test_init_fini_goal_status_array) EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); EXPECT_EQ(status_array.msg.status_list.size, 0u); EXPECT_EQ(status_array.msg.status_list.data, nullptr); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Initialize with non-zero size + status_array = rcl_action_get_zero_initialized_goal_status_array(); + status_array.msg.status_list.size = 1; + ret = rcl_action_goal_status_array_init(&status_array, num_status, rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_ALREADY_INIT); + // Expect array to be unchanged + EXPECT_EQ(status_array.msg.status_list.size, 1u); + EXPECT_EQ(status_array.msg.status_list.data, nullptr); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); // Initialize with valid arguments status_array = rcl_action_get_zero_initialized_goal_status_array(); ASSERT_EQ(status_array.msg.status_list.size, 0u); ret = rcl_action_goal_status_array_init(&status_array, num_status, rcl_get_default_allocator()); - EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_EQ(status_array.msg.status_list.size, num_status); EXPECT_NE(status_array.msg.status_list.data, nullptr); // Finalize with invalid status array ret = rcl_action_goal_status_array_fini(nullptr); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); // Finalize with valid arguments ret = rcl_action_goal_status_array_fini(&status_array); @@ -124,6 +168,8 @@ TEST(TestActionTypes, test_init_fini_cancel_response) num_goals_canceling, rcl_get_default_allocator()); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); // Initialize with invalid allocator rcl_allocator_t invalid_allocator = rcl_get_default_allocator(); @@ -135,6 +181,22 @@ TEST(TestActionTypes, test_init_fini_cancel_response) EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); EXPECT_EQ(cancel_response.msg.return_code, 0); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Initialize with failing allocator + rcl_allocator_t failing_allocator = rcl_get_default_allocator(); + failing_allocator.zero_allocate = bad_calloc; + cancel_response = rcl_action_get_zero_initialized_cancel_response(); + ASSERT_EQ(cancel_response.msg.goals_canceling.size, 0u); + ret = rcl_action_cancel_response_init(&cancel_response, num_goals_canceling, failing_allocator); + EXPECT_EQ(ret, RCL_RET_BAD_ALLOC); + EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); + EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.return_code, 0); + // Bad allocation doesn't set error message + EXPECT_FALSE(rcl_error_is_set()); + rcl_reset_error(); // Initialize with zero size cancel_response = rcl_action_get_zero_initialized_cancel_response(); @@ -144,6 +206,33 @@ TEST(TestActionTypes, test_init_fini_cancel_response) EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); EXPECT_EQ(cancel_response.msg.return_code, 0); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Initialize with non-zero size + cancel_response = rcl_action_get_zero_initialized_cancel_response(); + cancel_response.msg.goals_canceling.size = 1; + ret = rcl_action_cancel_response_init(&cancel_response, 1, rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_ALREADY_INIT); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Check unchanged + EXPECT_EQ(cancel_response.msg.goals_canceling.size, 1u); + EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.return_code, 0); + + // Initialize with non-zero return code + cancel_response = rcl_action_get_zero_initialized_cancel_response(); + cancel_response.msg.return_code = 1; + ret = rcl_action_cancel_response_init(&cancel_response, 0, rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + // Check unchanged + EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); + EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.return_code, 1); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); // Initialize with valid arguments cancel_response = rcl_action_get_zero_initialized_cancel_response(); @@ -152,7 +241,7 @@ TEST(TestActionTypes, test_init_fini_cancel_response) &cancel_response, num_goals_canceling, rcl_get_default_allocator()); - EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_EQ(cancel_response.msg.goals_canceling.size, num_goals_canceling); EXPECT_NE(cancel_response.msg.goals_canceling.data, nullptr); EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE); @@ -160,8 +249,10 @@ TEST(TestActionTypes, test_init_fini_cancel_response) // Finalize with invalid cancel response ret = rcl_action_cancel_response_fini(nullptr); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); // Finalize with valid arguments ret = rcl_action_cancel_response_fini(&cancel_response); - EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } diff --git a/rcl_action/test/rcl_action/test_wait.cpp b/rcl_action/test/rcl_action/test_wait.cpp new file mode 100644 index 0000000..45aaa39 --- /dev/null +++ b/rcl_action/test/rcl_action/test_wait.cpp @@ -0,0 +1,813 @@ +// 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 "rcl_action/action_client.h" +#include "rcl_action/action_server.h" +#include "rcl_action/wait.h" +#include "rcl_action/action_client_impl.h" +#include "rcl_action/action_server_impl.h" + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "test_msgs/action/fibonacci.h" + +class TestActionClientWait : public ::testing::Test +{ +protected: + void SetUp() override + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &this->context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_options = rcl_node_get_default_options(); + const char * node_name = "test_action_client_node"; + ret = rcl_node_init(&this->node, node_name, "", &this->context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str; + + const char * action_name = "test_action_client_name"; + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + const rcl_action_client_options_t action_client_options = + rcl_action_client_get_default_options(); + + action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_client_init( + &this->action_client, &this->node, action_typesupport, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t fini_ret = rcl_action_client_fini(&action_client, &this->node); + EXPECT_EQ(RCL_RET_OK, fini_ret) << rcl_get_error_string().str; + rcl_ret_t ret = rcl_node_fini(&this->node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + rcl_context_t context; + rcl_node_t node; + rcl_action_client_t action_client; +}; + +class TestActionServerWait : public ::testing::Test +{ +protected: + void SetUp() override + { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(&this->node, "test_action_server_node", "", &context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_clock_init(RCL_ROS_TIME, &this->clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT( + test_msgs, Fibonacci); + const rcl_action_server_options_t options = rcl_action_server_get_default_options(); + const char * action_name = "test_action_server_name"; + this->action_server = rcl_action_get_zero_initialized_server(); + ret = rcl_action_server_init( + &this->action_server, &this->node, &this->clock, ts, action_name, &options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str; + } + + void TearDown() override + { + // Finalize + rcl_ret_t ret = rcl_action_server_fini(&this->action_server, &this->node); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_clock_fini(&this->clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_node_fini(&this->node); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_shutdown(&context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void init_test_uuid0(uint8_t * uuid) + { + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + uuid[i] = i; + } + } + + void init_test_uuid1(uint8_t * uuid) + { + for (uint8_t i = 0; i < UUID_SIZE; ++i) { + uuid[i] = 15 - i; + } + } + + rcl_action_server_t action_server; + rcl_context_t context; + rcl_node_t node; + rcl_clock_t clock; +}; // class TestActionServer + +TEST_F(TestActionClientWait, test_wait_set_add_action_client) { + // Check wait_set is null + size_t client_index = 42; + size_t subscription_index = 42; + rcl_ret_t ret = rcl_action_wait_set_add_action_client( + nullptr, &action_client, &client_index, &subscription_index); + EXPECT_EQ(RCL_RET_WAIT_SET_INVALID, ret); + EXPECT_EQ(42u, client_index); + EXPECT_EQ(42u, subscription_index); + rcl_reset_error(); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + }); + + // Check action client is null + ret = rcl_action_wait_set_add_action_client( + &wait_set, nullptr, &client_index, &subscription_index); + EXPECT_EQ(RCL_RET_ACTION_CLIENT_INVALID, ret); + EXPECT_EQ(42u, client_index); + EXPECT_EQ(42u, subscription_index); + rcl_reset_error(); + + // Failed to add goal client + wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 0, 0, 0, &this->context, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &action_client, &client_index, &subscription_index); + EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret); + EXPECT_EQ(42u, client_index); + EXPECT_EQ(42u, subscription_index); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Failed to add cancel client + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 1, 0, 0, &this->context, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &action_client, &client_index, &subscription_index); + EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret); + EXPECT_EQ(42u, client_index); + EXPECT_EQ(42u, subscription_index); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + + // Failed to add result client + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 2, 0, 0, &this->context, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &action_client, &client_index, &subscription_index); + EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret); + EXPECT_EQ(42u, client_index); + EXPECT_EQ(42u, subscription_index); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Failed to add feedback subscription + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 3, 0, 0, &this->context, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &action_client, &client_index, &subscription_index); + EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret); + EXPECT_EQ(42u, client_index); + EXPECT_EQ(42u, subscription_index); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Failed to add status subscription + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 1, 0, 0, 3, 0, 0, &this->context, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &action_client, &client_index, &subscription_index); + EXPECT_EQ(RCL_RET_WAIT_SET_FULL, ret); + EXPECT_EQ(42u, client_index); + EXPECT_EQ(42u, subscription_index); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Typical case + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 2, 0, 0, 3, 0, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &action_client, &client_index, &subscription_index); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(0u, client_index); + EXPECT_EQ(0u, subscription_index); + + // Should work fine, but doesn't increment counts + // wait_set = rcl_get_zero_initialized_wait_set(); + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 2, 0, 0, 3, 0, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_action_wait_set_add_action_client(&wait_set, &action_client, nullptr, nullptr); + EXPECT_EQ(RCL_RET_OK, ret); +} + +TEST_F(TestActionServerWait, test_wait_set_add_action_server) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + }); + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 0, 0, 0, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + size_t service_index = 42; + ret = rcl_action_wait_set_add_action_server(nullptr, &this->action_server, &service_index); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_INVALID); + EXPECT_EQ(service_index, 42u); + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_server(&wait_set, nullptr, &service_index); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + EXPECT_EQ(service_index, 42u); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Check adding goal service fails + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 0, 0, 0, &this->context, rcl_get_default_allocator()); + + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str; + EXPECT_EQ(service_index, 42u); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Check adding cancel service fails + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 0, 1, 0, &this->context, rcl_get_default_allocator()); + + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str; + EXPECT_EQ(service_index, 42u); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Check adding result service fails + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 0, 2, 0, &this->context, rcl_get_default_allocator()); + + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str; + EXPECT_EQ(service_index, 42u); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Check adding expire timer fails + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 0, 0, 3, 0, &this->context, rcl_get_default_allocator()); + + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_FULL) << rcl_get_error_string().str; + EXPECT_EQ(service_index, 42u); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Check everything is good + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 1, 0, 3, 0, &this->context, rcl_get_default_allocator()); + + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, &service_index); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(service_index, 0u); + + // Everything should be ok without a valid service index. + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &wait_set, 0, 0, 1, 0, 3, 0, &this->context, rcl_get_default_allocator()); + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, nullptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(rcl_error_is_set()) << rcl_get_error_string().str; +} + + +TEST_F(TestActionClientWait, test_client_wait_set_get_num_entities) { + const char * action_name = "test_action_client_name"; + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + const rcl_action_client_options_t action_client_options = + rcl_action_client_get_default_options(); + + rcl_action_client_t action_client = rcl_action_get_zero_initialized_client(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t fini_ret = rcl_action_client_fini(&action_client, &this->node); + EXPECT_EQ(RCL_RET_OK, fini_ret) << rcl_get_error_string().str; + }); + + rcl_ret_t ret = rcl_action_client_init( + &action_client, &this->node, action_typesupport, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + size_t num_subscriptions = 0; + size_t num_guard_conditions = 0; + size_t num_timers = 0; + size_t num_clients = 0; + size_t num_services = 0; + + ret = rcl_action_client_wait_set_get_num_entities( + nullptr, &num_subscriptions, &num_guard_conditions, &num_timers, &num_clients, &num_services); + EXPECT_EQ(RCL_RET_ACTION_CLIENT_INVALID, ret); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_num_entities( + &action_client, nullptr, &num_guard_conditions, &num_timers, &num_clients, &num_services); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_num_entities( + &action_client, &num_subscriptions, nullptr, &num_timers, &num_clients, &num_services); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_num_entities( + &action_client, + &num_subscriptions, + &num_guard_conditions, + nullptr, + &num_clients, + &num_services); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_num_entities( + &action_client, + &num_subscriptions, + &num_guard_conditions, + &num_clients, + nullptr, + &num_services); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_num_entities( + &action_client, + &num_subscriptions, + &num_guard_conditions, + &num_clients, + &num_clients, + nullptr); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_num_entities( + &action_client, + &num_subscriptions, + &num_guard_conditions, + &num_clients, + &num_clients, + &num_services); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(num_subscriptions, 2u); + EXPECT_EQ(num_guard_conditions, 0u); + EXPECT_EQ(num_timers, 0u); + EXPECT_EQ(num_clients, 3u); + EXPECT_EQ(num_services, 0u); +} + +TEST_F(TestActionServerWait, test_server_wait_set_get_num_entities) { + size_t num_subscriptions = 0; + size_t num_guard_conditions = 0; + size_t num_timers = 0; + size_t num_clients = 0; + size_t num_services = 0; + + rcl_ret_t ret = rcl_action_server_wait_set_get_num_entities( + nullptr, &num_subscriptions, &num_guard_conditions, &num_timers, &num_clients, &num_services); + EXPECT_EQ(RCL_RET_ACTION_SERVER_INVALID, ret); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_num_entities( + &this->action_server, + nullptr, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_num_entities( + &this->action_server, + &num_subscriptions, + nullptr, + &num_timers, + &num_clients, + &num_services); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_num_entities( + &this->action_server, + &num_subscriptions, + &num_guard_conditions, + nullptr, + &num_clients, + &num_services); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_num_entities( + &this->action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + nullptr, + &num_services); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_num_entities( + &this->action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_num_entities( + &this->action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(num_subscriptions, 0u); + EXPECT_EQ(num_guard_conditions, 0u); + EXPECT_EQ(num_timers, 1u); + EXPECT_EQ(num_clients, 0u); + EXPECT_EQ(num_services, 3u); +} + +TEST_F(TestActionClientWait, test_client_wait_set_get_entities_ready) { + const char * action_name = "test_action_client_name"; + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + const rcl_action_client_options_t action_client_options = + rcl_action_client_get_default_options(); + + rcl_action_client_t action_client = rcl_action_get_zero_initialized_client(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t fini_ret = rcl_action_client_fini(&action_client, &this->node); + EXPECT_EQ(RCL_RET_OK, fini_ret) << rcl_get_error_string().str; + }); + + rcl_ret_t ret = rcl_action_client_init( + &action_client, &this->node, action_typesupport, + action_name, &action_client_options); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 1, &this->context, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + bool is_feedback_ready = false; + bool is_status_ready = false; + bool is_goal_response_ready = false; + bool is_cancel_response_ready = false; + bool is_result_response_ready = false; + + // Check valid arguments + ret = rcl_action_client_wait_set_get_entities_ready( + nullptr, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_INVALID); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + nullptr, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + nullptr, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + nullptr, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + nullptr, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + nullptr, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + // Even though they should be different subscriptions and clients, we can mock the correct + // behavior by assigning all three clients to the same index, and both subscriptions as well + wait_set.size_of_subscriptions = 1; + wait_set.size_of_clients = 1; + + // Check wait indices are out of bounds + action_client.impl->wait_set_feedback_subscription_index = 10; + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcl_reset_error(); + action_client.impl->wait_set_feedback_subscription_index = 0; + + action_client.impl->wait_set_status_subscription_index = 10; + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcl_reset_error(); + action_client.impl->wait_set_status_subscription_index = 0; + + action_client.impl->wait_set_goal_client_index = 10; + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcl_reset_error(); + action_client.impl->wait_set_goal_client_index = 0; + + action_client.impl->wait_set_cancel_client_index = 10; + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcl_reset_error(); + action_client.impl->wait_set_cancel_client_index = 0; + + action_client.impl->wait_set_result_client_index = 10; + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_ERROR); + rcl_reset_error(); + action_client.impl->wait_set_result_client_index = 0; + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &action_client, + &is_feedback_ready, + &is_status_ready, + &is_goal_response_ready, + &is_cancel_response_ready, + &is_result_response_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(is_feedback_ready); + EXPECT_FALSE(is_status_ready); + EXPECT_FALSE(is_goal_response_ready); + EXPECT_FALSE(is_cancel_response_ready); + EXPECT_FALSE(is_result_response_ready); +} + +TEST_F(TestActionServerWait, test_server_wait_set_get_entities_ready) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + }); + + bool is_goal_request_ready = false; + bool is_cancel_request_ready = false; + bool is_result_request_ready = false; + bool is_goal_expired = false; + + rcl_ret_t ret = rcl_action_server_wait_set_get_entities_ready( + nullptr, + &this->action_server, + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + EXPECT_EQ(ret, RCL_RET_WAIT_SET_INVALID); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_entities_ready( + &wait_set, + nullptr, + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_entities_ready( + &wait_set, + &this->action_server, + nullptr, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_entities_ready( + &wait_set, + &this->action_server, + &is_goal_request_ready, + nullptr, + &is_result_request_ready, + &is_goal_expired); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + ret = rcl_action_server_wait_set_get_entities_ready( + &wait_set, + &this->action_server, + &is_goal_request_ready, + &is_cancel_request_ready, + nullptr, + &is_goal_expired); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + + ret = rcl_action_server_wait_set_get_entities_ready( + &wait_set, + &this->action_server, + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + + ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 3, 0, &this->context, rcl_get_default_allocator()); + wait_set.services[0] = &this->action_server.impl->goal_service; + this->action_server.impl->wait_set_goal_service_index = 0; + wait_set.services[1] = &this->action_server.impl->cancel_service; + this->action_server.impl->wait_set_cancel_service_index = 1; + wait_set.services[2] = &this->action_server.impl->result_service; + this->action_server.impl->wait_set_result_service_index = 2; + wait_set.timers[0] = &this->action_server.impl->expire_timer; + this->action_server.impl->wait_set_expire_timer_index = 0; + ret = rcl_action_server_wait_set_get_entities_ready( + &wait_set, + &this->action_server, + &is_goal_request_ready, + &is_cancel_request_ready, + &is_result_request_ready, + &is_goal_expired); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_TRUE(is_goal_request_ready); + EXPECT_TRUE(is_cancel_request_ready); + EXPECT_TRUE(is_result_request_ready); + EXPECT_TRUE(is_goal_expired); +}