diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 271d195..9d17d17 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -746,6 +746,8 @@ rcl_action_process_cancel_request( if (!uuidcmpzero(request_uuid) && (0u == request_nanosec)) { // UUID is not zero and timestamp is zero; cancel exactly one goal (if it exists) rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info(); + // Assume the goal ID is invalid until we find it + cancel_response->msg.return_code = action_msgs__srv__CancelGoal_Response__ERROR_UNKNOWN_GOAL_ID; rcl_action_goal_handle_t * goal_handle; for (size_t i = 0u; i < total_num_goals; ++i) { goal_handle = action_server->impl->goal_handles[i]; @@ -758,11 +760,18 @@ rcl_action_process_cancel_request( if (uuidcmp(request_uuid, goal_info.goal_id.uuid)) { if (rcl_action_goal_handle_is_cancelable(goal_handle)) { goal_handles_to_cancel[num_goals_to_cancel++] = goal_handle; + cancel_response->msg.return_code = action_msgs__srv__CancelGoal_Response__ERROR_NONE; + } else { + // If the goal is not cancelable, it must be because it is in a terminal state + cancel_response->msg.return_code = + action_msgs__srv__CancelGoal_Response__ERROR_GOAL_TERMINATED; } break; } } } else { + // The caller can later update the response code to reject the request if desired + cancel_response->msg.return_code = action_msgs__srv__CancelGoal_Response__ERROR_NONE; if (uuidcmpzero(request_uuid) && (0u == request_nanosec)) { // UUID and timestamp are both zero; cancel all goals // Set timestamp to max to cancel all active goals in the following for-loop diff --git a/rcl_action/src/rcl_action/types.c b/rcl_action/src/rcl_action/types.c index c072845..4e6ce33 100644 --- a/rcl_action/src/rcl_action/types.c +++ b/rcl_action/src/rcl_action/types.c @@ -44,7 +44,7 @@ rcl_action_get_zero_initialized_cancel_request(void) rcl_action_cancel_response_t rcl_action_get_zero_initialized_cancel_response(void) { - static rcl_action_cancel_response_t response = {{{0, 0, 0}}, {0, 0, 0, 0, 0}}; + static rcl_action_cancel_response_t response = {{0, {0, 0, 0}}, {0, 0, 0, 0, 0}}; return response; } @@ -104,7 +104,7 @@ rcl_action_cancel_response_init( return RCL_RET_INVALID_ARGUMENT; } // Ensure cancel response is zero initialized - if (cancel_response->msg.goals_canceling.size > 0) { + if (0 != cancel_response->msg.return_code || cancel_response->msg.goals_canceling.size > 0) { RCL_SET_ERROR_MSG("cancel_response already inititalized"); return RCL_RET_ALREADY_INIT; } diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp index 0bd6c54..aa6fe75 100644 --- a/rcl_action/test/rcl_action/test_action_server.cpp +++ b/rcl_action/test/rcl_action/test_action_server.cpp @@ -17,6 +17,8 @@ #include #include +#include "action_msgs/srv/cancel_goal.h" + #include "rcl_action/action_server.h" #include "rcl/error_handling.h" @@ -376,6 +378,8 @@ TEST_F(TestActionServer, test_action_process_cancel_request) EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); + // A zero request means "cancel all goals", which succeeds if there's nothing to cancel + EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE); } TEST_F(TestActionServer, test_action_server_get_goal_status_array) @@ -537,6 +541,7 @@ TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_all_goal EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_NE(cancel_response.msg.goals_canceling.data, nullptr); ASSERT_EQ(cancel_response.msg.goals_canceling.size, (size_t)NUM_GOALS); + EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE); rcl_action_goal_info_t * goal_info_out; for (int i = 0; i < NUM_GOALS; ++i) { goal_info_out = &cancel_response.msg.goals_canceling.data[i]; @@ -549,18 +554,60 @@ TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_all_goal TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_single_goal) { - // Request to cancel a specific goal - rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); - init_test_uuid0(cancel_request.goal_info.goal_id.uuid); - rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); - rcl_ret_t ret = rcl_action_process_cancel_request( - &this->action_server, &cancel_request, &cancel_response); - EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; - EXPECT_NE(cancel_response.msg.goals_canceling.data, nullptr); - ASSERT_EQ(cancel_response.msg.goals_canceling.size, 1u); - rcl_action_goal_info_t * goal_info = &cancel_response.msg.goals_canceling.data[0]; - EXPECT_TRUE(uuidcmp(goal_info->goal_id.uuid, cancel_request.goal_info.goal_id.uuid)); - EXPECT_EQ(RCL_RET_OK, rcl_action_cancel_response_fini(&cancel_response)); + { + // Request to cancel a specific goal + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + init_test_uuid0(cancel_request.goal_info.goal_id.uuid); + rcl_action_cancel_response_t cancel_response = + rcl_action_get_zero_initialized_cancel_response(); + rcl_ret_t ret = rcl_action_process_cancel_request( + &this->action_server, &cancel_request, &cancel_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_NE(cancel_response.msg.goals_canceling.data, nullptr); + ASSERT_EQ(cancel_response.msg.goals_canceling.size, 1u); + EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE); + rcl_action_goal_info_t * goal_info = &cancel_response.msg.goals_canceling.data[0]; + EXPECT_TRUE(uuidcmp(goal_info->goal_id.uuid, cancel_request.goal_info.goal_id.uuid)); + EXPECT_EQ(RCL_RET_OK, rcl_action_cancel_response_fini(&cancel_response)); + } + { + // Request to cancel an invalid goal + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + init_test_uuid1(cancel_request.goal_info.goal_id.uuid); + rcl_action_cancel_response_t cancel_response = + rcl_action_get_zero_initialized_cancel_response(); + rcl_ret_t ret = rcl_action_process_cancel_request( + &this->action_server, &cancel_request, &cancel_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); + EXPECT_EQ( + cancel_response.msg.return_code, + action_msgs__srv__CancelGoal_Response__ERROR_UNKNOWN_GOAL_ID); + EXPECT_EQ(RCL_RET_OK, rcl_action_cancel_response_fini(&cancel_response)); + } + { + // Request to cancel a terminated goal + // First, transition a goal handle to a terminal state + rcl_ret_t ret = rcl_action_update_goal_state(&this->handles[3], GOAL_EVENT_EXECUTE); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_action_update_goal_state(&this->handles[3], GOAL_EVENT_SUCCEED); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // Attempt to cancel the terminated goal + rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request(); + cancel_request.goal_info.goal_id = this->goal_infos_out[3].goal_id; + rcl_action_cancel_response_t cancel_response = + rcl_action_get_zero_initialized_cancel_response(); + ret = rcl_action_process_cancel_request( + &this->action_server, &cancel_request, &cancel_response); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); + EXPECT_EQ( + cancel_response.msg.return_code, + action_msgs__srv__CancelGoal_Response__ERROR_GOAL_TERMINATED); + EXPECT_EQ(RCL_RET_OK, rcl_action_cancel_response_fini(&cancel_response)); + } } TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time) @@ -575,6 +622,7 @@ TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time) EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_NE(cancel_response.msg.goals_canceling.data, nullptr); ASSERT_EQ(cancel_response.msg.goals_canceling.size, time_index + 1); // goals at indices [0, 7] + EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE); rcl_action_goal_info_t * goal_info_out; for (size_t i = 0; i < cancel_response.msg.goals_canceling.size; ++i) { goal_info_out = &cancel_response.msg.goals_canceling.data[i]; @@ -600,6 +648,7 @@ TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time_ &this->action_server, &cancel_request, &cancel_response); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; EXPECT_NE(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE); const size_t num_goals_canceling = cancel_response.msg.goals_canceling.size; ASSERT_EQ(num_goals_canceling, time_index + 2); // goals at indices [0, 2] and 8 rcl_action_goal_info_t * goal_info_out; diff --git a/rcl_action/test/rcl_action/test_types.cpp b/rcl_action/test/rcl_action/test_types.cpp index f9f24d0..b31c110 100644 --- a/rcl_action/test/rcl_action/test_types.cpp +++ b/rcl_action/test/rcl_action/test_types.cpp @@ -67,6 +67,7 @@ TEST(TestActionTypes, test_get_zero_initialized_cancel_response) rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response(); EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.return_code, 0); } TEST(TestActionTypes, test_init_fini_goal_status_array) @@ -133,6 +134,7 @@ TEST(TestActionTypes, test_init_fini_cancel_response) EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.return_code, 0); // Initialize with zero size cancel_response = rcl_action_get_zero_initialized_cancel_response(); @@ -141,6 +143,7 @@ TEST(TestActionTypes, test_init_fini_cancel_response) EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); EXPECT_EQ(cancel_response.msg.goals_canceling.size, 0u); EXPECT_EQ(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.return_code, 0); // Initialize with valid arguments cancel_response = rcl_action_get_zero_initialized_cancel_response(); @@ -152,6 +155,7 @@ TEST(TestActionTypes, test_init_fini_cancel_response) EXPECT_EQ(ret, RCL_RET_OK); EXPECT_EQ(cancel_response.msg.goals_canceling.size, num_goals_canceling); EXPECT_NE(cancel_response.msg.goals_canceling.data, nullptr); + EXPECT_EQ(cancel_response.msg.return_code, action_msgs__srv__CancelGoal_Response__ERROR_NONE); // Finalize with invalid cancel response ret = rcl_action_cancel_response_fini(nullptr);