diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index f8e924d89..ad004c32b 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 f6bcf1fd8..3aae23c0a 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -13,11 +13,15 @@ // limitations under the License. #include +#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" +#include "rosidl_generator_c/primitives_sequence_functions.h" + #include "test_msgs/action/fibonacci.h" #ifdef RMW_IMPLEMENTATION @@ -27,7 +31,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 +46,54 @@ class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing 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; + + 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, + &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_action_client_wait_set_get_num_entities( + &this->action_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, + 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; } @@ -57,273 +103,974 @@ class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing 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; + ret = rcl_wait_set_fini(&this->wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + rcl_reset_error(); } 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; + + rcl_wait_set_t wait_set; + + 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_take_goal_request) +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_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_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(); + + 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(); - // Take request with null action server + 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, + &this->action_server, + &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(); + + 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; - rcl_ret_t ret = rcl_action_take_goal_request(nullptr, &request_header, &goal_request); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + 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(); + + 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, + &this->action_client, + &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(); + + 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); + 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(); + + 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, + &this->action_server, + &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(); - // Take request with null header - ret = rcl_action_take_goal_request(&this->action_server, nullptr, &goal_request); + 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( + &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(); + + 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, + &this->action_client, + &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(); + + 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); + 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(); + + 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, + &this->action_server, + &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(); + + 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( + &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(); + + 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, + &this->action_client, + &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(); + + 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( + &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; + + // 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(); + + 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, + &this->action_client, + &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(); + + 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; + 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(); + + 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, + &this->action_client, + &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(); + + 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; + 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); + test_msgs__action__Fibonacci_Goal_Request__init(&incoming_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 + 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, &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, &sequence_number); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << 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, &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); - - test_msgs__action__Fibonacci_Goal_Request__fini(&goal_request); + 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_invalid_goal_response_opts) { - 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); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; rcl_reset_error(); - // Send response with invalid action server + // 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, &response_header, &goal_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + 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(); + + // 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 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 null message + ret = rcl_action_take_goal_response(&this->action_client, &response_header, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + // 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, &response_header, &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); + 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_invalid_cancel_request_opts) { - 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); + + // 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 + 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, &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, &sequence_number); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); - // Take request with null action server + // Take cancel 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); + 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 request with null header - ret = rcl_action_take_cancel_request(&this->action_server, nullptr, &cancel_request); + // 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); - - action_msgs__srv__CancelGoal_Request__fini(&cancel_request); + 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) -{ - action_msgs__srv__CancelGoal_Response cancel_response; - action_msgs__srv__CancelGoal_Response__init(&cancel_response); - // Send response with null action server +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); + action_msgs__srv__CancelGoal_Response__init(&incoming_cancel_response); + + // 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 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 response with null message + // 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); + // Take cancel response with null action client + 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(); - // 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 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, &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, &response_header, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); - action_msgs__srv__CancelGoal_Response__fini(&cancel_response); + 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_invalid_result_request_opts) { - 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); + + // Initialize result request + init_test_uuid0(outgoing_result_request.uuid); + + // Send result request with null action client + 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(); - // Take 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); + // 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, &sequence_number); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_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); + // Send result request with null message + 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(); - // Take request with null message - ret = rcl_action_take_result_request(&this->action_server, &request_header, nullptr); - EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + // 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); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Take request with invalid action server + // 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, &result_request); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + 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 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); + // 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(); - test_msgs__action__Fibonacci_Result_Request__fini(&result_request); + // 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) << rcl_get_error_string().str; + rcl_reset_error(); + + 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_invalid_result_response_opts) { - test_msgs__action__Fibonacci_Result_Response result_response; - test_msgs__action__Fibonacci_Result_Response__init(&result_response); - - // Send response with null action server + 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); + + // 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); + // 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); + EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str; rcl_reset_error(); - // Send response with null message + // 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, &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_result_response(&invalid_action_server, &response_header, &result_response); - EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); + // Take result response with null action client + 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(); - // 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); + // Take result response with null message + 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(); - test_msgs__action__Fibonacci_Result_Response__fini(&result_response); + // 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, &response_header, &incoming_result_response); + EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str; + rcl_reset_error(); + + 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_invalid_feedback_opts) { - 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); + // 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(); - test_msgs__action__Fibonacci_Feedback__fini(&feedback); + 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_invalid_status_opts) { + 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 +1078,38 @@ 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; + // 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(); - // 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; - 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; - // 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; + // 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(); 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); }