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

73
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_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;

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