From bde76ab40e34217368e5dc32234fd8fea85b5cd2 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Tue, 13 Nov 2018 11:10:16 -0300 Subject: [PATCH 1/5] [rcl action] Augments test_action_communication test. --- .../rcl_action/test_action_communication.cpp | 625 ++++++++++++++---- 1 file changed, 479 insertions(+), 146 deletions(-) diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp index f6bcf1f..cf09ec6 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -13,11 +13,14 @@ // limitations under the License. #include +#include "rcl_action/action_client.h" #include "rcl_action/action_server.h" #include "rcl/error_handling.h" #include "rcl/rcl.h" +#include "rosidl_generator_c/primitives_sequence_functions.h" + #include "test_msgs/action/fibonacci.h" #ifdef RMW_IMPLEMENTATION @@ -27,7 +30,6 @@ # define CLASSNAME(NAME, SUFFIX) NAME #endif -// TODO(jacobperron): Add action client to complete tests class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing::Test { protected: @@ -43,11 +45,16 @@ protected: ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator); 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_commmunication_name"; + const rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); 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); + &this->action_server, &this->node, &this->clock, ts, action_name, &server_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const rcl_action_client_options_t client_options = rcl_action_client_get_default_options(); + this->action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_client_init( + &this->action_client, &this->node, ts, action_name, &client_options); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } @@ -57,273 +64,555 @@ protected: 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); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_action_client_fini(&this->action_client, &this->node); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_node_fini(&this->node); - EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_shutdown(); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } void init_test_uuid0(uint8_t * uuid) { - for (uint8_t i = 0; i < 16; ++i) { + 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 < 16; ++i) { + for (uint8_t i = 0; i < UUID_SIZE; ++i) { uuid[i] = 15 - i; } } + rcl_action_client_t action_client; rcl_action_server_t action_server; rcl_node_t node; rcl_clock_t clock; }; // class TestActionCommunication -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_take_goal_request) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_goal_request_comm) { - test_msgs__action__Fibonacci_Goal_Request goal_request; - test_msgs__action__Fibonacci_Goal_Request__init(&goal_request); + test_msgs__action__Fibonacci_Goal_Request outgoing_goal_request; + test_msgs__action__Fibonacci_Goal_Request incoming_goal_request; + test_msgs__action__Fibonacci_Goal_Request__init(&outgoing_goal_request); + test_msgs__action__Fibonacci_Goal_Request__init(&incoming_goal_request); - // Take request with null action server - rmw_request_id_t request_header; - rcl_ret_t ret = rcl_action_take_goal_request(nullptr, &request_header, &goal_request); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + // Initialize goal request + init_test_uuid0(outgoing_goal_request.uuid); + outgoing_goal_request.order = 10; + + // Send goal request with null action client + rcl_ret_t ret = rcl_action_send_goal_request(nullptr, &outgoing_goal_request); + ASSERT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID); rcl_reset_error(); - // Take request with null header - ret = rcl_action_take_goal_request(&this->action_server, nullptr, &goal_request); + // Send goal request with invalid action client + rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_send_goal_request(&invalid_action_client, &outgoing_goal_request); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID); + rcl_reset_error(); + + // Send goal request with null message + ret = rcl_action_send_goal_request(&this->action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send goal request with valid arguments + ret = rcl_action_send_goal_request(&this->action_client, &outgoing_goal_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take goal request with null action server + rmw_request_id_t request_header; + ret = rcl_action_take_goal_request(nullptr, &request_header, &incoming_goal_request); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take goal request with null header + ret = rcl_action_take_goal_request(&this->action_server, nullptr, &incoming_goal_request); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); rcl_reset_error(); - // Take request with null message + // Take goal request with null message ret = rcl_action_take_goal_request(&this->action_server, &request_header, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Take request with invalid action server + // Take goal request with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_take_goal_request(&invalid_action_server, &request_header, &goal_request); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + ret = rcl_action_take_goal_request(&invalid_action_server, &incoming_goal_request); + ret = rcl_action_take_goal_request(&invalid_action_server, &request_header, &incoming_goal_request); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take with valid arguments - // TODO(jacobperron): Send a request from a client - // ret = rcl_action_take_goal_request(&this->action_server, &request_header, &goal_request); - // EXPECT_EQ(ret, RCL_RET_OK); + // Take goal request with valid arguments + ret = rcl_action_take_goal_request(&this->action_server, &request_header, &incoming_goal_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); - test_msgs__action__Fibonacci_Goal_Request__fini(&goal_request); + // Check that the goal request was received correctly + EXPECT_EQ(outgoing_goal_request.order, incoming_goal_request.order); + EXPECT_TRUE(uuidcmp(outgoing_goal_request.uuid, incoming_goal_request.uuid)); + + test_msgs__action__Fibonacci_Goal_Request__fini(&outgoing_goal_request); + test_msgs__action__Fibonacci_Goal_Request__init(&incoming_goal_request); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_send_goal_response) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_goal_response_comm) { - test_msgs__action__Fibonacci_Goal_Response goal_response; - test_msgs__action__Fibonacci_Goal_Response__init(&goal_response); + test_msgs__action__Fibonacci_Goal_Response outgoing_goal_response; + test_msgs__action__Fibonacci_Goal_Response incoming_goal_response; + test_msgs__action__Fibonacci_Goal_Response__init(&outgoing_goal_response); + test_msgs__action__Fibonacci_Goal_Response__init(&incoming_goal_response); - // Send response with null action server + // Initialize goal response + outgoing_goal_response.accepted = true; + outgoing_goal_response.stamp.sec = 123; + outgoing_goal_response.stamp.nanosec = 456789u; + + // Send goal response with null action server rmw_request_id_t response_header; - rcl_ret_t ret = rcl_action_send_goal_response(nullptr, &response_header, &goal_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + rcl_ret_t ret = rcl_action_send_goal_response(nullptr, &response_header, &outgoing_goal_response); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Send response with null header - ret = rcl_action_send_goal_response(&this->action_server, nullptr, &goal_response); + // Send goal response with null header + ret = rcl_action_send_goal_response(&this->action_server, nullptr, &outgoing_goal_response); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); rcl_reset_error(); - // Send response with null message + // Send goal response with null message ret = rcl_action_send_goal_response(&this->action_server, &response_header, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send goal response with invalid action server + rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); + ret = rcl_action_send_goal_response(&invalid_action_server, &outgoing_goal_response); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send goal response with valid arguments + ret = rcl_action_send_goal_response(&this->action_server, &response_header, &outgoing_goal_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take goal response with null action client + ret = rcl_action_take_goal_response(nullptr, &response_header, &incoming_goal_response); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID); + rcl_reset_error(); + + // Take goal response with null header + ret = rcl_action_take_goal_response(&this->action_client, nullptr, &incoming_goal_response); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); rcl_reset_error(); - // Send response with invalid action server - rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_send_goal_response(&invalid_action_server, &response_header, &goal_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + // Take goal response with null message + ret = rcl_action_take_goal_response(&this->action_client, &response_header, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); rcl_reset_error(); - // Send with valid arguments - // TODO(jacobperron): Check with client on receiving end - ret = rcl_action_send_goal_response(&this->action_server, &response_header, &goal_response); - EXPECT_EQ(ret, RCL_RET_OK); + // Take goal response with invalid action client + rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_take_goal_response(&invalid_action_client, &incoming_goal_response); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); - test_msgs__action__Fibonacci_Goal_Response__fini(&goal_response); + // Take goal response with valid arguments + ret = rcl_action_take_goal_response(&this->action_client, &response_header, &incoming_goal_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the goal response was received correctly + EXPECT_EQ(outgoing_goal_response.accepted, incoming_goal_response.accepted); + EXPECT_EQ(outgoing_goal_response.stamp.sec, incoming_goal_response.stamp.sec); + EXPECT_EQ(outgoing_goal_response.stamp.nanosec, incoming_goal_response.stamp.nanosec); + + test_msgs__action__Fibonacci_Goal_Response__fini(&incoming_goal_response); + test_msgs__action__Fibonacci_Goal_Response__fini(&outgoing_goal_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_take_cancel_request) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_cancel_request_comm) { - action_msgs__srv__CancelGoal_Request cancel_request; - action_msgs__srv__CancelGoal_Request__init(&cancel_request); + action_msgs__srv__CancelGoal_Request outgoing_cancel_request; + action_msgs__srv__CancelGoal_Request incoming_cancel_request; + action_msgs__srv__CancelGoal_Request__init(&outgoing_cancel_request); + action_msgs__srv__CancelGoal_Request__init(&incoming_cancel_request); - // Take request with null action server - rmw_request_id_t request_header; - rcl_ret_t ret = rcl_action_take_cancel_request(nullptr, &request_header, &cancel_request); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + // Initialize cancel request + init_test_uuid0(outgoing_cancel_request.goal_info.uuid); + outgoing_cancel_request.goal_info.stamp.sec = 321; + outgoing_cancel_request.goal_info.stamp.nanosec = 987654u; + + // Send cancel request with null action client + rcl_ret_t ret = rcl_action_send_cancel_request(nullptr, &outgoing_cancel_request); + ASSERT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take request with null header - ret = rcl_action_take_cancel_request(&this->action_server, nullptr, &cancel_request); + // Send cancel request with invalid action client + rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_send_cancel_request(&invalid_action_client, &outgoing_cancel_request); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send cancel request with null message + ret = rcl_action_send_cancel_request(&this->action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send cancel request with valid arguments + ret = rcl_action_send_cancel_request(&this->action_client, &outgoing_cancel_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take cancel request with null action server + rmw_request_id_t request_header; + ret = rcl_action_take_cancel_request(nullptr, &request_header, &incoming_cancel_request); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take cancel request with null header + ret = rcl_action_take_cancel_request(&this->action_server, nullptr, &incoming_cancel_request); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); rcl_reset_error(); - // Take request with null message + // Take cancel request with null message ret = rcl_action_take_cancel_request(&this->action_server, &request_header, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Take request with invalid action server + // Take cancel request with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_take_cancel_request(&invalid_action_server, &request_header, &cancel_request); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + ret = rcl_action_take_cancel_request(&invalid_action_server, &request_header, &incoming_cancel_request); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take with valid arguments - // TODO(jacobperron): Send a request from a client - // ret = rcl_action_take_cancel_request(&this->action_server, &request_header, &cancel_request); - // EXPECT_EQ(ret, RCL_RET_OK); + // Take cancel request with valid arguments + ret = rcl_action_take_cancel_request(&this->action_server, &request_header, &incoming_cancel_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); - action_msgs__srv__CancelGoal_Request__fini(&cancel_request); + // Check that the cancel request was received correctly + EXPECT_TRUE(uuidcmp( + outgoing_cancel_request.goal_info.uuid, + incoming_cancel_request.goal_info.uuid)); + EXPECT_EQ( + outgoing_cancel_request.goal_info.stamp.sec, + incoming_cancel_request.goal_info.stamp.sec); + EXPECT_EQ( + outgoing_cancel_request.goal_info.stamp.nanosec, + incoming_cancel_request.goal_info.stamp.nanosec); + + action_msgs__srv__CancelGoal_Request__fini(&incoming_cancel_request); + action_msgs__srv__CancelGoal_Request__fini(&outgoing_cancel_request); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_send_cancel_response) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_cancel_response_comm) { - action_msgs__srv__CancelGoal_Response cancel_response; - action_msgs__srv__CancelGoal_Response__init(&cancel_response); + action_msgs__srv__CancelGoal_Response outgoing_cancel_response; + action_msgs__srv__CancelGoal_Response incoming_cancel_response; + action_msgs__srv__CancelGoal_Response__init(&outgoing_cancel_response); + action_msgs__srv__CancelGoal_Response__init(&incoming_cancel_response); - // Send response with null action server + // Initialize cancel request + ASSERT_TRUE(action_msgs__msg__GoalInfo__Sequence__init( + &outgoing_cancel_response.goals_canceling, 2)); + init_test_uuid0(outgoing_cancel_response.goals_canceling.data[0].uuid); + outgoing_cancel_response.goals_canceling.data[0].stamp.sec = 102; + outgoing_cancel_response.goals_canceling.data[0].stamp.nanosec = 9468u; + init_test_uuid1(outgoing_cancel_response.goals_canceling.data[1].uuid); + outgoing_cancel_response.goals_canceling.data[1].stamp.sec = 867; + outgoing_cancel_response.goals_canceling.data[1].stamp.nanosec = 6845u; + + // Send cancel response with null action server rmw_request_id_t response_header; - rcl_ret_t ret = rcl_action_send_cancel_response(nullptr, &response_header, &cancel_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + rcl_ret_t ret = rcl_action_send_cancel_response(nullptr, &response_header, &outgoing_cancel_response); + ASSERT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Send response with null header - ret = rcl_action_send_cancel_response(&this->action_server, nullptr, &cancel_response); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + // Send cancel response with invalid action server + rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); + ret = rcl_action_send_cancel_response(&invalid_action_server, &response_header, &outgoing_cancel_response); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Send response with null message + // Send cancel response with null header + ret = rcl_action_send_cancel_response(&this->action_server, nullptr, &outgoing_cancel_response); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send cancel response with null message ret = rcl_action_send_cancel_response(&this->action_server, &response_header, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Send response with invalid action server - rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_send_cancel_response(&invalid_action_server, &response_header, &cancel_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + // Send cancel response with valid arguments + ret = rcl_action_send_cancel_response(&this->action_server, &response_header, &outgoing_cancel_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - // Send with valid arguments - // TODO(jacobperron): Check with client on receiving end - ret = rcl_action_send_cancel_response(&this->action_server, &response_header, &cancel_response); - EXPECT_EQ(ret, RCL_RET_OK); + // Take cancel response with null action client + ret = rcl_action_take_cancel_response(nullptr, &incoming_cancel_response); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); - action_msgs__srv__CancelGoal_Response__fini(&cancel_response); + // Take cancel response with invalid action client + rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_take_cancel_response(&invalid_action_client, &incoming_cancel_response); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take cancel response with null message + ret = rcl_action_take_cancel_response(&this->action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take cancel response with valid arguments + ret = rcl_action_take_cancel_response(&this->action_client, &incoming_cancel_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the cancel response was received correctly + ASSERT_EQ( + outgoing_cancel_response.goals_canceling.size, + incoming_cancel_response.goals_canceling.size); + for (size_t i = 0; i < outgoing_cancel_response.goals_canceling.size; ++i) { + const action_msgs__msg__GoalInfo * outgoing_goal_info = + &outgoing_cancel_response.goals_canceling.data[i]; + const action_msgs__msg__GoalInfo * incoming_goal_info = + &incoming_cancel_response.goals_canceling.data[i]; + EXPECT_TRUE(uuidcmp(outgoing_goal_info->uuid, incoming_goal_info->uuid)); + EXPECT_EQ(outgoing_goal_info->stamp.sec, incoming_goal_info->stamp.sec); + EXPECT_EQ(outgoing_goal_info->stamp.nanosec, incoming_goal_info->stamp.nanosec); + } + + action_msgs__srv__CancelGoal_Response__fini(&incoming_cancel_response); + action_msgs__srv__CancelGoal_Response__fini(&outgoing_cancel_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_take_result_request) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_request_comm) { - test_msgs__action__Fibonacci_Result_Request result_request; - test_msgs__action__Fibonacci_Result_Request__init(&result_request); + test_msgs__action__Fibonacci_Result_Request outgoing_result_request; + test_msgs__action__Fibonacci_Result_Request incoming_result_request; + test_msgs__action__Fibonacci_Result_Request__init(&outgoing_result_request); + test_msgs__action__Fibonacci_Result_Request__init(&incoming_result_request); - // Take request with null action server + // Initialize result request + init_test_uuid0(outgoing_result_request.uuid); + + // Send result request with null action client + rcl_ret_t ret = rcl_action_send_result_request(nullptr, &outgoing_result_request); + ASSERT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send result request with invalid action client + rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_send_result_request(&invalid_action_client, &outgoing_result_request); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send result request with null message + ret = rcl_action_send_result_request(&this->action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send result request with valid arguments + ret = rcl_action_send_result_request(&this->action_client, &outgoing_result_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take result request with null action server rmw_request_id_t request_header; - rcl_ret_t ret = rcl_action_take_result_request(nullptr, &request_header, &result_request); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + ret = rcl_action_take_result_request(nullptr, &request_header, &incoming_result_request); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take request with null header - ret = rcl_action_take_result_request(&this->action_server, nullptr, &result_request); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + // Take result request with invalid action server + rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); + ret = rcl_action_take_result_request(&invalid_action_server, &request_header, &incoming_result_request); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take request with null message + // Take result request with null header + ret = rcl_action_take_result_request(&this->action_server, nullptr, &incoming_result_request); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take result request with null message ret = rcl_action_take_result_request(&this->action_server, &request_header, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Take request with invalid action server - rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_take_result_request(&invalid_action_server, &request_header, &result_request); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + // Take result request with valid arguments + ret = rcl_action_take_result_request(&this->action_server, &request_header, &incoming_result_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - // Take with valid arguments - // TODO(jacobperron): Send a request from a client - // ret = rcl_action_take_result_request(&this->action_server, &request_header, &result_request); - // EXPECT_EQ(ret, RCL_RET_OK); + // Check that the result request was received correctly + EXPECT_TRUE(uuidcmp( + outgoing_result_request.uuid, + incoming_result_request.uuid)); - test_msgs__action__Fibonacci_Result_Request__fini(&result_request); + test_msgs__action__Fibonacci_Result_Request__fini(&incoming_result_request); + test_msgs__action__Fibonacci_Result_Request__fini(&outgoing_result_request); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_send_result_response) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_response_comm) { - test_msgs__action__Fibonacci_Result_Response result_response; - test_msgs__action__Fibonacci_Result_Response__init(&result_response); + test_msgs__action__Fibonacci_Result_Response outgoing_result_response; + test_msgs__action__Fibonacci_Result_Response incoming_result_response; + test_msgs__action__Fibonacci_Result_Response__init(&outgoing_result_response); + test_msgs__action__Fibonacci_Result_Response__init(&incoming_result_response); - // Send response with null action server + // Initialize result response + ASSERT_TRUE(rosidl_generator_c__int32__Sequence__init( + &outgoing_result_response.sequence, 4)); + outgoing_result_response.sequence.data[0] = 0; + outgoing_result_response.sequence.data[1] = 1; + outgoing_result_response.sequence.data[2] = 2; + outgoing_result_response.sequence.data[3] = 6; + outgoing_result_response.status = + action_msgs__msg__GoalStatus__STATUS_SUCCEEDED; + + // Send result response with null action client rmw_request_id_t response_header; - rcl_ret_t ret = rcl_action_send_result_response(nullptr, &response_header, &result_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + rcl_ret_t ret = rcl_action_send_result_response(nullptr, &response_header, &outgoing_result_response); + ASSERT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Send response with null header - ret = rcl_action_send_result_response(&this->action_server, nullptr, &result_response); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); - rcl_reset_error(); - - // Send response with null message - ret = rcl_action_send_result_response(&this->action_server, &response_header, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); - rcl_reset_error(); - - // Send response with invalid action server + // Send result response with invalid action client rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_send_result_response(&invalid_action_server, &response_header, &result_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + ret = rcl_action_send_result_response(&invalid_action_server, &response_header, &outgoing_result_response); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Send with valid arguments - // TODO(jacobperron): Check with client on receiving end - ret = rcl_action_send_result_response(&this->action_server, &response_header, &result_response); - EXPECT_EQ(ret, RCL_RET_OK); + // Send result response with null header + ret = rcl_action_send_result_response(&this->action_server, nullptr, &outgoing_result_response); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); - test_msgs__action__Fibonacci_Result_Response__fini(&result_response); + // Send result response with null message + ret = rcl_action_send_result_response(&this->action_server, &response_header, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send result response with valid arguments + ret = rcl_action_send_result_response(&this->action_server, &response_header, &outgoing_result_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take result response with null action client + ret = rcl_action_take_result_response(nullptr, &incoming_result_response); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take result response with null message + ret = rcl_action_take_result_response(&this->action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take result response with invalid action client + rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_take_result_response(&invalid_action_client, &incoming_result_response); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take result response with valid arguments + ret = rcl_action_take_result_response(&this->action_client, &incoming_result_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the result response was received correctly + EXPECT_EQ(outgoing_result_response.status, incoming_result_response.status); + ASSERT_EQ(outgoing_result_response.sequence.size, incoming_result_response.sequence.size); + EXPECT_TRUE(!memcmp( + outgoing_result_response.sequence.data, + incoming_result_response.sequence.data, + outgoing_result_response.sequence.size)); + + test_msgs__action__Fibonacci_Result_Response__fini(&incoming_result_response); + test_msgs__action__Fibonacci_Result_Response__fini(&outgoing_result_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_publish_feedback) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_feedback_comm) { - test_msgs__action__Fibonacci_Feedback feedback; - test_msgs__action__Fibonacci_Feedback__init(&feedback); + test_msgs__action__Fibonacci_Feedback outgoing_feedback; + test_msgs__action__Fibonacci_Feedback incoming_feedback; + test_msgs__action__Fibonacci_Feedback__init(&outgoing_feedback); + test_msgs__action__Fibonacci_Feedback__init(&incoming_feedback); + + // Initialize feedback + ASSERT_TRUE(rosidl_generator_c__int32__Sequence__init( + &outgoing_feedback.sequence, 3)); + outgoing_feedback.sequence.data[0] = 0; + outgoing_feedback.sequence.data[1] = 1; + outgoing_feedback.sequence.data[2] = 2; + init_test_uuid0(outgoing_feedback.uuid); // Publish feedback with null action server - rcl_ret_t ret = rcl_action_publish_feedback(nullptr, &feedback); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + rcl_ret_t ret = rcl_action_publish_feedback(nullptr, &outgoing_feedback); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Publish feedback with null message ret = rcl_action_publish_feedback(&this->action_server, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); // Publish feedback with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_publish_feedback(&invalid_action_server, &feedback); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + ret = rcl_action_publish_feedback(&invalid_action_server, &outgoing_feedback); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Publish feedback with valid arguments - // TODO(jacobperron): Check with client on receiving end - ret = rcl_action_publish_feedback(&this->action_server, &feedback); - EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); - test_msgs__action__Fibonacci_Feedback__fini(&feedback); + // Take feedback with null action client + ret = rcl_action_take_feedback(nullptr, &incoming_feedback); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take feedback with invalid action client + rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_take_feedback(&invalid_action_client, &incoming_feedback); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take feedback with null message + ret = rcl_action_take_feedback(&this->action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take feedback with valid arguments + ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that feedback was received correctly + EXPECT_TRUE(uuidcmp(outgoing_feedback.uuid, incoming_feedback.uuid)); + ASSERT_EQ(outgoing_feedback.sequence.size, incoming_feedback.sequence.size); + EXPECT_TRUE(!memcmp( + outgoing_feedback.sequence.data, + incoming_feedback.sequence.data, + outgoing_feedback.sequence.size)); + + test_msgs__action__Fibonacci_Feedback__fini(&incoming_feedback); + test_msgs__action__Fibonacci_Feedback__fini(&outgoing_feedback); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_publish_status) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_status_comm) { + action_msgs__msg__GoalStatusArray incoming_status_array; + action_msgs__msg__GoalStatusArray__init(&incoming_status_array); + + // Using rcl_action_goal_status_array_t in lieu of a message instance works + // because these tests make use of C type support rcl_action_goal_status_array_t status_array = rcl_action_get_zero_initialized_goal_status_array(); rcl_ret_t ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); @@ -331,39 +620,83 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_publish_stat // Publish status with null action server ret = rcl_action_publish_status(nullptr, &status_array.msg); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Publish status with null message ret = rcl_action_publish_status(&this->action_server, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); // Publish status with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); ret = rcl_action_publish_status(&invalid_action_server, &status_array.msg); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Publish status with valid arguments (but empty array) - // TODO(jacobperron): Check with client on receiving end ret = rcl_action_publish_status(&this->action_server, &status_array.msg); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take status with null action client + ret = rcl_action_take_status(nullptr, &incoming_status_array); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take status with invalid action client + rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); + ret = rcl_action_take_status(&invalid_action_client, &incoming_status_array); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take status with null message + ret = rcl_action_take_status(&this->action_client, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take status with valid arguments (empty array) + ret = rcl_action_take_status(&this->action_client, &incoming_status_array); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); - // Add a goal before publishing the status array ret = rcl_action_goal_status_array_fini(&status_array); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + // Add a goal before publishing the status array rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); rcl_action_goal_handle_t * goal_handle; goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info); ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str; ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + // Publish status with valid arguments (one goal in array) - // TODO(jacobperron): Check with client on receiving end ret = rcl_action_publish_status(&this->action_server, &status_array.msg); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take status with valid arguments (one goal in array) + ret = rcl_action_take_status(&this->action_client, &incoming_status_array); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that status was received correctly + ASSERT_EQ(status_array.msg.status_list.size, incoming_status_array.status_list.size); + for (size_t i = 0; i < status_array.msg.status_list.size; ++i) { + const action_msgs__msg__GoalStatus * outgoing_status = + &status_array.msg.status_list.data[i]; + const action_msgs__msg__GoalStatus * incoming_status = + &incoming_status_array.status_list.data[i]; + EXPECT_TRUE(uuidcmp(outgoing_status->goal_info.uuid, incoming_status->goal_info.uuid)); + EXPECT_EQ(outgoing_status->goal_info.stamp.sec, incoming_status->goal_info.stamp.sec); + EXPECT_EQ(outgoing_status->goal_info.stamp.nanosec, incoming_status->goal_info.stamp.nanosec); + EXPECT_EQ(outgoing_status->status, incoming_status->status); + } ret = rcl_action_goal_status_array_fini(&status_array); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + action_msgs__msg__GoalStatusArray__fini(&incoming_status_array); } From 4ee318238087428539b0bb94688f3c280270c275 Mon Sep 17 00:00:00 2001 From: Alexis Pojomovsky Date: Thu, 22 Nov 2018 15:29:34 -0300 Subject: [PATCH 2/5] Updated action tests to match the updated API Added action cancel test Added action result communication test Added action status communication test Added action feedback communication test Fix wrong return code in action client --- rcl_action/src/rcl_action/action_client.c | 4 +- .../rcl_action/test_action_communication.cpp | 1041 +++++++++++++---- 2 files changed, 833 insertions(+), 212 deletions(-) diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index f8e924d..ad004c3 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -243,7 +243,7 @@ rcl_action_client_get_default_options(void) #define SEND_SERVICE_REQUEST(Type, request, sequence_number) \ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending action " #Type " request"); \ if (!rcl_action_client_is_valid(action_client)) { \ - return RCL_RET_ACTION_SERVER_INVALID; /* error already set */ \ + return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ } \ RCL_CHECK_ARGUMENT_FOR_NULL(request, RCL_RET_INVALID_ARGUMENT); \ RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, RCL_RET_INVALID_ARGUMENT); \ @@ -259,7 +259,7 @@ rcl_action_client_get_default_options(void) #define TAKE_SERVICE_RESPONSE(Type, response_header, response) \ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Taking action " #Type " response"); \ if (!rcl_action_client_is_valid(action_client)) { \ - return RCL_RET_ACTION_SERVER_INVALID; /* error already set */ \ + return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ \ } \ RCL_CHECK_ARGUMENT_FOR_NULL(response_header, RCL_RET_INVALID_ARGUMENT); \ RCL_CHECK_ARGUMENT_FOR_NULL(response, RCL_RET_INVALID_ARGUMENT); \ diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp index cf09ec6..7ac2c4b 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -15,6 +15,7 @@ #include "rcl_action/action_client.h" #include "rcl_action/action_server.h" +#include "rcl_action/wait.h" #include "rcl/error_handling.h" #include "rcl/rcl.h" @@ -93,8 +94,785 @@ protected: rcl_clock_t clock; }; // class TestActionCommunication -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_goal_request_comm) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_comm) { + test_msgs__action__Fibonacci_Goal_Request outgoing_goal_request; + test_msgs__action__Fibonacci_Goal_Request incoming_goal_request; + test_msgs__action__Fibonacci_Goal_Response outgoing_goal_response; + test_msgs__action__Fibonacci_Goal_Response incoming_goal_response; + test_msgs__action__Fibonacci_Goal_Request__init(&outgoing_goal_request); + test_msgs__action__Fibonacci_Goal_Request__init(&incoming_goal_request); + test_msgs__action__Fibonacci_Goal_Response__init(&outgoing_goal_response); + test_msgs__action__Fibonacci_Goal_Response__init(&incoming_goal_response); + + // Initialize goal request + init_test_uuid0(outgoing_goal_request.uuid); + outgoing_goal_request.order = 10; + + // Send goal request with valid arguments + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_goal_request( + &this->action_client, &outgoing_goal_request, &sequence_number); + 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(); + + 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_server_wait_set_get_num_entities( + &this->action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, NULL); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + bool is_goal_request_ready(false); + bool is_cancel_request_ready(false); + bool is_result_request_ready(false); + + ret = rcl_wait(&wait_set, 1000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + 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); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take goal request with valid arguments + rmw_request_id_t request_header; + ret = rcl_action_take_goal_request(&this->action_server, &request_header, &incoming_goal_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the goal request was received correctly + EXPECT_EQ(outgoing_goal_request.order, incoming_goal_request.order); + rcl_reset_error(); + EXPECT_TRUE(uuidcmp(outgoing_goal_request.uuid, incoming_goal_request.uuid)); + rcl_reset_error(); + + // Initialize goal response + outgoing_goal_response.accepted = true; + outgoing_goal_response.stamp.sec = 123; + outgoing_goal_response.stamp.nanosec = 456789u; + + // Send goal response with valid arguments + ret = rcl_action_send_goal_response( + &this->action_server, &request_header, &outgoing_goal_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + wait_set = rcl_get_zero_initialized_wait_set(); + + num_subscriptions = 0; + num_guard_conditions = 0; + num_timers = 0; + num_clients = 0; + num_services = 0; + + ret = rcl_action_client_wait_set_get_num_entities( + &this->action_client, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &this->action_client, NULL, NULL); + + ret = rcl_wait(&wait_set, 100000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + 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); + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &this->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_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take goal response with valid arguments + + ret = rcl_action_take_goal_response( + &this->action_client, &request_header, &incoming_goal_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the goal response was received correctly + EXPECT_EQ(outgoing_goal_response.accepted, incoming_goal_response.accepted); + rcl_reset_error(); + EXPECT_EQ(outgoing_goal_response.stamp.sec, incoming_goal_response.stamp.sec); + rcl_reset_error(); + EXPECT_EQ(outgoing_goal_response.stamp.nanosec, incoming_goal_response.stamp.nanosec); + rcl_reset_error(); + + test_msgs__action__Fibonacci_Goal_Request__fini(&outgoing_goal_request); + test_msgs__action__Fibonacci_Goal_Request__fini(&incoming_goal_request); + test_msgs__action__Fibonacci_Goal_Response__fini(&incoming_goal_response); + test_msgs__action__Fibonacci_Goal_Response__fini(&outgoing_goal_response); +} + + +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel_comm) +{ + action_msgs__srv__CancelGoal_Request outgoing_cancel_request; + action_msgs__srv__CancelGoal_Request incoming_cancel_request; + action_msgs__srv__CancelGoal_Response outgoing_cancel_response; + action_msgs__srv__CancelGoal_Response incoming_cancel_response; + action_msgs__srv__CancelGoal_Request__init(&outgoing_cancel_request); + action_msgs__srv__CancelGoal_Request__init(&incoming_cancel_request); + action_msgs__srv__CancelGoal_Response__init(&outgoing_cancel_response); + action_msgs__srv__CancelGoal_Response__init(&incoming_cancel_response); + + // Initialize cancel request + init_test_uuid0(outgoing_cancel_request.goal_info.uuid); + outgoing_cancel_request.goal_info.stamp.sec = 321; + outgoing_cancel_request.goal_info.stamp.nanosec = 987654u; + + // Send cancel request with valid arguments + int64_t sequence_number = 1324; + rcl_ret_t ret = rcl_action_send_cancel_request( + &this->action_client, &outgoing_cancel_request, &sequence_number); + 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(); + + 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_server_wait_set_get_num_entities( + &this->action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, NULL); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + bool is_goal_request_ready(false); + bool is_cancel_request_ready(false); + bool is_result_request_ready(false); + + ret = rcl_wait(&wait_set, 1000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + 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); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take cancel request with valid arguments + rmw_request_id_t request_header; + ret = rcl_action_take_cancel_request( + &this->action_server, &request_header, &incoming_cancel_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the cancel request was received correctly + EXPECT_TRUE(uuidcmp( + outgoing_cancel_request.goal_info.uuid, + incoming_cancel_request.goal_info.uuid)); + EXPECT_EQ( + outgoing_cancel_request.goal_info.stamp.sec, + incoming_cancel_request.goal_info.stamp.sec); + EXPECT_EQ( + outgoing_cancel_request.goal_info.stamp.nanosec, + incoming_cancel_request.goal_info.stamp.nanosec); + + // Initialize cancel request + ASSERT_TRUE(action_msgs__msg__GoalInfo__Sequence__init( + &outgoing_cancel_response.goals_canceling, 2)); + init_test_uuid0(outgoing_cancel_response.goals_canceling.data[0].uuid); + outgoing_cancel_response.goals_canceling.data[0].stamp.sec = 102; + outgoing_cancel_response.goals_canceling.data[0].stamp.nanosec = 9468u; + init_test_uuid1(outgoing_cancel_response.goals_canceling.data[1].uuid); + outgoing_cancel_response.goals_canceling.data[1].stamp.sec = 867; + outgoing_cancel_response.goals_canceling.data[1].stamp.nanosec = 6845u; + + // Send cancel response with valid arguments + // rmw_request_id_t response_header; + ret = rcl_action_send_cancel_response( + &this->action_server, &request_header, &outgoing_cancel_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + wait_set = rcl_get_zero_initialized_wait_set(); + + num_subscriptions = 0; + num_guard_conditions = 0; + num_timers = 0; + num_clients = 0; + num_services = 0; + + ret = rcl_action_client_wait_set_get_num_entities( + &this->action_client, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &this->action_client, NULL, NULL); + + ret = rcl_wait(&wait_set, 100000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + 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); + + ret = rcl_action_client_wait_set_get_entities_ready( + &wait_set, + &this->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_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take cancel response with valid arguments + ret = rcl_action_take_cancel_response( + &this->action_client, &request_header, &incoming_cancel_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the cancel response was received correctly + ASSERT_EQ( + outgoing_cancel_response.goals_canceling.size, + incoming_cancel_response.goals_canceling.size); + for (size_t i = 0; i < outgoing_cancel_response.goals_canceling.size; ++i) { + const action_msgs__msg__GoalInfo * outgoing_goal_info = + &outgoing_cancel_response.goals_canceling.data[i]; + const action_msgs__msg__GoalInfo * incoming_goal_info = + &incoming_cancel_response.goals_canceling.data[i]; + EXPECT_TRUE(uuidcmp(outgoing_goal_info->uuid, incoming_goal_info->uuid)); + EXPECT_EQ(outgoing_goal_info->stamp.sec, incoming_goal_info->stamp.sec); + EXPECT_EQ(outgoing_goal_info->stamp.nanosec, incoming_goal_info->stamp.nanosec); + } + + action_msgs__srv__CancelGoal_Request__fini(&incoming_cancel_request); + action_msgs__srv__CancelGoal_Request__fini(&outgoing_cancel_request); + action_msgs__srv__CancelGoal_Response__fini(&incoming_cancel_response); + action_msgs__srv__CancelGoal_Response__fini(&outgoing_cancel_response); +} + +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result_comm) +{ + test_msgs__action__Fibonacci_Result_Request outgoing_result_request; + test_msgs__action__Fibonacci_Result_Request incoming_result_request; + test_msgs__action__Fibonacci_Result_Response outgoing_result_response; + test_msgs__action__Fibonacci_Result_Response incoming_result_response; + test_msgs__action__Fibonacci_Result_Request__init(&outgoing_result_request); + test_msgs__action__Fibonacci_Result_Request__init(&incoming_result_request); + test_msgs__action__Fibonacci_Result_Response__init(&outgoing_result_response); + test_msgs__action__Fibonacci_Result_Response__init(&incoming_result_response); + + // Initialize result request + init_test_uuid0(outgoing_result_request.uuid); + + // Send result request with valid arguments + int64_t sequence_number; + rcl_ret_t ret = rcl_action_send_result_request( + &this->action_client, &outgoing_result_request, &sequence_number); + 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(); + + 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_server_wait_set_get_num_entities( + &this->action_server, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, NULL); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + bool is_goal_request_ready(false); + bool is_cancel_request_ready(false); + bool is_result_request_ready(false); + + ret = rcl_wait(&wait_set, 1000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + 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); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take result request with valid arguments + rmw_request_id_t request_header; + ret = rcl_action_take_result_request( + &this->action_server, &request_header, &incoming_result_request); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the result request was received correctly + EXPECT_TRUE(uuidcmp( + outgoing_result_request.uuid, + incoming_result_request.uuid)); + + // Initialize result response + ASSERT_TRUE(rosidl_generator_c__int32__Sequence__init( + &outgoing_result_response.sequence, 4)); + outgoing_result_response.sequence.data[0] = 0; + outgoing_result_response.sequence.data[1] = 1; + outgoing_result_response.sequence.data[2] = 2; + outgoing_result_response.sequence.data[3] = 6; + outgoing_result_response.status = + action_msgs__msg__GoalStatus__STATUS_SUCCEEDED; + + // Send result response with valid arguments + ret = rcl_action_send_result_response( + &this->action_server, &request_header, &outgoing_result_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + wait_set = rcl_get_zero_initialized_wait_set(); + + num_subscriptions = 0; + num_guard_conditions = 0; + num_timers = 0; + num_clients = 0; + num_services = 0; + + ret = rcl_action_client_wait_set_get_num_entities( + &this->action_client, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &this->action_client, NULL, NULL); + + ret = rcl_wait(&wait_set, 100000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send result response with invalid action client + rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); + ret = rcl_action_send_result_response( + &invalid_action_server, &request_header, &outgoing_result_response); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send result response with null header + ret = rcl_action_send_result_response(&this->action_server, nullptr, &outgoing_result_response); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send result response with null message + ret = rcl_action_send_result_response(&this->action_server, &request_header, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + + // Send result response with valid arguments + ret = rcl_action_send_result_response( + &this->action_server, &request_header, &outgoing_result_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take result response with valid arguments + ret = rcl_action_take_result_response( + &this->action_client, &request_header, &incoming_result_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that the result response was received correctly + EXPECT_EQ(outgoing_result_response.status, incoming_result_response.status); + ASSERT_EQ(outgoing_result_response.sequence.size, incoming_result_response.sequence.size); + EXPECT_TRUE(!memcmp( + outgoing_result_response.sequence.data, + incoming_result_response.sequence.data, + outgoing_result_response.sequence.size)); + + test_msgs__action__Fibonacci_Result_Request__fini(&incoming_result_request); + test_msgs__action__Fibonacci_Result_Request__fini(&outgoing_result_request); + test_msgs__action__Fibonacci_Result_Response__fini(&incoming_result_response); + test_msgs__action__Fibonacci_Result_Response__fini(&outgoing_result_response); +} + +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status_comm) +{ + action_msgs__msg__GoalStatusArray incoming_status_array; + action_msgs__msg__GoalStatusArray__init(&incoming_status_array); + + // Using rcl_action_goal_status_array_t in lieu of a message instance works + // because these tests make use of C type support + rcl_action_goal_status_array_t status_array = + rcl_action_get_zero_initialized_goal_status_array(); + rcl_ret_t ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + // Publish status with valid arguments (but empty array) + ret = rcl_action_publish_status(&this->action_server, &status_array.msg); + 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(); + + 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( + &this->action_client, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &this->action_client, NULL, NULL); + + ret = rcl_wait(&wait_set, 100000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take status with valid arguments (empty array) + ret = rcl_action_take_status(&this->action_client, &incoming_status_array); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_goal_status_array_fini(&status_array); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + // Add a goal before publishing the status array + rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + rcl_action_goal_handle_t * goal_handle; + goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info); + ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str; + + ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Publish status with valid arguments (one goal in array) + ret = rcl_action_publish_status(&this->action_server, &status_array.msg); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + wait_set = rcl_get_zero_initialized_wait_set(); + + num_subscriptions = 0; + num_guard_conditions = 0; + num_timers = 0; + num_clients = 0; + num_services = 0; + + ret = rcl_action_client_wait_set_get_num_entities( + &this->action_client, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &this->action_client, NULL, NULL); + + ret = rcl_wait(&wait_set, 100000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take status with valid arguments (one goal in array) + ret = rcl_action_take_status(&this->action_client, &incoming_status_array); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that status was received correctly + ASSERT_EQ(status_array.msg.status_list.size, incoming_status_array.status_list.size); + for (size_t i = 0; i < status_array.msg.status_list.size; ++i) { + const action_msgs__msg__GoalStatus * outgoing_status = + &status_array.msg.status_list.data[i]; + const action_msgs__msg__GoalStatus * incoming_status = + &incoming_status_array.status_list.data[i]; + EXPECT_TRUE(uuidcmp(outgoing_status->goal_info.uuid, incoming_status->goal_info.uuid)); + EXPECT_EQ(outgoing_status->goal_info.stamp.sec, incoming_status->goal_info.stamp.sec); + EXPECT_EQ(outgoing_status->goal_info.stamp.nanosec, incoming_status->goal_info.stamp.nanosec); + EXPECT_EQ(outgoing_status->status, incoming_status->status); + } + + ret = rcl_action_goal_status_array_fini(&status_array); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + action_msgs__msg__GoalStatusArray__fini(&incoming_status_array); +} + +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedback_comm) +{ + test_msgs__action__Fibonacci_Feedback outgoing_feedback; + test_msgs__action__Fibonacci_Feedback incoming_feedback; + test_msgs__action__Fibonacci_Feedback__init(&outgoing_feedback); + test_msgs__action__Fibonacci_Feedback__init(&incoming_feedback); + + // Initialize feedback + ASSERT_TRUE(rosidl_generator_c__int32__Sequence__init( + &outgoing_feedback.sequence, 3)); + outgoing_feedback.sequence.data[0] = 0; + outgoing_feedback.sequence.data[1] = 1; + outgoing_feedback.sequence.data[2] = 2; + init_test_uuid0(outgoing_feedback.uuid); + + // Publish feedback with valid arguments + rcl_ret_t ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback); + 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(); + + 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( + &this->action_client, + &num_subscriptions, + &num_guard_conditions, + &num_timers, + &num_clients, + &num_services); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, // number_of_subscriptions + num_guard_conditions, // number_of_guard_conditions + num_timers, // number_of_timers + num_clients, // number_of_clients + num_services, // number_of_services + rcl_get_default_allocator()); + + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_action_wait_set_add_action_client( + &wait_set, &this->action_client, NULL, NULL); + + ret = rcl_wait(&wait_set, 100000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Take feedback with valid arguments + ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check that feedback was received correctly + EXPECT_TRUE(uuidcmp(outgoing_feedback.uuid, incoming_feedback.uuid)); + ASSERT_EQ(outgoing_feedback.sequence.size, incoming_feedback.sequence.size); + EXPECT_TRUE(!memcmp( + outgoing_feedback.sequence.data, + incoming_feedback.sequence.data, + outgoing_feedback.sequence.size)); + + test_msgs__action__Fibonacci_Feedback__fini(&incoming_feedback); + test_msgs__action__Fibonacci_Feedback__fini(&outgoing_feedback); +} + +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_request_opts) { test_msgs__action__Fibonacci_Goal_Request outgoing_goal_request; test_msgs__action__Fibonacci_Goal_Request incoming_goal_request; test_msgs__action__Fibonacci_Goal_Request__init(&outgoing_goal_request); @@ -103,28 +881,26 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_goal_request // Initialize goal request init_test_uuid0(outgoing_goal_request.uuid); outgoing_goal_request.order = 10; + int64_t sequence_number = 1234; + rcl_ret_t ret = 0; // Send goal request with null action client - rcl_ret_t ret = rcl_action_send_goal_request(nullptr, &outgoing_goal_request); + ret = rcl_action_send_goal_request(nullptr, &outgoing_goal_request, &sequence_number); ASSERT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID); rcl_reset_error(); // Send goal request with invalid action client rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); - ret = rcl_action_send_goal_request(&invalid_action_client, &outgoing_goal_request); + ret = rcl_action_send_goal_request( + &invalid_action_client, &outgoing_goal_request, &sequence_number); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID); rcl_reset_error(); // Send goal request with null message - ret = rcl_action_send_goal_request(&this->action_client, nullptr); + ret = rcl_action_send_goal_request(&this->action_client, nullptr, &sequence_number); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Send goal request with valid arguments - ret = rcl_action_send_goal_request(&this->action_client, &outgoing_goal_request); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take goal request with null action server rmw_request_id_t request_header; ret = rcl_action_take_goal_request(nullptr, &request_header, &incoming_goal_request); @@ -143,26 +919,16 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_goal_request // Take goal request with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_take_goal_request(&invalid_action_server, &incoming_goal_request); - ret = rcl_action_take_goal_request(&invalid_action_server, &request_header, &incoming_goal_request); + ret = rcl_action_take_goal_request( + &invalid_action_server, &request_header, &incoming_goal_request); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take goal request with valid arguments - ret = rcl_action_take_goal_request(&this->action_server, &request_header, &incoming_goal_request); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Check that the goal request was received correctly - EXPECT_EQ(outgoing_goal_request.order, incoming_goal_request.order); - EXPECT_TRUE(uuidcmp(outgoing_goal_request.uuid, incoming_goal_request.uuid)); - test_msgs__action__Fibonacci_Goal_Request__fini(&outgoing_goal_request); test_msgs__action__Fibonacci_Goal_Request__init(&incoming_goal_request); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_goal_response_comm) -{ +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_response_opts) { test_msgs__action__Fibonacci_Goal_Response outgoing_goal_response; test_msgs__action__Fibonacci_Goal_Response incoming_goal_response; test_msgs__action__Fibonacci_Goal_Response__init(&outgoing_goal_response); @@ -191,15 +957,11 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_goal_respons // Send goal response with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_send_goal_response(&invalid_action_server, &outgoing_goal_response); + ret = rcl_action_send_goal_response( + &invalid_action_server, &response_header, &outgoing_goal_response); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Send goal response with valid arguments - ret = rcl_action_send_goal_response(&this->action_server, &response_header, &outgoing_goal_response); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take goal response with null action client ret = rcl_action_take_goal_response(nullptr, &response_header, &incoming_goal_response); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID); @@ -217,26 +979,16 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_goal_respons // Take goal response with invalid action client rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); - ret = rcl_action_take_goal_response(&invalid_action_client, &incoming_goal_response); + ret = rcl_action_take_goal_response( + &invalid_action_client, &response_header, &incoming_goal_response); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take goal response with valid arguments - ret = rcl_action_take_goal_response(&this->action_client, &response_header, &incoming_goal_response); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Check that the goal response was received correctly - EXPECT_EQ(outgoing_goal_response.accepted, incoming_goal_response.accepted); - EXPECT_EQ(outgoing_goal_response.stamp.sec, incoming_goal_response.stamp.sec); - EXPECT_EQ(outgoing_goal_response.stamp.nanosec, incoming_goal_response.stamp.nanosec); - test_msgs__action__Fibonacci_Goal_Response__fini(&incoming_goal_response); test_msgs__action__Fibonacci_Goal_Response__fini(&outgoing_goal_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_cancel_request_comm) -{ +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_cancel_request_opts) { action_msgs__srv__CancelGoal_Request outgoing_cancel_request; action_msgs__srv__CancelGoal_Request incoming_cancel_request; action_msgs__srv__CancelGoal_Request__init(&outgoing_cancel_request); @@ -248,26 +1000,24 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_cancel_reque outgoing_cancel_request.goal_info.stamp.nanosec = 987654u; // Send cancel request with null action client - rcl_ret_t ret = rcl_action_send_cancel_request(nullptr, &outgoing_cancel_request); + int64_t sequence_number = 1324; + rcl_ret_t ret = rcl_action_send_cancel_request( + nullptr, &outgoing_cancel_request, &sequence_number); ASSERT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Send cancel request with invalid action client rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); - ret = rcl_action_send_cancel_request(&invalid_action_client, &outgoing_cancel_request); + ret = rcl_action_send_cancel_request( + &invalid_action_client, &outgoing_cancel_request, &sequence_number); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Send cancel request with null message - ret = rcl_action_send_cancel_request(&this->action_client, nullptr); + ret = rcl_action_send_cancel_request(&this->action_client, nullptr, &sequence_number); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Send cancel request with valid arguments - ret = rcl_action_send_cancel_request(&this->action_client, &outgoing_cancel_request); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take cancel request with null action server rmw_request_id_t request_header; ret = rcl_action_take_cancel_request(nullptr, &request_header, &incoming_cancel_request); @@ -286,32 +1036,17 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_cancel_reque // Take cancel request with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_take_cancel_request(&invalid_action_server, &request_header, &incoming_cancel_request); + ret = rcl_action_take_cancel_request( + &invalid_action_server, &request_header, &incoming_cancel_request); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take cancel request with valid arguments - ret = rcl_action_take_cancel_request(&this->action_server, &request_header, &incoming_cancel_request); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Check that the cancel request was received correctly - EXPECT_TRUE(uuidcmp( - outgoing_cancel_request.goal_info.uuid, - incoming_cancel_request.goal_info.uuid)); - EXPECT_EQ( - outgoing_cancel_request.goal_info.stamp.sec, - incoming_cancel_request.goal_info.stamp.sec); - EXPECT_EQ( - outgoing_cancel_request.goal_info.stamp.nanosec, - incoming_cancel_request.goal_info.stamp.nanosec); - action_msgs__srv__CancelGoal_Request__fini(&incoming_cancel_request); action_msgs__srv__CancelGoal_Request__fini(&outgoing_cancel_request); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_cancel_response_comm) -{ + +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_cancel_response_opts) { action_msgs__srv__CancelGoal_Response outgoing_cancel_response; action_msgs__srv__CancelGoal_Response incoming_cancel_response; action_msgs__srv__CancelGoal_Response__init(&outgoing_cancel_response); @@ -329,13 +1064,15 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_cancel_respo // Send cancel response with null action server rmw_request_id_t response_header; - rcl_ret_t ret = rcl_action_send_cancel_response(nullptr, &response_header, &outgoing_cancel_response); + rcl_ret_t ret = rcl_action_send_cancel_response( + nullptr, &response_header, &outgoing_cancel_response); ASSERT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Send cancel response with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_send_cancel_response(&invalid_action_server, &response_header, &outgoing_cancel_response); + ret = rcl_action_send_cancel_response( + &invalid_action_server, &response_header, &outgoing_cancel_response); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); @@ -349,52 +1086,28 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_cancel_respo EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Send cancel response with valid arguments - ret = rcl_action_send_cancel_response(&this->action_server, &response_header, &outgoing_cancel_response); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take cancel response with null action client - ret = rcl_action_take_cancel_response(nullptr, &incoming_cancel_response); + ret = rcl_action_take_cancel_response(nullptr, &response_header, &incoming_cancel_response); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Take cancel response with invalid action client rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); - ret = rcl_action_take_cancel_response(&invalid_action_client, &incoming_cancel_response); + ret = rcl_action_take_cancel_response( + &invalid_action_client, &response_header, &incoming_cancel_response); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Take cancel response with null message - ret = rcl_action_take_cancel_response(&this->action_client, nullptr); + ret = rcl_action_take_cancel_response(&this->action_client, &response_header, nullptr); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Take cancel response with valid arguments - ret = rcl_action_take_cancel_response(&this->action_client, &incoming_cancel_response); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Check that the cancel response was received correctly - ASSERT_EQ( - outgoing_cancel_response.goals_canceling.size, - incoming_cancel_response.goals_canceling.size); - for (size_t i = 0; i < outgoing_cancel_response.goals_canceling.size; ++i) { - const action_msgs__msg__GoalInfo * outgoing_goal_info = - &outgoing_cancel_response.goals_canceling.data[i]; - const action_msgs__msg__GoalInfo * incoming_goal_info = - &incoming_cancel_response.goals_canceling.data[i]; - EXPECT_TRUE(uuidcmp(outgoing_goal_info->uuid, incoming_goal_info->uuid)); - EXPECT_EQ(outgoing_goal_info->stamp.sec, incoming_goal_info->stamp.sec); - EXPECT_EQ(outgoing_goal_info->stamp.nanosec, incoming_goal_info->stamp.nanosec); - } - action_msgs__srv__CancelGoal_Response__fini(&incoming_cancel_response); action_msgs__srv__CancelGoal_Response__fini(&outgoing_cancel_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_request_comm) -{ +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_result_request_opts) { test_msgs__action__Fibonacci_Result_Request outgoing_result_request; test_msgs__action__Fibonacci_Result_Request incoming_result_request; test_msgs__action__Fibonacci_Result_Request__init(&outgoing_result_request); @@ -404,26 +1117,24 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_reque init_test_uuid0(outgoing_result_request.uuid); // Send result request with null action client - rcl_ret_t ret = rcl_action_send_result_request(nullptr, &outgoing_result_request); + int64_t sequence_number = 1324; + rcl_ret_t ret = rcl_action_send_result_request( + nullptr, &outgoing_result_request, &sequence_number); ASSERT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Send result request with invalid action client rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); - ret = rcl_action_send_result_request(&invalid_action_client, &outgoing_result_request); + ret = rcl_action_send_result_request( + &invalid_action_client, &outgoing_result_request, &sequence_number); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Send result request with null message - ret = rcl_action_send_result_request(&this->action_client, nullptr); + ret = rcl_action_send_result_request(&this->action_client, nullptr, &sequence_number); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Send result request with valid arguments - ret = rcl_action_send_result_request(&this->action_client, &outgoing_result_request); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take result request with null action server rmw_request_id_t request_header; ret = rcl_action_take_result_request(nullptr, &request_header, &incoming_result_request); @@ -432,7 +1143,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_reque // Take result request with invalid action server rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_take_result_request(&invalid_action_server, &request_header, &incoming_result_request); + ret = rcl_action_take_result_request( + &invalid_action_server, &request_header, &incoming_result_request); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); @@ -446,22 +1158,11 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_reque EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Take result request with valid arguments - ret = rcl_action_take_result_request(&this->action_server, &request_header, &incoming_result_request); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Check that the result request was received correctly - EXPECT_TRUE(uuidcmp( - outgoing_result_request.uuid, - incoming_result_request.uuid)); - test_msgs__action__Fibonacci_Result_Request__fini(&incoming_result_request); test_msgs__action__Fibonacci_Result_Request__fini(&outgoing_result_request); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_response_comm) -{ +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_result_response_opts) { test_msgs__action__Fibonacci_Result_Response outgoing_result_response; test_msgs__action__Fibonacci_Result_Response incoming_result_response; test_msgs__action__Fibonacci_Result_Response__init(&outgoing_result_response); @@ -479,13 +1180,15 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_respo // Send result response with null action client rmw_request_id_t response_header; - rcl_ret_t ret = rcl_action_send_result_response(nullptr, &response_header, &outgoing_result_response); + rcl_ret_t ret = rcl_action_send_result_response( + nullptr, &response_header, &outgoing_result_response); ASSERT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Send result response with invalid action client rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_send_result_response(&invalid_action_server, &response_header, &outgoing_result_response); + ret = rcl_action_send_result_response( + &invalid_action_server, &response_header, &outgoing_result_response); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); @@ -499,46 +1202,28 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_result_respo EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Send result response with valid arguments - ret = rcl_action_send_result_response(&this->action_server, &response_header, &outgoing_result_response); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take result response with null action client - ret = rcl_action_take_result_response(nullptr, &incoming_result_response); + ret = rcl_action_take_result_response(nullptr, &response_header, &incoming_result_response); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); // Take result response with null message - ret = rcl_action_take_result_response(&this->action_client, nullptr); + ret = rcl_action_take_result_response(&this->action_client, &response_header, nullptr); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); // Take result response with invalid action client rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client(); - ret = rcl_action_take_result_response(&invalid_action_client, &incoming_result_response); + ret = rcl_action_take_result_response( + &invalid_action_client, &response_header, &incoming_result_response); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take result response with valid arguments - ret = rcl_action_take_result_response(&this->action_client, &incoming_result_response); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Check that the result response was received correctly - EXPECT_EQ(outgoing_result_response.status, incoming_result_response.status); - ASSERT_EQ(outgoing_result_response.sequence.size, incoming_result_response.sequence.size); - EXPECT_TRUE(!memcmp( - outgoing_result_response.sequence.data, - incoming_result_response.sequence.data, - outgoing_result_response.sequence.size)); - test_msgs__action__Fibonacci_Result_Response__fini(&incoming_result_response); test_msgs__action__Fibonacci_Result_Response__fini(&outgoing_result_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_feedback_comm) -{ +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_feedback_opts) { test_msgs__action__Fibonacci_Feedback outgoing_feedback; test_msgs__action__Fibonacci_Feedback incoming_feedback; test_msgs__action__Fibonacci_Feedback__init(&outgoing_feedback); @@ -568,11 +1253,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_feedback_com EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Publish feedback with valid arguments - ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take feedback with null action client ret = rcl_action_take_feedback(nullptr, &incoming_feedback); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; @@ -589,25 +1269,11 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_feedback_com EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Take feedback with valid arguments - ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Check that feedback was received correctly - EXPECT_TRUE(uuidcmp(outgoing_feedback.uuid, incoming_feedback.uuid)); - ASSERT_EQ(outgoing_feedback.sequence.size, incoming_feedback.sequence.size); - EXPECT_TRUE(!memcmp( - outgoing_feedback.sequence.data, - incoming_feedback.sequence.data, - outgoing_feedback.sequence.size)); - test_msgs__action__Fibonacci_Feedback__fini(&incoming_feedback); test_msgs__action__Fibonacci_Feedback__fini(&outgoing_feedback); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_status_comm) -{ +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_status_opts) { action_msgs__msg__GoalStatusArray incoming_status_array; action_msgs__msg__GoalStatusArray__init(&incoming_status_array); @@ -634,11 +1300,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_status_comm) EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Publish status with valid arguments (but empty array) - ret = rcl_action_publish_status(&this->action_server, &status_array.msg); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take status with null action client ret = rcl_action_take_status(nullptr, &incoming_status_array); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; @@ -655,46 +1316,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_status_comm) EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Take status with valid arguments (empty array) - ret = rcl_action_take_status(&this->action_client, &incoming_status_array); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_goal_status_array_fini(&status_array); - ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - - // Add a goal before publishing the status array - rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); - rcl_action_goal_handle_t * goal_handle; - goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info); - ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str; - ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Publish status with valid arguments (one goal in array) - ret = rcl_action_publish_status(&this->action_server, &status_array.msg); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Take status with valid arguments (one goal in array) - ret = rcl_action_take_status(&this->action_client, &incoming_status_array); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Check that status was received correctly - ASSERT_EQ(status_array.msg.status_list.size, incoming_status_array.status_list.size); - for (size_t i = 0; i < status_array.msg.status_list.size; ++i) { - const action_msgs__msg__GoalStatus * outgoing_status = - &status_array.msg.status_list.data[i]; - const action_msgs__msg__GoalStatus * incoming_status = - &incoming_status_array.status_list.data[i]; - EXPECT_TRUE(uuidcmp(outgoing_status->goal_info.uuid, incoming_status->goal_info.uuid)); - EXPECT_EQ(outgoing_status->goal_info.stamp.sec, incoming_status->goal_info.stamp.sec); - EXPECT_EQ(outgoing_status->goal_info.stamp.nanosec, incoming_status->goal_info.stamp.nanosec); - EXPECT_EQ(outgoing_status->status, incoming_status->status); - } - ret = rcl_action_goal_status_array_fini(&status_array); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; From d03d5605c12afd7c7059b23d008e77b0b8ce044f Mon Sep 17 00:00:00 2001 From: Alexis Pojomovsky Date: Tue, 27 Nov 2018 13:44:33 -0300 Subject: [PATCH 3/5] Addressed peer review comments --- .../rcl_action/test_action_communication.cpp | 523 +++++------------- 1 file changed, 133 insertions(+), 390 deletions(-) diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp index 7ac2c4b..0f982d7 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -57,6 +57,51 @@ protected: ret = rcl_action_client_init( &this->action_client, &this->node, ts, action_name, &client_options); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + this->wait_set_server = rcl_get_zero_initialized_wait_set(); + ret = rcl_action_server_wait_set_get_num_entities( + &this->action_server, + &this->num_subscriptions_server, + &this->num_guard_conditions_server, + &this->num_timers_server, + &this->num_clients_server, + &this->num_services_server); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &this->wait_set_server, + this->num_subscriptions_server, + this->num_guard_conditions_server, + this->num_timers_server, + this->num_clients_server, + this->num_services_server, + rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_action_wait_set_add_action_server(&this->wait_set_server, &this->action_server, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + this->wait_set_client = rcl_get_zero_initialized_wait_set(); + ret = rcl_action_client_wait_set_get_num_entities( + &this->action_client, + &this->num_subscriptions_client, + &this->num_guard_conditions_client, + &this->num_timers_client, + &this->num_clients_client, + &this->num_services_client); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_wait_set_init( + &this->wait_set_client, + this->num_subscriptions_client, + this->num_guard_conditions_client, + this->num_timers_client, + this->num_clients_client, + this->num_services_client, + rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_action_wait_set_add_action_client( + &this->wait_set_client, &this->action_client, NULL, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); } void TearDown() override @@ -72,6 +117,12 @@ protected: EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_shutdown(); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_fini(&this->wait_set_server); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_wait_set_fini(&this->wait_set_client); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); } void init_test_uuid0(uint8_t * uuid) @@ -92,6 +143,30 @@ protected: rcl_action_server_t action_server; rcl_node_t node; rcl_clock_t clock; + + rcl_wait_set_t wait_set_server; + size_t num_subscriptions_server; + size_t num_guard_conditions_server; + size_t num_timers_server; + size_t num_clients_server; + size_t num_services_server; + + rcl_wait_set_t wait_set_client; + size_t num_subscriptions_client; + size_t num_guard_conditions_client; + size_t num_timers_client; + size_t num_clients_client; + size_t num_services_client; + + bool is_goal_request_ready; + bool is_cancel_request_ready; + bool is_result_request_ready; + + bool is_feedback_ready; + bool is_status_ready; + bool is_goal_response_ready; + bool is_cancel_response_ready; + bool is_result_response_ready; }; // class TestActionCommunication TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_comm) @@ -116,59 +191,20 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c 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(); - - 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_server_wait_set_get_num_entities( - &this->action_server, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, NULL); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - bool is_goal_request_ready(false); - bool is_cancel_request_ready(false); - bool is_result_request_ready(false); - - ret = rcl_wait(&wait_set, 1000000); + ret = rcl_wait(&this->wait_set_server, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_server_wait_set_get_entities_ready( - &wait_set, + &this->wait_set_server, &this->action_server, - &is_goal_request_ready, - &is_cancel_request_ready, - &is_result_request_ready); + &this->is_goal_request_ready, + &this->is_cancel_request_ready, + &this->is_result_request_ready); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait_set_fini(&wait_set); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); + EXPECT_EQ(this->is_goal_request_ready, true) << rcl_get_error_string().str; // Take goal request with valid arguments rmw_request_id_t request_header; @@ -193,68 +229,23 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - wait_set = rcl_get_zero_initialized_wait_set(); - - num_subscriptions = 0; - num_guard_conditions = 0; - num_timers = 0; - num_clients = 0; - num_services = 0; - - ret = rcl_action_client_wait_set_get_num_entities( - &this->action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - + ret = rcl_wait(&this->wait_set_client, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_client( - &wait_set, &this->action_client, NULL, NULL); - - ret = rcl_wait(&wait_set, 100000000); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - 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); - ret = rcl_action_client_wait_set_get_entities_ready( - &wait_set, + &this->wait_set_client, &this->action_client, - &is_feedback_ready, - &is_status_ready, - &is_goal_response_ready, - &is_cancel_response_ready, - &is_result_response_ready); + &this->is_feedback_ready, + &this->is_status_ready, + &this->is_goal_response_ready, + &this->is_cancel_response_ready, + &this->is_result_response_ready); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait_set_fini(&wait_set); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take goal response with valid arguments - ret = rcl_action_take_goal_response( &this->action_client, &request_header, &incoming_goal_response); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -275,8 +266,7 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel_comm) -{ +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel_comm) { action_msgs__srv__CancelGoal_Request outgoing_cancel_request; action_msgs__srv__CancelGoal_Request incoming_cancel_request; action_msgs__srv__CancelGoal_Response outgoing_cancel_response; @@ -298,57 +288,16 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel 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(); - - 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_server_wait_set_get_num_entities( - &this->action_server, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, NULL); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - bool is_goal_request_ready(false); - bool is_cancel_request_ready(false); - bool is_result_request_ready(false); - - ret = rcl_wait(&wait_set, 1000000); + ret = rcl_wait(&this->wait_set_server, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_server_wait_set_get_entities_ready( - &wait_set, + &this->wait_set_server, &this->action_server, - &is_goal_request_ready, - &is_cancel_request_ready, - &is_result_request_ready); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_fini(&wait_set); + &this->is_goal_request_ready, + &this->is_cancel_request_ready, + &this->is_result_request_ready); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); @@ -387,63 +336,18 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - wait_set = rcl_get_zero_initialized_wait_set(); - - num_subscriptions = 0; - num_guard_conditions = 0; - num_timers = 0; - num_clients = 0; - num_services = 0; - - ret = rcl_action_client_wait_set_get_num_entities( - &this->action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - + ret = rcl_wait(&this->wait_set_client, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_client( - &wait_set, &this->action_client, NULL, NULL); - - ret = rcl_wait(&wait_set, 100000000); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - 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); - ret = rcl_action_client_wait_set_get_entities_ready( - &wait_set, + &this->wait_set_client, &this->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_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_fini(&wait_set); + &this->is_feedback_ready, + &this->is_status_ready, + &this->is_goal_response_ready, + &this->is_cancel_response_ready, + &this->is_result_response_ready); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); @@ -494,49 +398,12 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result 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(); - - 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_server_wait_set_get_num_entities( - &this->action_server, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_server(&wait_set, &this->action_server, NULL); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - bool is_goal_request_ready(false); - bool is_cancel_request_ready(false); - bool is_result_request_ready(false); - - ret = rcl_wait(&wait_set, 1000000); + ret = rcl_wait(&this->wait_set_server, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_server_wait_set_get_entities_ready( - &wait_set, + &this->wait_set_server, &this->action_server, &is_goal_request_ready, &is_cancel_request_ready, @@ -544,10 +411,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait_set_fini(&wait_set); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - // Take result request with valid arguments rmw_request_id_t request_header; ret = rcl_action_take_result_request( @@ -576,41 +439,18 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait(&this->wait_set_client, 1000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); - num_subscriptions = 0; - num_guard_conditions = 0; - num_timers = 0; - num_clients = 0; - num_services = 0; - - ret = rcl_action_client_wait_set_get_num_entities( + ret = rcl_action_client_wait_set_get_entities_ready( + &this->wait_set_client, &this->action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_client( - &wait_set, &this->action_client, NULL, NULL); - - ret = rcl_wait(&wait_set, 100000000); + &this->is_feedback_ready, + &this->is_status_ready, + &this->is_goal_response_ready, + &this->is_cancel_response_ready, + &this->is_result_response_ready); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); @@ -669,57 +509,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status rcl_ret_t ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - // Publish status with valid arguments (but empty array) - ret = rcl_action_publish_status(&this->action_server, &status_array.msg); - 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(); - - 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( - &this->action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_client( - &wait_set, &this->action_client, NULL, NULL); - - ret = rcl_wait(&wait_set, 100000000); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - // Take status with valid arguments (empty array) - ret = rcl_action_take_status(&this->action_client, &incoming_status_array); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_goal_status_array_fini(&status_array); - ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - // Add a goal before publishing the status array rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); rcl_action_goal_handle_t * goal_handle; @@ -735,41 +524,18 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait(&this->wait_set_client, 1000000); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); - num_subscriptions = 0; - num_guard_conditions = 0; - num_timers = 0; - num_clients = 0; - num_services = 0; - - ret = rcl_action_client_wait_set_get_num_entities( + ret = rcl_action_client_wait_set_get_entities_ready( + &this->wait_set_client, &this->action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_client( - &wait_set, &this->action_client, NULL, NULL); - - ret = rcl_wait(&wait_set, 100000000); + &this->is_feedback_ready, + &this->is_status_ready, + &this->is_goal_response_ready, + &this->is_cancel_response_ready, + &this->is_result_response_ready); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); @@ -817,41 +583,18 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedba 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(); + ret = rcl_wait(&this->wait_set_client, 1000000); + 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( + ret = rcl_action_client_wait_set_get_entities_ready( + &this->wait_set_client, &this->action_client, - &num_subscriptions, - &num_guard_conditions, - &num_timers, - &num_clients, - &num_services); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_wait_set_init( - &wait_set, - num_subscriptions, // number_of_subscriptions - num_guard_conditions, // number_of_guard_conditions - num_timers, // number_of_timers - num_clients, // number_of_clients - num_services, // number_of_services - rcl_get_default_allocator()); - - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - ret = rcl_action_wait_set_add_action_client( - &wait_set, &this->action_client, NULL, NULL); - - ret = rcl_wait(&wait_set, 100000000); + &this->is_feedback_ready, + &this->is_status_ready, + &this->is_goal_response_ready, + &this->is_cancel_response_ready, + &this->is_result_response_ready); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); From 10eebe3a3bce8cfdfbb0346b388be6d89197b216 Mon Sep 17 00:00:00 2001 From: Alexis Pojomovsky Date: Tue, 27 Nov 2018 14:25:56 -0300 Subject: [PATCH 4/5] Added checks for wait set entities --- .../rcl_action/test_action_communication.cpp | 53 +++++++++++-------- 1 file changed, 31 insertions(+), 22 deletions(-) diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp index 0f982d7..0870f15 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -301,6 +301,10 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_TRUE(this->is_cancel_request_ready); + EXPECT_FALSE(this->is_goal_request_ready); + EXPECT_FALSE(this->is_result_request_ready); + // Take cancel request with valid arguments rmw_request_id_t request_header; ret = rcl_action_take_cancel_request( @@ -351,6 +355,12 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_TRUE(this->is_cancel_response_ready); + EXPECT_FALSE(this->is_feedback_ready); + EXPECT_FALSE(this->is_status_ready); + EXPECT_FALSE(this->is_goal_response_ready); + EXPECT_FALSE(this->is_result_response_ready); + // Take cancel response with valid arguments ret = rcl_action_take_cancel_response( &this->action_client, &request_header, &incoming_cancel_response); @@ -411,6 +421,10 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_TRUE(this->is_result_request_ready); + EXPECT_FALSE(this->is_cancel_request_ready); + EXPECT_FALSE(this->is_goal_request_ready); + // Take result request with valid arguments rmw_request_id_t request_header; ret = rcl_action_take_result_request( @@ -454,28 +468,11 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - // Send result response with invalid action client - rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); - ret = rcl_action_send_result_response( - &invalid_action_server, &request_header, &outgoing_result_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; - rcl_reset_error(); - - // Send result response with null header - ret = rcl_action_send_result_response(&this->action_server, nullptr, &outgoing_result_response); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; - rcl_reset_error(); - - // Send result response with null message - ret = rcl_action_send_result_response(&this->action_server, &request_header, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; - rcl_reset_error(); - - // Send result response with valid arguments - ret = rcl_action_send_result_response( - &this->action_server, &request_header, &outgoing_result_response); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); + EXPECT_TRUE(this->is_result_response_ready); + EXPECT_FALSE(this->is_cancel_response_ready); + EXPECT_FALSE(this->is_feedback_ready); + EXPECT_FALSE(this->is_status_ready); + EXPECT_FALSE(this->is_goal_response_ready); // Take result response with valid arguments ret = rcl_action_take_result_response( @@ -539,6 +536,12 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_TRUE(this->is_status_ready); + EXPECT_FALSE(this->is_result_response_ready); + EXPECT_FALSE(this->is_cancel_response_ready); + EXPECT_FALSE(this->is_feedback_ready); + EXPECT_FALSE(this->is_goal_response_ready); + // Take status with valid arguments (one goal in array) ret = rcl_action_take_status(&this->action_client, &incoming_status_array); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; @@ -598,6 +601,12 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedba EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_TRUE(this->is_feedback_ready); + EXPECT_FALSE(this->is_status_ready); + EXPECT_FALSE(this->is_result_response_ready); + EXPECT_FALSE(this->is_cancel_response_ready); + EXPECT_FALSE(this->is_goal_response_ready); + // Take feedback with valid arguments ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; From aead12917a7e8369df503c1ed259a55db6434eae Mon Sep 17 00:00:00 2001 From: Alexis Pojomovsky Date: Tue, 27 Nov 2018 16:16:30 -0300 Subject: [PATCH 5/5] Addressed peer review comments v2 --- .../rcl_action/test_action_communication.cpp | 202 +++++++++++------- 1 file changed, 121 insertions(+), 81 deletions(-) diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp index 0870f15..3aae23c 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -58,50 +58,43 @@ protected: &this->action_client, &this->node, ts, action_name, &client_options); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - this->wait_set_server = rcl_get_zero_initialized_wait_set(); + size_t num_subscriptions_server; + size_t num_guard_conditions_server; + size_t num_timers_server; + size_t num_clients_server; + size_t num_services_server; + size_t num_subscriptions_client; + size_t num_guard_conditions_client; + size_t num_timers_client; + size_t num_clients_client; + size_t num_services_client; + + this->wait_set = rcl_get_zero_initialized_wait_set(); ret = rcl_action_server_wait_set_get_num_entities( &this->action_server, - &this->num_subscriptions_server, - &this->num_guard_conditions_server, - &this->num_timers_server, - &this->num_clients_server, - &this->num_services_server); + &num_subscriptions_server, + &num_guard_conditions_server, + &num_timers_server, + &num_clients_server, + &num_services_server); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - ret = rcl_wait_set_init( - &this->wait_set_server, - this->num_subscriptions_server, - this->num_guard_conditions_server, - this->num_timers_server, - this->num_clients_server, - this->num_services_server, - rcl_get_default_allocator()); - ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - ret = rcl_action_wait_set_add_action_server(&this->wait_set_server, &this->action_server, NULL); - ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - - this->wait_set_client = rcl_get_zero_initialized_wait_set(); ret = rcl_action_client_wait_set_get_num_entities( &this->action_client, - &this->num_subscriptions_client, - &this->num_guard_conditions_client, - &this->num_timers_client, - &this->num_clients_client, - &this->num_services_client); + &num_subscriptions_client, + &num_guard_conditions_client, + &num_timers_client, + &num_clients_client, + &num_services_client); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_wait_set_init( - &this->wait_set_client, - this->num_subscriptions_client, - this->num_guard_conditions_client, - this->num_timers_client, - this->num_clients_client, - this->num_services_client, + &this->wait_set, + num_subscriptions_server + num_subscriptions_client, + num_guard_conditions_server + num_guard_conditions_client, + num_timers_server + num_timers_client, + num_clients_server + num_clients_client, + num_services_server + num_services_client, rcl_get_default_allocator()); ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - ret = rcl_action_wait_set_add_action_client( - &this->wait_set_client, &this->action_client, NULL, NULL); - ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); } void TearDown() override @@ -117,10 +110,7 @@ protected: EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ret = rcl_shutdown(); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_fini(&this->wait_set_server); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - rcl_reset_error(); - ret = rcl_wait_set_fini(&this->wait_set_client); + ret = rcl_wait_set_fini(&this->wait_set); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); } @@ -144,19 +134,7 @@ protected: rcl_node_t node; rcl_clock_t clock; - rcl_wait_set_t wait_set_server; - size_t num_subscriptions_server; - size_t num_guard_conditions_server; - size_t num_timers_server; - size_t num_clients_server; - size_t num_services_server; - - rcl_wait_set_t wait_set_client; - size_t num_subscriptions_client; - size_t num_guard_conditions_client; - size_t num_timers_client; - size_t num_clients_client; - size_t num_services_client; + rcl_wait_set_t wait_set; bool is_goal_request_ready; bool is_cancel_request_ready; @@ -191,12 +169,16 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait(&this->wait_set_server, 1000000); + ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait(&this->wait_set, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_server_wait_set_get_entities_ready( - &this->wait_set_server, + &this->wait_set, &this->action_server, &this->is_goal_request_ready, &this->is_cancel_request_ready, @@ -204,7 +186,9 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - EXPECT_EQ(this->is_goal_request_ready, true) << rcl_get_error_string().str; + EXPECT_TRUE(this->is_goal_request_ready) << rcl_get_error_string().str; + EXPECT_FALSE(this->is_cancel_request_ready) << rcl_get_error_string().str; + EXPECT_FALSE(this->is_result_request_ready) << rcl_get_error_string().str; // Take goal request with valid arguments rmw_request_id_t request_header; @@ -229,12 +213,19 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait(&this->wait_set_client, 1000000); + ret = rcl_wait_set_clear(&this->wait_set); + + ret = rcl_action_wait_set_add_action_client( + &this->wait_set, &this->action_client, NULL, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait(&this->wait_set, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_client_wait_set_get_entities_ready( - &this->wait_set_client, + &this->wait_set, &this->action_client, &this->is_feedback_ready, &this->is_status_ready, @@ -245,6 +236,12 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_TRUE(this->is_goal_response_ready); + EXPECT_FALSE(this->is_cancel_response_ready); + EXPECT_FALSE(this->is_feedback_ready); + EXPECT_FALSE(this->is_status_ready); + EXPECT_FALSE(this->is_result_response_ready); + // Take goal response with valid arguments ret = rcl_action_take_goal_response( &this->action_client, &request_header, &incoming_goal_response); @@ -266,7 +263,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel_comm) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel_comm) +{ action_msgs__srv__CancelGoal_Request outgoing_cancel_request; action_msgs__srv__CancelGoal_Request incoming_cancel_request; action_msgs__srv__CancelGoal_Response outgoing_cancel_response; @@ -288,12 +286,16 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait(&this->wait_set_server, 1000000); + ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait(&this->wait_set, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_server_wait_set_get_entities_ready( - &this->wait_set_server, + &this->wait_set, &this->action_server, &this->is_goal_request_ready, &this->is_cancel_request_ready, @@ -340,12 +342,19 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait(&this->wait_set_client, 1000000); + ret = rcl_wait_set_clear(&this->wait_set); + + ret = rcl_action_wait_set_add_action_client( + &this->wait_set, &this->action_client, NULL, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait(&this->wait_set, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_client_wait_set_get_entities_ready( - &this->wait_set_client, + &this->wait_set, &this->action_client, &this->is_feedback_ready, &this->is_status_ready, @@ -408,16 +417,20 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait(&this->wait_set_server, 1000000); + ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait(&this->wait_set, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_server_wait_set_get_entities_ready( - &this->wait_set_server, + &this->wait_set, &this->action_server, - &is_goal_request_ready, - &is_cancel_request_ready, - &is_result_request_ready); + &this->is_goal_request_ready, + &this->is_cancel_request_ready, + &this->is_result_request_ready); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); @@ -453,12 +466,19 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait(&this->wait_set_client, 1000000); + ret = rcl_wait_set_clear(&this->wait_set); + + ret = rcl_action_wait_set_add_action_client( + &this->wait_set, &this->action_client, NULL, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait(&this->wait_set, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_client_wait_set_get_entities_ready( - &this->wait_set_client, + &this->wait_set, &this->action_client, &this->is_feedback_ready, &this->is_status_ready, @@ -521,12 +541,19 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait(&this->wait_set_client, 1000000); + ret = rcl_wait_set_clear(&this->wait_set); + + ret = rcl_action_wait_set_add_action_client( + &this->wait_set, &this->action_client, NULL, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait(&this->wait_set, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_client_wait_set_get_entities_ready( - &this->wait_set_client, + &this->wait_set, &this->action_client, &this->is_feedback_ready, &this->is_status_ready, @@ -586,12 +613,17 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedba EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); - ret = rcl_wait(&this->wait_set_client, 1000000); + ret = rcl_action_wait_set_add_action_client( + &this->wait_set, &this->action_client, NULL, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_wait(&this->wait_set, 1000000); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; rcl_reset_error(); ret = rcl_action_client_wait_set_get_entities_ready( - &this->wait_set_client, + &this->wait_set, &this->action_client, &this->is_feedback_ready, &this->is_status_ready, @@ -624,7 +656,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedba test_msgs__action__Fibonacci_Feedback__fini(&outgoing_feedback); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_request_opts) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_request_opts) +{ test_msgs__action__Fibonacci_Goal_Request outgoing_goal_request; test_msgs__action__Fibonacci_Goal_Request incoming_goal_request; test_msgs__action__Fibonacci_Goal_Request__init(&outgoing_goal_request); @@ -680,7 +713,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal test_msgs__action__Fibonacci_Goal_Request__init(&incoming_goal_request); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_response_opts) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_response_opts) +{ test_msgs__action__Fibonacci_Goal_Response outgoing_goal_response; test_msgs__action__Fibonacci_Goal_Response incoming_goal_response; test_msgs__action__Fibonacci_Goal_Response__init(&outgoing_goal_response); @@ -740,7 +774,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal test_msgs__action__Fibonacci_Goal_Response__fini(&outgoing_goal_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_cancel_request_opts) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_cancel_request_opts) +{ action_msgs__srv__CancelGoal_Request outgoing_cancel_request; action_msgs__srv__CancelGoal_Request incoming_cancel_request; action_msgs__srv__CancelGoal_Request__init(&outgoing_cancel_request); @@ -798,7 +833,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_canc } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_cancel_response_opts) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_cancel_response_opts) +{ action_msgs__srv__CancelGoal_Response outgoing_cancel_response; action_msgs__srv__CancelGoal_Response incoming_cancel_response; action_msgs__srv__CancelGoal_Response__init(&outgoing_cancel_response); @@ -859,7 +895,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_canc action_msgs__srv__CancelGoal_Response__fini(&outgoing_cancel_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_result_request_opts) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_result_request_opts) +{ test_msgs__action__Fibonacci_Result_Request outgoing_result_request; test_msgs__action__Fibonacci_Result_Request incoming_result_request; test_msgs__action__Fibonacci_Result_Request__init(&outgoing_result_request); @@ -914,7 +951,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_resu test_msgs__action__Fibonacci_Result_Request__fini(&outgoing_result_request); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_result_response_opts) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_result_response_opts) +{ test_msgs__action__Fibonacci_Result_Response outgoing_result_response; test_msgs__action__Fibonacci_Result_Response incoming_result_response; test_msgs__action__Fibonacci_Result_Response__init(&outgoing_result_response); @@ -975,7 +1013,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_resu test_msgs__action__Fibonacci_Result_Response__fini(&outgoing_result_response); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_feedback_opts) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_feedback_opts) +{ test_msgs__action__Fibonacci_Feedback outgoing_feedback; test_msgs__action__Fibonacci_Feedback incoming_feedback; test_msgs__action__Fibonacci_Feedback__init(&outgoing_feedback); @@ -1025,7 +1064,8 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_feed test_msgs__action__Fibonacci_Feedback__fini(&outgoing_feedback); } -TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_status_opts) { +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_status_opts) +{ action_msgs__msg__GoalStatusArray incoming_status_array; action_msgs__msg__GoalStatusArray__init(&incoming_status_array);