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

Add return code to CancelGoal service response (#422)

Browse source 
* Add return code to CancelGoal service response

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Add case for terminated goals

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Check for ERROR_NONE instead of 0

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
This commit is contained in:
Jacob Perron 2019-05-02 15:25:07 -07:00 • committed by GitHub
parent d947655a53
commit eabe426866
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 76 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

9
rcl_action/src/rcl_action/action_server.c
View file

@ -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

4
rcl_action/src/rcl_action/types.c
View file

@ -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;
}

51
rcl_action/test/rcl_action/test_action_server.cpp
View file

@ -17,6 +17,8 @@
#include <thread>
#include <vector>
#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_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;

4
rcl_action/test/rcl_action/test_types.cpp
View file

@ -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);