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

Adds action interaction tests (#352)

Browse source 
* Add action_interaction_tests

* Fixed tests to work with new uuid

* Added comments for each test

* Addressed peer review comments

* Removed unnecessary rcl_reset_error calls from test_action_communication

* Addressed second part of peer review
This commit is contained in:
Alexis Pojomovsky 2018-12-06 19:09:03 -03:00 • committed by Shane Loretz
parent a30ccdf894
commit c5798e4774
3 changed files with 810 additions and 56 deletions
Download patch file Download diff file Expand all files Collapse all files

18
rcl_action/CMakeLists.txt
View file

@ -106,7 +106,7 @@ if(BUILD_TESTING)
function(custom_test_c target) function(custom_test_c target)
ament_add_gtest( ament_add_gtest(
"test_action_communication_${target_suffix}" ${ARGN} "${target}${target_suffix}" ${ARGN}
TIMEOUT 60 TIMEOUT 60
APPEND_LIBRARY_DIRS "${append_library_dirs}" APPEND_LIBRARY_DIRS "${append_library_dirs}"
ENV ENV
@ -115,24 +115,22 @@ if(BUILD_TESTING)
ROS_SECURITY_ROOT_DIRECTORY="${VALID_ROS_SECURITY_ROOT_DIRECTORY}" ROS_SECURITY_ROOT_DIRECTORY="${VALID_ROS_SECURITY_ROOT_DIRECTORY}"
PATH="${TEST_PATH}" PATH="${TEST_PATH}"
) )
if(TARGET test_action_communication_${target_suffix}) if(TARGET ${target}${target_suffix})
target_compile_definitions(test_action_communication_${target_suffix} target_compile_definitions(${target}${target_suffix}
PUBLIC "RMW_IMPLEMENTATION=${rmw_implementation}") PUBLIC "RMW_IMPLEMENTATION=${rmw_implementation}")
target_include_directories(test_action_communication_${target_suffix} PUBLIC target_include_directories(${target}${target_suffix} PUBLIC
include include
${rcl_INCLUDE_DIRS} ${rcl_INCLUDE_DIRS}
) )
target_link_libraries(test_action_communication_${target_suffix} target_link_libraries(${target}${target_suffix}
${PROJECT_NAME} ${PROJECT_NAME}
) )
ament_target_dependencies(test_action_communication_${target_suffix} ament_target_dependencies(${target}${target_suffix}
"test_msgs") "test_msgs")
endif() endif()
endfunction() endfunction()
macro(targets) macro(targets)
set(VALID_ROS_SECURITY_ROOT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/test/test_security_files")
set(ENV_PATH "$ENV{PATH}") set(ENV_PATH "$ENV{PATH}")
file(TO_CMAKE_PATH "${ENV_PATH}" ENV_PATH) file(TO_CMAKE_PATH "${ENV_PATH}" ENV_PATH)
set(TEST_PATH "${ENV_PATH}") set(TEST_PATH "${ENV_PATH}")
@ -146,8 +144,10 @@ if(BUILD_TESTING)
endif() endif()
endif() endif()
endif() endif()
custom_test_c(rcl_action custom_test_c(test_action_communication
"test/rcl_action/test_action_communication.cpp") "test/rcl_action/test_action_communication.cpp")
custom_test_c(test_action_interaction
"test/rcl_action/test_action_interaction.cpp")
endmacro() endmacro()
call_for_each_rmw_implementation(targets) call_for_each_rmw_implementation(targets)

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

@ -117,7 +117,6 @@ protected:
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_shutdown(&context); ret = rcl_shutdown(&context);
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();
} }
void init_test_uuid0(uint8_t * uuid) void init_test_uuid0(uint8_t * uuid)
@ -174,16 +173,13 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c
rcl_ret_t ret = rcl_action_send_goal_request( rcl_ret_t ret = rcl_action_send_goal_request(
&this->action_client, &outgoing_goal_request, &sequence_number); &this->action_client, &outgoing_goal_request, &sequence_number);
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();
ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); 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; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
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();
ret = rcl_action_server_wait_set_get_entities_ready( ret = rcl_action_server_wait_set_get_entities_ready(
&this->wait_set, &this->wait_set,
@ -193,7 +189,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c
&this->is_result_request_ready, &this->is_result_request_ready,
&this->is_goal_expired); &this->is_goal_expired);
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();
EXPECT_TRUE(this->is_goal_request_ready) << rcl_get_error_string().str; EXPECT_TRUE(this->is_goal_request_ready) << rcl_get_error_string().str;
EXPECT_FALSE(this->is_cancel_request_ready) << rcl_get_error_string().str; EXPECT_FALSE(this->is_cancel_request_ready) << rcl_get_error_string().str;
@ -203,13 +198,10 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c
rmw_request_id_t request_header; rmw_request_id_t request_header;
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, &incoming_goal_request);
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();
// Check that the goal request was received correctly // Check that the goal request was received correctly
EXPECT_EQ(outgoing_goal_request.order, incoming_goal_request.order); EXPECT_EQ(outgoing_goal_request.order, incoming_goal_request.order);
rcl_reset_error();
EXPECT_TRUE(uuidcmp(outgoing_goal_request.uuid, incoming_goal_request.uuid)); EXPECT_TRUE(uuidcmp(outgoing_goal_request.uuid, incoming_goal_request.uuid));
rcl_reset_error();
// Initialize goal response // Initialize goal response
outgoing_goal_response.accepted = true; outgoing_goal_response.accepted = true;
@ -220,18 +212,15 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c
ret = rcl_action_send_goal_response( ret = rcl_action_send_goal_response(
&this->action_server, &request_header, &outgoing_goal_response); &this->action_server, &request_header, &outgoing_goal_response);
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();
ret = rcl_wait_set_clear(&this->wait_set); ret = rcl_wait_set_clear(&this->wait_set);
ret = rcl_action_wait_set_add_action_client( ret = rcl_action_wait_set_add_action_client(
&this->wait_set, &this->action_client, NULL, NULL); &this->wait_set, &this->action_client, NULL, NULL);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
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();
ret = rcl_action_client_wait_set_get_entities_ready( ret = rcl_action_client_wait_set_get_entities_ready(
&this->wait_set, &this->wait_set,
@ -243,7 +232,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c
&this->is_result_response_ready); &this->is_result_response_ready);
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();
EXPECT_TRUE(this->is_goal_response_ready); EXPECT_TRUE(this->is_goal_response_ready);
EXPECT_FALSE(this->is_cancel_response_ready); EXPECT_FALSE(this->is_cancel_response_ready);
@ -255,15 +243,11 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_goal_c
ret = rcl_action_take_goal_response( ret = rcl_action_take_goal_response(
&this->action_client, &request_header, &incoming_goal_response); &this->action_client, &request_header, &incoming_goal_response);
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();
// Check that the goal response was received correctly // Check that the goal response was received correctly
EXPECT_EQ(outgoing_goal_response.accepted, incoming_goal_response.accepted); 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); 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); 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(&outgoing_goal_request);
test_msgs__action__Fibonacci_Goal_Request__fini(&incoming_goal_request); test_msgs__action__Fibonacci_Goal_Request__fini(&incoming_goal_request);
@ -293,15 +277,12 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel
rcl_ret_t ret = rcl_action_send_cancel_request( rcl_ret_t ret = rcl_action_send_cancel_request(
&this->action_client, &outgoing_cancel_request, &sequence_number); &this->action_client, &outgoing_cancel_request, &sequence_number);
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();
ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); 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; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
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();
ret = rcl_action_server_wait_set_get_entities_ready( ret = rcl_action_server_wait_set_get_entities_ready(
&this->wait_set, &this->wait_set,
@ -311,7 +292,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel
&this->is_result_request_ready, &this->is_result_request_ready,
&this->is_goal_expired); &this->is_goal_expired);
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();
EXPECT_TRUE(this->is_cancel_request_ready); EXPECT_TRUE(this->is_cancel_request_ready);
EXPECT_FALSE(this->is_goal_request_ready); EXPECT_FALSE(this->is_goal_request_ready);
@ -322,7 +302,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel
ret = rcl_action_take_cancel_request( ret = rcl_action_take_cancel_request(
&this->action_server, &request_header, &incoming_cancel_request); &this->action_server, &request_header, &incoming_cancel_request);
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();
// Check that the cancel request was received correctly // Check that the cancel request was received correctly
EXPECT_TRUE(uuidcmp( EXPECT_TRUE(uuidcmp(
@ -350,18 +329,15 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel
ret = rcl_action_send_cancel_response( ret = rcl_action_send_cancel_response(
&this->action_server, &request_header, &outgoing_cancel_response); &this->action_server, &request_header, &outgoing_cancel_response);
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();
ret = rcl_wait_set_clear(&this->wait_set); ret = rcl_wait_set_clear(&this->wait_set);
ret = rcl_action_wait_set_add_action_client( ret = rcl_action_wait_set_add_action_client(
&this->wait_set, &this->action_client, NULL, NULL); &this->wait_set, &this->action_client, NULL, NULL);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
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();
ret = rcl_action_client_wait_set_get_entities_ready( ret = rcl_action_client_wait_set_get_entities_ready(
&this->wait_set, &this->wait_set,
@ -372,7 +348,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel
&this->is_cancel_response_ready, &this->is_cancel_response_ready,
&this->is_result_response_ready); &this->is_result_response_ready);
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();
EXPECT_TRUE(this->is_cancel_response_ready); EXPECT_TRUE(this->is_cancel_response_ready);
EXPECT_FALSE(this->is_feedback_ready); EXPECT_FALSE(this->is_feedback_ready);
@ -384,7 +359,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_cancel
ret = rcl_action_take_cancel_response( ret = rcl_action_take_cancel_response(
&this->action_client, &request_header, &incoming_cancel_response); &this->action_client, &request_header, &incoming_cancel_response);
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();
// Check that the cancel response was received correctly // Check that the cancel response was received correctly
ASSERT_EQ( ASSERT_EQ(
@ -425,15 +399,12 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result
rcl_ret_t ret = rcl_action_send_result_request( rcl_ret_t ret = rcl_action_send_result_request(
&this->action_client, &outgoing_result_request, &sequence_number); &this->action_client, &outgoing_result_request, &sequence_number);
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();
ret = rcl_action_wait_set_add_action_server(&this->wait_set, &this->action_server, NULL); 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; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
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();
ret = rcl_action_server_wait_set_get_entities_ready( ret = rcl_action_server_wait_set_get_entities_ready(
&this->wait_set, &this->wait_set,
@ -443,7 +414,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result
&this->is_result_request_ready, &this->is_result_request_ready,
&this->is_goal_expired); &this->is_goal_expired);
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();
EXPECT_TRUE(this->is_result_request_ready); EXPECT_TRUE(this->is_result_request_ready);
EXPECT_FALSE(this->is_cancel_request_ready); EXPECT_FALSE(this->is_cancel_request_ready);
@ -454,7 +424,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result
ret = rcl_action_take_result_request( ret = rcl_action_take_result_request(
&this->action_server, &request_header, &incoming_result_request); &this->action_server, &request_header, &incoming_result_request);
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();
// Check that the result request was received correctly // Check that the result request was received correctly
EXPECT_TRUE(uuidcmp( EXPECT_TRUE(uuidcmp(
@ -475,18 +444,15 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result
ret = rcl_action_send_result_response( ret = rcl_action_send_result_response(
&this->action_server, &request_header, &outgoing_result_response); &this->action_server, &request_header, &outgoing_result_response);
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();
ret = rcl_wait_set_clear(&this->wait_set); ret = rcl_wait_set_clear(&this->wait_set);
ret = rcl_action_wait_set_add_action_client( ret = rcl_action_wait_set_add_action_client(
&this->wait_set, &this->action_client, NULL, NULL); &this->wait_set, &this->action_client, NULL, NULL);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
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();
ret = rcl_action_client_wait_set_get_entities_ready( ret = rcl_action_client_wait_set_get_entities_ready(
&this->wait_set, &this->wait_set,
@ -497,7 +463,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result
&this->is_cancel_response_ready, &this->is_cancel_response_ready,
&this->is_result_response_ready); &this->is_result_response_ready);
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();
EXPECT_TRUE(this->is_result_response_ready); EXPECT_TRUE(this->is_result_response_ready);
EXPECT_FALSE(this->is_cancel_response_ready); EXPECT_FALSE(this->is_cancel_response_ready);
@ -509,7 +474,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_result
ret = rcl_action_take_result_response( ret = rcl_action_take_result_response(
&this->action_client, &request_header, &incoming_result_response); &this->action_client, &request_header, &incoming_result_response);
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();
// Check that the result response was received correctly // Check that the result response was received correctly
EXPECT_EQ(outgoing_result_response.status, incoming_result_response.status); EXPECT_EQ(outgoing_result_response.status, incoming_result_response.status);
@ -545,23 +509,19 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status
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)
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();
ret = rcl_wait_set_clear(&this->wait_set); ret = rcl_wait_set_clear(&this->wait_set);
ret = rcl_action_wait_set_add_action_client( ret = rcl_action_wait_set_add_action_client(
&this->wait_set, &this->action_client, NULL, NULL); &this->wait_set, &this->action_client, NULL, NULL);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
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();
ret = rcl_action_client_wait_set_get_entities_ready( ret = rcl_action_client_wait_set_get_entities_ready(
&this->wait_set, &this->wait_set,
@ -572,7 +532,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status
&this->is_cancel_response_ready, &this->is_cancel_response_ready,
&this->is_result_response_ready); &this->is_result_response_ready);
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();
EXPECT_TRUE(this->is_status_ready); EXPECT_TRUE(this->is_status_ready);
EXPECT_FALSE(this->is_result_response_ready); EXPECT_FALSE(this->is_result_response_ready);
@ -583,7 +542,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_status
// Take status with valid arguments (one goal in array) // Take status with valid arguments (one goal in array)
ret = rcl_action_take_status(&this->action_client, &incoming_status_array); ret = rcl_action_take_status(&this->action_client, &incoming_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();
// Check that status was received correctly // Check that status was received correctly
ASSERT_EQ(status_array.msg.status_list.size, incoming_status_array.status_list.size); ASSERT_EQ(status_array.msg.status_list.size, incoming_status_array.status_list.size);
@ -624,16 +582,13 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedba
// Publish feedback with valid arguments // Publish feedback with valid arguments
rcl_ret_t ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback); rcl_ret_t ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback);
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();
ret = rcl_action_wait_set_add_action_client( ret = rcl_action_wait_set_add_action_client(
&this->wait_set, &this->action_client, NULL, NULL); &this->wait_set, &this->action_client, NULL, NULL);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_reset_error();
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10));
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();
ret = rcl_action_client_wait_set_get_entities_ready( ret = rcl_action_client_wait_set_get_entities_ready(
&this->wait_set, &this->wait_set,
@ -644,7 +599,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedba
&this->is_cancel_response_ready, &this->is_cancel_response_ready,
&this->is_result_response_ready); &this->is_result_response_ready);
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();
EXPECT_TRUE(this->is_feedback_ready); EXPECT_TRUE(this->is_feedback_ready);
EXPECT_FALSE(this->is_status_ready); EXPECT_FALSE(this->is_status_ready);
@ -655,7 +609,6 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedba
// Take feedback with valid arguments // Take feedback with valid arguments
ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback); ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback);
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();
// Check that feedback was received correctly // Check that feedback was received correctly
EXPECT_TRUE(uuidcmp(outgoing_feedback.uuid, incoming_feedback.uuid)); EXPECT_TRUE(uuidcmp(outgoing_feedback.uuid, incoming_feedback.uuid));

801
rcl_action/test/rcl_action/test_action_interaction.cpp Normal file
View file

@ -0,0 +1,801 @@
// Copyright 2018 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#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
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
#else
# define CLASSNAME(NAME, SUFFIX) NAME
#endif
class CLASSNAME (TestActionClientServerInteraction, RMW_IMPLEMENTATION) : public ::testing::Test
{
protected:
void SetUp() override
{
test_msgs__action__Fibonacci_Goal_Request__init(&this->outgoing_goal_request);
test_msgs__action__Fibonacci_Goal_Request__init(&this->incoming_goal_request);
test_msgs__action__Fibonacci_Goal_Response__init(&this->outgoing_goal_response);
test_msgs__action__Fibonacci_Goal_Response__init(&this->incoming_goal_response);
test_msgs__action__Fibonacci_Result_Request__init(&this->outgoing_result_request);
test_msgs__action__Fibonacci_Result_Request__init(&this->incoming_result_request);
test_msgs__action__Fibonacci_Result_Response__init(&this->outgoing_result_response);
test_msgs__action__Fibonacci_Result_Response__init(&this->incoming_result_response);
test_msgs__action__Fibonacci_Feedback__init(&this->outgoing_feedback);
test_msgs__action__Fibonacci_Feedback__init(&this->incoming_feedback);
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_ret_t ret;
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
ret = rcl_init_options_init(&init_options, allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
context = rcl_get_zero_initialized_context();
ret = rcl_init(0, nullptr, &init_options, &context);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
this->node = rcl_get_zero_initialized_node();
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(&this->node, "test_action_communication_node", "", &context, &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
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 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, &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;
}
void TearDown() override
{
// Finalize
test_msgs__action__Fibonacci_Goal_Request__fini(&this->outgoing_goal_request);
test_msgs__action__Fibonacci_Goal_Request__fini(&this->incoming_goal_request);
test_msgs__action__Fibonacci_Goal_Response__fini(&this->incoming_goal_response);
test_msgs__action__Fibonacci_Goal_Response__fini(&this->outgoing_goal_response);
test_msgs__action__Fibonacci_Result_Request__fini(&this->incoming_result_request);
test_msgs__action__Fibonacci_Result_Request__fini(&this->outgoing_result_request);
test_msgs__action__Fibonacci_Result_Response__fini(&this->incoming_result_response);
test_msgs__action__Fibonacci_Result_Response__fini(&this->outgoing_result_response);
test_msgs__action__Fibonacci_Feedback__fini(&this->outgoing_feedback);
test_msgs__action__Fibonacci_Feedback__fini(&this->incoming_feedback);
rcl_ret_t ret = rcl_action_server_fini(&this->action_server, &this->node);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_clock_fini(&this->clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_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) << rcl_get_error_string().str;
ret = rcl_wait_set_fini(&this->wait_set);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_shutdown(&context);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
}
void init_test_uuid0(uint8_t * uuid)
{
for (uint8_t i = 0; i < UUID_SIZE; ++i) {
uuid[i] = i;
}
}
void init_test_uuid1(uint8_t * uuid)
{
for (uint8_t i = 0; i < UUID_SIZE; ++i) {
uuid[i] = 15 - i;
}
}
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_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_Feedback outgoing_feedback;
test_msgs__action__Fibonacci_Feedback incoming_feedback;
rcl_action_client_t action_client;
rcl_action_server_t action_server;
rcl_node_t node;
rcl_clock_t clock;
rcl_context_t context;
rcl_wait_set_t wait_set;
bool is_goal_request_ready;
bool is_cancel_request_ready;
bool is_result_request_ready;
bool is_goal_expired;
bool is_feedback_ready;
bool is_status_ready;
bool is_goal_response_ready;
bool is_cancel_response_ready;
bool is_result_response_ready;
}; // class TestActionClientServerInteraction
// Exercises the "Example 1" sequence diagram found in the actions_proposal document.
// In this example, the action client request a goal and gets a response from
// the server accepting the goal (synchronous). Upon accepting the goal, the
// action server starts the required tasks for completing the goal.
// Following the goal request, the client makes an asynchronous request for the
// result. The feedback is published to the action client as it executes the goal.
// Ultimately, a result message is populated which is then used as part of the result response.
TEST_F(CLASSNAME(TestActionClientServerInteraction, RMW_IMPLEMENTATION), test_interaction)
{
// Initialize goal request
init_test_uuid0(this->outgoing_goal_request.uuid);
this->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, &this->outgoing_goal_request, &sequence_number);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_wait_set_clear(&this->wait_set);
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;
// Wait for goal request to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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,
&this->is_goal_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_TRUE(this->is_goal_request_ready) << rcl_get_error_string().str;
EXPECT_FALSE(this->is_cancel_request_ready) << rcl_get_error_string().str;
EXPECT_FALSE(this->is_result_request_ready) << rcl_get_error_string().str;
// Take goal request with valid arguments
rmw_request_id_t request_header;
ret = rcl_action_take_goal_request(
&this->action_server, &request_header, &this->incoming_goal_request);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the goal request was received correctly
EXPECT_EQ(this->outgoing_goal_request.order, this->incoming_goal_request.order);
EXPECT_TRUE(uuidcmp(this->outgoing_goal_request.uuid, this->incoming_goal_request.uuid));
// Initialize goal response
this->outgoing_goal_response.accepted = true;
this->outgoing_goal_response.stamp.sec = 123;
this->outgoing_goal_response.stamp.nanosec = 456789u;
// Send goal response with valid arguments
ret = rcl_action_send_goal_response(
&this->action_server, &request_header, &this->outgoing_goal_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
// Wait for goal response to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
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, &this->incoming_goal_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the goal response was received correctly
EXPECT_EQ(this->outgoing_goal_response.accepted, this->incoming_goal_response.accepted);
EXPECT_EQ(this->outgoing_goal_response.stamp.sec, this->incoming_goal_response.stamp.sec);
EXPECT_EQ(this->outgoing_goal_response.stamp.nanosec, this->incoming_goal_response.stamp.nanosec);
// Initialize result request
init_test_uuid0(this->outgoing_result_request.uuid);
// Send result request with valid arguments
ret = rcl_action_send_result_request(
&this->action_client, &this->outgoing_result_request, &sequence_number);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Initialize feedback
ASSERT_TRUE(rosidl_generator_c__int32__Sequence__init(
&this->outgoing_feedback.sequence, 3));
this->outgoing_feedback.sequence.data[0] = 0;
this->outgoing_feedback.sequence.data[1] = 1;
this->outgoing_feedback.sequence.data[2] = 2;
init_test_uuid0(this->outgoing_feedback.uuid);
// Publish feedback with valid arguments
ret = rcl_action_publish_feedback(&this->action_server, &this->outgoing_feedback);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
// Wait for feedback to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
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, &this->incoming_feedback);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that feedback was received correctly
EXPECT_TRUE(uuidcmp(this->outgoing_feedback.uuid, this->incoming_feedback.uuid));
ASSERT_EQ(this->outgoing_feedback.sequence.size, this->incoming_feedback.sequence.size);
EXPECT_TRUE(!memcmp(
this->outgoing_feedback.sequence.data,
this->incoming_feedback.sequence.data,
this->outgoing_feedback.sequence.size));
ret = rcl_wait_set_clear(&this->wait_set);
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;
// Wait for result request to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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,
&this->is_goal_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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, &this->incoming_result_request);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the result request was received correctly
EXPECT_TRUE(uuidcmp(
this->outgoing_result_request.uuid,
this->incoming_result_request.uuid));
// Initialize result response
ASSERT_TRUE(rosidl_generator_c__int32__Sequence__init(
&this->outgoing_result_response.sequence, 4));
this->outgoing_result_response.sequence.data[0] = 0;
this->outgoing_result_response.sequence.data[1] = 1;
this->outgoing_result_response.sequence.data[2] = 2;
this->outgoing_result_response.sequence.data[3] = 6;
this->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, &this->outgoing_result_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
// Wait for result response to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
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, &this->incoming_result_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the result response was received correctly
EXPECT_EQ(this->outgoing_result_response.status, this->incoming_result_response.status);
ASSERT_EQ(
this->outgoing_result_response.sequence.size, this->incoming_result_response.sequence.size);
EXPECT_TRUE(!memcmp(
this->outgoing_result_response.sequence.data,
this->incoming_result_response.sequence.data,
this->outgoing_result_response.sequence.size));
}
// Exercises the "Example 2" sequence diagram found in the actions_proposal document.
// This example is almost identical to the first, but this time the action client requests
// for the goal to be canceled mid-execution. Note that it is allowed to perform any shutdown
// operations after the cancel request before returning with the cancellation result.
TEST_F(
CLASSNAME(TestActionClientServerInteraction, RMW_IMPLEMENTATION), test_interaction_with_cancel)
{
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 goal request
init_test_uuid0(this->outgoing_goal_request.uuid);
this->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, &this->outgoing_goal_request, &sequence_number);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_wait_set_clear(&this->wait_set);
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;
// Wait for goal request to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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,
&this->is_goal_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_TRUE(this->is_goal_request_ready) << rcl_get_error_string().str;
EXPECT_FALSE(this->is_cancel_request_ready) << rcl_get_error_string().str;
EXPECT_FALSE(this->is_result_request_ready) << rcl_get_error_string().str;
// Take goal request with valid arguments
rmw_request_id_t request_header;
ret = rcl_action_take_goal_request(
&this->action_server, &request_header, &this->incoming_goal_request);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the goal request was received correctly
EXPECT_EQ(this->outgoing_goal_request.order, this->incoming_goal_request.order);
EXPECT_TRUE(uuidcmp(this->outgoing_goal_request.uuid, this->incoming_goal_request.uuid));
// Initialize goal response
this->outgoing_goal_response.accepted = true;
this->outgoing_goal_response.stamp.sec = 123;
this->outgoing_goal_response.stamp.nanosec = 456789u;
// Send goal response with valid arguments
ret = rcl_action_send_goal_response(
&this->action_server, &request_header, &this->outgoing_goal_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
// Wait for goal response to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
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, &this->incoming_goal_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the goal response was received correctly
EXPECT_EQ(this->outgoing_goal_response.accepted, this->incoming_goal_response.accepted);
EXPECT_EQ(this->outgoing_goal_response.stamp.sec, this->incoming_goal_response.stamp.sec);
EXPECT_EQ(this->outgoing_goal_response.stamp.nanosec, this->incoming_goal_response.stamp.nanosec);
// Initialize result request
init_test_uuid0(this->outgoing_result_request.uuid);
// Send result request with valid arguments
ret = rcl_action_send_result_request(
&this->action_client, &this->outgoing_result_request, &sequence_number);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Initialize feedback
ASSERT_TRUE(rosidl_generator_c__int32__Sequence__init(
&this->outgoing_feedback.sequence, 3));
this->outgoing_feedback.sequence.data[0] = 0;
this->outgoing_feedback.sequence.data[1] = 1;
this->outgoing_feedback.sequence.data[2] = 2;
init_test_uuid0(this->outgoing_feedback.uuid);
// Publish feedback with valid arguments
ret = rcl_action_publish_feedback(&this->action_server, &this->outgoing_feedback);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
// Wait for feedback to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
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, &this->incoming_feedback);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that feedback was received correctly
EXPECT_TRUE(uuidcmp(this->outgoing_feedback.uuid, this->incoming_feedback.uuid));
ASSERT_EQ(this->outgoing_feedback.sequence.size, this->incoming_feedback.sequence.size);
EXPECT_TRUE(!memcmp(
this->outgoing_feedback.sequence.data,
this->incoming_feedback.sequence.data,
this->outgoing_feedback.sequence.size));
ret = rcl_wait_set_clear(&this->wait_set);
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;
// Wait for result request to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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,
&this->is_goal_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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, &this->incoming_result_request);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the result request was received correctly
EXPECT_TRUE(uuidcmp(
this->outgoing_result_request.uuid,
this->incoming_result_request.uuid));
// Initialize result response
ASSERT_TRUE(rosidl_generator_c__int32__Sequence__init(
&this->outgoing_result_response.sequence, 4));
this->outgoing_result_response.sequence.data[0] = 0;
this->outgoing_result_response.sequence.data[1] = 1;
this->outgoing_result_response.sequence.data[2] = 2;
this->outgoing_result_response.sequence.data[3] = 6;
this->outgoing_result_response.status =
action_msgs__msg__GoalStatus__STATUS_SUCCEEDED;
// Initialize cancel request
rmw_request_id_t cancel_request_header;
init_test_uuid0(outgoing_cancel_request.goal_info.goal_id.uuid);
outgoing_cancel_request.goal_info.stamp.sec = 321;
outgoing_cancel_request.goal_info.stamp.nanosec = 987654u;
// Send cancel request with valid arguments
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;
ret = rcl_wait_set_clear(&this->wait_set);
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;
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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,
&this->is_goal_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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
ret = rcl_action_take_cancel_request(
&this->action_server, &cancel_request_header, &incoming_cancel_request);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the cancel request was received correctly
EXPECT_TRUE(uuidcmp(
outgoing_cancel_request.goal_info.goal_id.uuid,
incoming_cancel_request.goal_info.goal_id.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);
// Get a list of goal info that should be attempted to be cancelled
rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response();
ret = rcl_action_process_cancel_request(
&this->action_server, &incoming_cancel_request, &cancel_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Send cancel response with valid arguments
// rmw_request_id_t response_header;
ret = rcl_action_send_cancel_response(
&this->action_server, &cancel_request_header, &outgoing_cancel_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
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, &cancel_request_header, &incoming_cancel_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// 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->goal_id.uuid, incoming_goal_info->goal_id.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);
}
// Send result response with valid arguments
ret = rcl_action_send_result_response(
&this->action_server, &request_header, &this->outgoing_result_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
// Wait for result response to be ready
ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(1));
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
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;
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, &this->incoming_result_response);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Check that the result response was received correctly
EXPECT_EQ(this->outgoing_result_response.status, this->incoming_result_response.status);
ASSERT_EQ(
this->outgoing_result_response.sequence.size, this->incoming_result_response.sequence.size);
EXPECT_TRUE(!memcmp(
this->outgoing_result_response.sequence.data,
this->incoming_result_response.sequence.data,
this->outgoing_result_response.sequence.size));
test_msgs__action__Fibonacci_Goal_Request__fini(&this->outgoing_goal_request);
test_msgs__action__Fibonacci_Goal_Request__fini(&this->incoming_goal_request);
test_msgs__action__Fibonacci_Goal_Response__fini(&this->incoming_goal_response);
test_msgs__action__Fibonacci_Goal_Response__fini(&this->outgoing_goal_response);
test_msgs__action__Fibonacci_Result_Request__fini(&this->incoming_result_request);
test_msgs__action__Fibonacci_Result_Request__fini(&this->outgoing_result_request);
test_msgs__action__Fibonacci_Result_Response__fini(&this->incoming_result_response);
test_msgs__action__Fibonacci_Result_Response__fini(&this->outgoing_result_response);
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);
}