niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

[rcl action] Augments test_action_communication test.

Browse source
This commit is contained in:
Michel Hidalgo 2018-11-13 11:10:16 -03:00 • committed by Alexis Pojomovsky
parent f531f682ea
commit bde76ab40e
1 changed files with 479 additions and 146 deletions
Download patch file Download diff file Expand all files Collapse all files

625
rcl_action/test/rcl_action/test_action_communication.cpp
View file

@ -13,11 +13,14 @@
// limitations under the License. // limitations under the License.
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include "rcl_action/action_client.h"
#include "rcl_action/action_server.h" #include "rcl_action/action_server.h"
#include "rcl/error_handling.h" #include "rcl/error_handling.h"
#include "rcl/rcl.h" #include "rcl/rcl.h"
#include "rosidl_generator_c/primitives_sequence_functions.h"
#include "test_msgs/action/fibonacci.h" #include "test_msgs/action/fibonacci.h"
#ifdef RMW_IMPLEMENTATION #ifdef RMW_IMPLEMENTATION
@ -27,7 +30,6 @@
# define CLASSNAME(NAME, SUFFIX) NAME # define CLASSNAME(NAME, SUFFIX) NAME
#endif #endif
// TODO(jacobperron): Add action client to complete tests
class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing::Test class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing::Test
{ {
protected: protected:
@ -43,11 +45,16 @@ protected:
ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator); ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator);
const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT( const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(
test_msgs, Fibonacci); 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 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(); this->action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_server_init( 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; 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); rcl_ret_t ret = rcl_action_server_fini(&this->action_server, &this->node);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_clock_fini(&this->clock); 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); 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(); ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
} }
void init_test_uuid0(uint8_t * uuid) 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; uuid[i] = i;
} }
} }
void init_test_uuid1(uint8_t * uuid) 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; uuid[i] = 15 - i;
} }
} }
rcl_action_client_t action_client;
rcl_action_server_t action_server; rcl_action_server_t action_server;
rcl_node_t node; rcl_node_t node;
rcl_clock_t clock; rcl_clock_t clock;
}; // class TestActionCommunication }; // 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 outgoing_goal_request;
test_msgs__action__Fibonacci_Goal_Request__init(&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 // Initialize goal request
rmw_request_id_t request_header; init_test_uuid0(outgoing_goal_request.uuid);
rcl_ret_t ret = rcl_action_take_goal_request(nullptr, &request_header, &goal_request); outgoing_goal_request.order = 10;
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
// 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(); rcl_reset_error();
// Take request with null header // Send goal request with invalid action client
ret = rcl_action_take_goal_request(&this->action_server, nullptr, &goal_request); 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); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error(); 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); 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(); 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(); 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); ret = rcl_action_take_goal_request(&invalid_action_server, &incoming_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(); rcl_reset_error();
// Take with valid arguments // Take goal request with valid arguments
// TODO(jacobperron): Send a request from a client ret = rcl_action_take_goal_request(&this->action_server, &request_header, &incoming_goal_request);
// ret = rcl_action_take_goal_request(&this->action_server, &request_header, &goal_request); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// EXPECT_EQ(ret, RCL_RET_OK); 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 outgoing_goal_response;
test_msgs__action__Fibonacci_Goal_Response__init(&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; rmw_request_id_t response_header;
rcl_ret_t ret = rcl_action_send_goal_response(nullptr, &response_header, &goal_response); rcl_ret_t ret = rcl_action_send_goal_response(nullptr, &response_header, &outgoing_goal_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error(); rcl_reset_error();
// Send response with null header // Send goal response with null header
ret = rcl_action_send_goal_response(&this->action_server, nullptr, &goal_response); ret = rcl_action_send_goal_response(&this->action_server, nullptr, &outgoing_goal_response);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error(); 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); 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); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error(); rcl_reset_error();
// Send response with invalid action server // Take goal response with null message
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); ret = rcl_action_take_goal_response(&this->action_client, &response_header, nullptr);
ret = rcl_action_send_goal_response(&invalid_action_server, &response_header, &goal_response); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error(); rcl_reset_error();
// Send with valid arguments // Take goal response with invalid action client
// TODO(jacobperron): Check with client on receiving end rcl_action_client_t invalid_action_client = rcl_action_get_zero_initialized_client();
ret = rcl_action_send_goal_response(&this->action_server, &response_header, &goal_response); ret = rcl_action_take_goal_response(&invalid_action_client, &incoming_goal_response);
EXPECT_EQ(ret, RCL_RET_OK); 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 outgoing_cancel_request;
action_msgs__srv__CancelGoal_Request__init(&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 // Initialize cancel request
rmw_request_id_t request_header; init_test_uuid0(outgoing_cancel_request.goal_info.uuid);
rcl_ret_t ret = rcl_action_take_cancel_request(nullptr, &request_header, &cancel_request); outgoing_cancel_request.goal_info.stamp.sec = 321;
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); 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(); rcl_reset_error();
// Take request with null header // Send cancel request with invalid action client
ret = rcl_action_take_cancel_request(&this->action_server, nullptr, &cancel_request); 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); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error(); 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); 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(); 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(); 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); ret = rcl_action_take_cancel_request(&invalid_action_server, &request_header, &incoming_cancel_request);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error(); rcl_reset_error();
// Take with valid arguments // Take cancel request with valid arguments
// TODO(jacobperron): Send a request from a client ret = rcl_action_take_cancel_request(&this->action_server, &request_header, &incoming_cancel_request);
// ret = rcl_action_take_cancel_request(&this->action_server, &request_header, &cancel_request); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// EXPECT_EQ(ret, RCL_RET_OK); 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 outgoing_cancel_response;
action_msgs__srv__CancelGoal_Response__init(&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; rmw_request_id_t response_header;
rcl_ret_t ret = rcl_action_send_cancel_response(nullptr, &response_header, &cancel_response); rcl_ret_t ret = rcl_action_send_cancel_response(nullptr, &response_header, &outgoing_cancel_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); ASSERT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error(); rcl_reset_error();
// Send response with null header // Send cancel response with invalid action server
ret = rcl_action_send_cancel_response(&this->action_server, nullptr, &cancel_response); rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); 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(); 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); 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(); rcl_reset_error();
// Send response with invalid action server // Send cancel response with valid arguments
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); ret = rcl_action_send_cancel_response(&this->action_server, &response_header, &outgoing_cancel_response);
ret = rcl_action_send_cancel_response(&invalid_action_server, &response_header, &cancel_response); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error(); rcl_reset_error();
// Send with valid arguments // Take cancel response with null action client
// TODO(jacobperron): Check with client on receiving end ret = rcl_action_take_cancel_response(nullptr, &incoming_cancel_response);
ret = rcl_action_send_cancel_response(&this->action_server, &response_header, &cancel_response); EXPECT_EQ(ret, RCL_RET_ACTION_CLIENT_INVALID) << rcl_get_error_string().str;
EXPECT_EQ(ret, RCL_RET_OK); 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 outgoing_result_request;
test_msgs__action__Fibonacci_Result_Request__init(&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; rmw_request_id_t request_header;
rcl_ret_t ret = rcl_action_take_result_request(nullptr, &request_header, &result_request); ret = rcl_action_take_result_request(nullptr, &request_header, &incoming_result_request);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error(); rcl_reset_error();
// Take request with null header // Take result request with invalid action server
ret = rcl_action_take_result_request(&this->action_server, nullptr, &result_request); rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); 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(); 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); 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(); rcl_reset_error();
// Take request with invalid action server // Take result request with valid arguments
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); ret = rcl_action_take_result_request(&this->action_server, &request_header, &incoming_result_request);
ret = rcl_action_take_result_request(&invalid_action_server, &request_header, &result_request); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error(); rcl_reset_error();
// Take with valid arguments // Check that the result request was received correctly
// TODO(jacobperron): Send a request from a client EXPECT_TRUE(uuidcmp(
// ret = rcl_action_take_result_request(&this->action_server, &request_header, &result_request); outgoing_result_request.uuid,
// EXPECT_EQ(ret, RCL_RET_OK); 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 outgoing_result_response;
test_msgs__action__Fibonacci_Result_Response__init(&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; rmw_request_id_t response_header;
rcl_ret_t ret = rcl_action_send_result_response(nullptr, &response_header, &result_response); rcl_ret_t ret = rcl_action_send_result_response(nullptr, &response_header, &outgoing_result_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); ASSERT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error(); rcl_reset_error();
// Send response with null header // Send result response with invalid action client
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
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_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); ret = rcl_action_send_result_response(&invalid_action_server, &response_header, &outgoing_result_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error(); rcl_reset_error();
// Send with valid arguments // Send result response with null header
// TODO(jacobperron): Check with client on receiving end ret = rcl_action_send_result_response(&this->action_server, nullptr, &outgoing_result_response);
ret = rcl_action_send_result_response(&this->action_server, &response_header, &result_response); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
EXPECT_EQ(ret, RCL_RET_OK); 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 outgoing_feedback;
test_msgs__action__Fibonacci_Feedback__init(&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 // Publish feedback with null action server
rcl_ret_t ret = rcl_action_publish_feedback(nullptr, &feedback); rcl_ret_t ret = rcl_action_publish_feedback(nullptr, &outgoing_feedback);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error(); rcl_reset_error();
// Publish feedback with null message // Publish feedback with null message
ret = rcl_action_publish_feedback(&this->action_server, nullptr); 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(); rcl_reset_error();
// Publish feedback with invalid action server // Publish feedback with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server(); rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_publish_feedback(&invalid_action_server, &feedback); ret = rcl_action_publish_feedback(&invalid_action_server, &outgoing_feedback);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID); EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error(); rcl_reset_error();
// Publish feedback with valid arguments // Publish feedback with valid arguments
// TODO(jacobperron): Check with client on receiving end ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback);
ret = rcl_action_publish_feedback(&this->action_server, &feedback); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_EQ(ret, RCL_RET_OK); 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_goal_status_array_t status_array =
rcl_action_get_zero_initialized_goal_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); 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 // Publish status with null action server
ret = rcl_action_publish_status(nullptr, &status_array.msg); 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(); rcl_reset_error();
// Publish status with null message // Publish status with null message
ret = rcl_action_publish_status(&this->action_server, nullptr); 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(); rcl_reset_error();
// Publish status with invalid action server // Publish status with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_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); 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(); rcl_reset_error();
// Publish status with valid arguments (but empty array) // 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); ret = rcl_action_publish_status(&this->action_server, &status_array.msg);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; 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); ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; 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_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
rcl_action_goal_handle_t * goal_handle; rcl_action_goal_handle_t * goal_handle;
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info); goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info);
ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str; ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
ret = rcl_action_get_goal_status_array(&this->action_server, &status_array); ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
// Publish status with valid arguments (one goal in array) // 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); ret = rcl_action_publish_status(&this->action_server, &status_array.msg);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; 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); ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
action_msgs__msg__GoalStatusArray__fini(&incoming_status_array);
} }