Skip to content

Commit

Permalink
Add return code to CancelGoal service response (#422)
Browse files Browse the repository at this point in the history 
* 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>
  • Loading branch information
@jacobperron
jacobperron authored May 2, 2019
1 parent d947655 commit eabe426
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 14 deletions.
9 changes: 9 additions & 0 deletions rcl_action/src/rcl_action/action_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand All @@ -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
Expand Down
4 changes: 2 additions & 2 deletions rcl_action/src/rcl_action/types.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
}
Expand Down
73 changes: 61 additions & 12 deletions rcl_action/test/rcl_action/test_action_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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];
Expand All @@ -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)
Expand All @@ -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];
Expand All @@ -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;
Expand Down
4 changes: 4 additions & 0 deletions rcl_action/test/rcl_action/test_types.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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();
Expand All @@ -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();
Expand All @@ -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);
Expand Down

0 comments on commit eabe426

Please sign in to comment.