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

Add action server implementation (#323)

Browse source 
* Implement action server init, fini, and is_valid functions

* Add macros for initializing services and publishers

* Implement rcl_action_server_get_default_options()

* Implement rcl_action_accept_new_goal()

* Add function, rcl_action_server_goal_exists(), for checking if goal is already being tracked by an action server

* Add unit tests

* Implement rcl_action_server_get_goal_handles()

* Implement rcl_action_server_get_options()

* Implement rcl_action_server_get_action_name()

* Implement rcl_action_get_goal_status_array()

* Bugfix: reset pointers and size in type finalize functions
Also let finalize functions be called on already finalized objects

* Implement send/take functions for action server services

* Implement action server publishers for feedback and status

* Implement rcl_action_process_cancel_request()

* Add partial communication tests

* Define UUID_SIZE

* Use type-erased pointer for rcl_action_publish_status()

* Implement rcl_action_clear_expired_goals()
Introduce rcl_clock_t to action server implementation.

* Change internal goal handle array to be an array of pointers.

* Add check for invalid action names

* Do heap allocation of temporary array to satisfy MSVC compiler

* Bugfix: finalize node in test tear downs and reset expected errors

* Update documentation

* Update package.xml

* Pass in rcl_clock_t to action server
Rather than initializing internally.

* Do not finalize goal handles in expire function
Instead, leave it up to the caller to finalize goal handles.
Renamed the function to rcl_action_expire_goals.
This commit is contained in:
Jacob Perron 2018-11-20 12:04:13 -08:00 • committed by GitHub
parent d86d1c4135
commit acc974e43b
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
11 changed files with 1879 additions and 94 deletions
Download patch file Download diff file Expand all files Collapse all files

333
rcl_action/test/rcl_action/test_action_communication.cpp Normal file
View file

@ -0,0 +1,333 @@
// 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_server.h"
#include "rcl/error_handling.h"
#include "rcl/rcl.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
// TODO(jacobperron): Add action client to complete tests
class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing::Test
{
protected:
void SetUp() override
{
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_ret_t ret = rcl_init(0, nullptr, allocator);
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", "", &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 rcl_action_server_options_t options = rcl_action_server_get_default_options();
const char * action_name = "test_action_commmunication_name";
this->action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_server_init(
&this->action_server, &this->node, &this->clock, ts, action_name, &options);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
}
void TearDown() override
{
// Finalize
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);
ret = rcl_node_fini(&this->node);
EXPECT_EQ(ret, RCL_RET_OK);
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
void init_test_uuid0(uint8_t * uuid)
{
for (uint8_t i = 0; i < 16; ++i) {
uuid[i] = i;
}
}
void init_test_uuid1(uint8_t * uuid)
{
for (uint8_t i = 0; i < 16; ++i) {
uuid[i] = 15 - i;
}
}
rcl_action_server_t action_server;
rcl_node_t node;
rcl_clock_t clock;
}; // class TestActionCommunication
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_take_goal_request)
{
test_msgs__action__Fibonacci_Goal_Request goal_request;
test_msgs__action__Fibonacci_Goal_Request__init(&goal_request);
// Take request with null action server
rcl_ret_t ret = rcl_action_take_goal_request(nullptr, &goal_request);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Take request with null message
ret = rcl_action_take_goal_request(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Take request with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_take_goal_request(&invalid_action_server, &goal_request);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Take with valid arguments
// TODO(jacobperron): Send a request from a client
// ret = rcl_action_take_goal_request(&this->action_server, &goal_request);
// EXPECT_EQ(ret, RCL_RET_OK);
test_msgs__action__Fibonacci_Goal_Request__fini(&goal_request);
}
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_send_goal_response)
{
test_msgs__action__Fibonacci_Goal_Response goal_response;
test_msgs__action__Fibonacci_Goal_Response__init(&goal_response);
// Send response with null action server
rcl_ret_t ret = rcl_action_send_goal_response(nullptr, &goal_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Send response with null message
ret = rcl_action_send_goal_response(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Send response with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_send_goal_response(&invalid_action_server, &goal_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Send with valid arguments
// TODO(jacobperron): Check with client on receiving end
ret = rcl_action_send_goal_response(&this->action_server, &goal_response);
EXPECT_EQ(ret, RCL_RET_OK);
test_msgs__action__Fibonacci_Goal_Response__fini(&goal_response);
}
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_take_cancel_request)
{
action_msgs__srv__CancelGoal_Request cancel_request;
action_msgs__srv__CancelGoal_Request__init(&cancel_request);
// Take request with null action server
rcl_ret_t ret = rcl_action_take_cancel_request(nullptr, &cancel_request);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Take request with null message
ret = rcl_action_take_cancel_request(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Take request with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_take_cancel_request(&invalid_action_server, &cancel_request);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Take with valid arguments
// TODO(jacobperron): Send a request from a client
// ret = rcl_action_take_cancel_request(&this->action_server, &cancel_request);
// EXPECT_EQ(ret, RCL_RET_OK);
action_msgs__srv__CancelGoal_Request__fini(&cancel_request);
}
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_send_cancel_response)
{
action_msgs__srv__CancelGoal_Response cancel_response;
action_msgs__srv__CancelGoal_Response__init(&cancel_response);
// Send response with null action server
rcl_ret_t ret = rcl_action_send_cancel_response(nullptr, &cancel_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Send response with null message
ret = rcl_action_send_cancel_response(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Send response with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_send_cancel_response(&invalid_action_server, &cancel_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Send with valid arguments
// TODO(jacobperron): Check with client on receiving end
ret = rcl_action_send_cancel_response(&this->action_server, &cancel_response);
EXPECT_EQ(ret, RCL_RET_OK);
action_msgs__srv__CancelGoal_Response__fini(&cancel_response);
}
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_take_result_request)
{
test_msgs__action__Fibonacci_Result_Request result_request;
test_msgs__action__Fibonacci_Result_Request__init(&result_request);
// Take request with null action server
rcl_ret_t ret = rcl_action_take_result_request(nullptr, &result_request);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Take request with null message
ret = rcl_action_take_result_request(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Take request with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_take_result_request(&invalid_action_server, &result_request);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Take with valid arguments
// TODO(jacobperron): Send a request from a client
// ret = rcl_action_take_result_request(&this->action_server, &result_request);
// EXPECT_EQ(ret, RCL_RET_OK);
test_msgs__action__Fibonacci_Result_Request__fini(&result_request);
}
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_send_result_response)
{
test_msgs__action__Fibonacci_Result_Response result_response;
test_msgs__action__Fibonacci_Result_Response__init(&result_response);
// Send response with null action server
rcl_ret_t ret = rcl_action_send_result_response(nullptr, &result_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Send response with null message
ret = rcl_action_send_result_response(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Send response with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_send_result_response(&invalid_action_server, &result_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Send with valid arguments
// TODO(jacobperron): Check with client on receiving end
ret = rcl_action_send_result_response(&this->action_server, &result_response);
EXPECT_EQ(ret, RCL_RET_OK);
test_msgs__action__Fibonacci_Result_Response__fini(&result_response);
}
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_publish_feedback)
{
test_msgs__action__Fibonacci_Feedback feedback;
test_msgs__action__Fibonacci_Feedback__init(&feedback);
// Publish feedback with null action server
rcl_ret_t ret = rcl_action_publish_feedback(nullptr, &feedback);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Publish feedback with null message
ret = rcl_action_publish_feedback(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Publish feedback with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_publish_feedback(&invalid_action_server, &feedback);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Publish feedback with valid arguments
// TODO(jacobperron): Check with client on receiving end
ret = rcl_action_publish_feedback(&this->action_server, &feedback);
EXPECT_EQ(ret, RCL_RET_OK);
test_msgs__action__Fibonacci_Feedback__fini(&feedback);
}
TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_publish_status)
{
rcl_action_goal_status_array_t status_array =
rcl_action_get_zero_initialized_goal_status_array();
rcl_ret_t ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Publish status with null action server
ret = rcl_action_publish_status(nullptr, &status_array.msg);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Publish status with null message
ret = rcl_action_publish_status(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Publish status with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_publish_status(&invalid_action_server, &status_array.msg);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Publish status with valid arguments (but empty array)
// TODO(jacobperron): Check with client on receiving end
ret = rcl_action_publish_status(&this->action_server, &status_array.msg);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Add a goal before publishing the status array
ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
rcl_action_goal_handle_t * goal_handle;
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info);
ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Publish status with valid arguments (one goal in array)
// TODO(jacobperron): Check with client on receiving end
ret = rcl_action_publish_status(&this->action_server, &status_array.msg);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
}

551
rcl_action/test/rcl_action/test_action_server.cpp Normal file
View file

@ -0,0 +1,551 @@
// 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 <chrono>
#include <thread>
#include "rcl_action/action_server.h"
#include "rcl/error_handling.h"
#include "rcl/rcl.h"
#include "test_msgs/action/fibonacci.h"
TEST(TestActionServerInitFini, test_action_server_init_fini)
{
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_ret_t ret = rcl_init(0, nullptr, allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
rcl_node_t node = rcl_get_zero_initialized_node();
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(&node, "test_action_server_node", "", &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
rcl_clock_t clock;
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci);
const rcl_action_server_options_t options = rcl_action_server_get_default_options();
const char * action_name = "test_action_server_name";
rcl_action_server_t action_server = rcl_action_get_zero_initialized_server();
// Initialize with a null action server
ret = rcl_action_server_init(nullptr, &node, &clock, ts, action_name, &options);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with a null node
ret = rcl_action_server_init(&action_server, nullptr, &clock, ts, action_name, &options);
EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with an invalid node
rcl_node_t invalid_node = rcl_get_zero_initialized_node();
ret = rcl_action_server_init(&action_server, &invalid_node, &clock, ts, action_name, &options);
EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with a null clock
ret = rcl_action_server_init(&action_server, &node, nullptr, ts, action_name, &options);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with an invalid clock
rcl_clock_t invalid_clock;
invalid_clock.get_now = nullptr;
ret = rcl_action_server_init(&action_server, &node, &invalid_clock, ts, action_name, &options);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with a null typesupport
ret = rcl_action_server_init(&action_server, &node, &clock, nullptr, action_name, &options);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with a null name
ret = rcl_action_server_init(&action_server, &node, &clock, ts, nullptr, &options);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with an empty name
const char * empty_action_name = "";
ret = rcl_action_server_init(&action_server, &node, &clock, ts, empty_action_name, &options);
EXPECT_EQ(ret, RCL_RET_ACTION_NAME_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with an invalid name
const char * invalid_action_name = "42";
ret = rcl_action_server_init(&action_server, &node, &clock, ts, invalid_action_name, &options);
EXPECT_EQ(ret, RCL_RET_ACTION_NAME_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with a null options
ret = rcl_action_server_init(&action_server, &node, &clock, ts, action_name, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str;
rcl_reset_error();
// Initialize with valid arguments
ret = rcl_action_server_init(&action_server, &node, &clock, ts, action_name, &options);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Try to initialize again
ret = rcl_action_server_init(&action_server, &node, &clock, ts, action_name, &options);
EXPECT_EQ(ret, RCL_RET_ALREADY_INIT) << rcl_get_error_string().str;
rcl_reset_error();
// Finalize with null action server
ret = rcl_action_server_fini(nullptr, &node);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Finalize with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_server_fini(&invalid_action_server, &node);
// Nothing happens
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Finalize with null node
ret = rcl_action_server_fini(&action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_NODE_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Finalize with valid arguments
ret = rcl_action_server_fini(&action_server, &node);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Finalize clock
ret = rcl_clock_fini(&clock);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Finalize node
ret = rcl_node_fini(&node);
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
class TestActionServer : public ::testing::Test
{
protected:
void SetUp() override
{
rcl_allocator_t allocator = rcl_get_default_allocator();
rcl_ret_t ret = rcl_init(0, nullptr, allocator);
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_server_node", "", &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_clock_init(RCL_STEADY_TIME, &this->clock, &allocator);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
const rosidl_action_type_support_t * ts = ROSIDL_GET_ACTION_TYPE_SUPPORT(
test_msgs, Fibonacci);
const rcl_action_server_options_t options = rcl_action_server_get_default_options();
const char * action_name = "test_action_server_name";
this->action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_server_init(
&this->action_server, &this->node, &this->clock, ts, action_name, &options);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
}
void TearDown() override
{
// Finalize
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_node_fini(&this->node);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
ret = rcl_shutdown();
EXPECT_EQ(RCL_RET_OK, ret) << 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;
}
}
rcl_action_server_t action_server;
rcl_node_t node;
rcl_clock_t clock;
}; // class TestActionServer
TEST_F(TestActionServer, test_action_server_is_valid)
{
// Check with null pointer
bool is_valid = rcl_action_server_is_valid(nullptr);
EXPECT_FALSE(is_valid) << rcl_get_error_string().str;
rcl_reset_error();
// Check with uninitialized action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
is_valid = rcl_action_server_is_valid(&invalid_action_server);
EXPECT_FALSE(is_valid) << rcl_get_error_string().str;
rcl_reset_error();
// Check valid action server
is_valid = rcl_action_server_is_valid(&this->action_server);
EXPECT_TRUE(is_valid) << rcl_get_error_string().str;
}
TEST_F(TestActionServer, test_action_accept_new_goal)
{
// Initialize a goal info
rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
init_test_uuid0(goal_info_in.uuid);
// Accept goal with a null action server
rcl_action_goal_handle_t * goal_handle = rcl_action_accept_new_goal(nullptr, &goal_info_in);
EXPECT_EQ(goal_handle, nullptr);
rcl_reset_error();
// Accept goal with null goal info
goal_handle = rcl_action_accept_new_goal(&this->action_server, nullptr);
EXPECT_EQ(goal_handle, nullptr);
rcl_reset_error();
// Accept goal with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
goal_handle = rcl_action_accept_new_goal(&invalid_action_server, &goal_info_in);
EXPECT_EQ(goal_handle, nullptr);
rcl_reset_error();
// Accept with valid arguments
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
EXPECT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
rcl_action_goal_info_t goal_info_out = rcl_action_get_zero_initialized_goal_info();
rcl_ret_t ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info_out);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_TRUE(uuidcmp(goal_info_out.uuid, goal_info_in.uuid));
size_t num_goals = 0u;
rcl_action_goal_handle_t ** goal_handle_array = {nullptr};
ret = rcl_action_server_get_goal_handles(&this->action_server, &goal_handle_array, &num_goals);
ASSERT_EQ(ret, RCL_RET_OK);
EXPECT_EQ(num_goals, 1u);
EXPECT_NE(goal_handle_array, nullptr) << rcl_get_error_string().str;
EXPECT_NE(goal_handle_array[0], nullptr) << rcl_get_error_string().str;
// Accept with the same goal ID
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
EXPECT_EQ(goal_handle, nullptr);
rcl_reset_error();
// Accept a different goal
goal_info_in = rcl_action_get_zero_initialized_goal_info();
init_test_uuid1(goal_info_in.uuid);
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
EXPECT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info_out);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_TRUE(uuidcmp(goal_info_out.uuid, goal_info_in.uuid));
ret = rcl_action_server_get_goal_handles(&this->action_server, &goal_handle_array, &num_goals);
ASSERT_EQ(ret, RCL_RET_OK);
EXPECT_EQ(num_goals, 2u);
EXPECT_NE(goal_handle_array, nullptr) << rcl_get_error_string().str;
EXPECT_NE(goal_handle_array[0], nullptr) << rcl_get_error_string().str;
EXPECT_NE(goal_handle_array[1], nullptr) << rcl_get_error_string().str;
}
TEST_F(TestActionServer, test_action_clear_expired_goals)
{
size_t num_expired = 1u;
// Clear expired goals with null action server
rcl_ret_t ret = rcl_action_expire_goals(nullptr, &num_expired);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Clear with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_expire_goals(&invalid_action_server, &num_expired);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID) << rcl_get_error_string().str;
rcl_reset_error();
// Clear with valid arguments
ret = rcl_action_expire_goals(&this->action_server, &num_expired);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_EQ(num_expired, 0u);
// Clear with valid arguments (optional num_expired)
ret = rcl_action_expire_goals(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// TODO(jacobperron): Test with goals that actually expire
}
TEST_F(TestActionServer, test_action_process_cancel_request)
{
rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
rcl_action_cancel_response_t cancel_response = rcl_action_get_zero_initialized_cancel_response();
// Process cancel request with null action server
rcl_ret_t ret = rcl_action_process_cancel_request(nullptr, &cancel_request, &cancel_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Process cancel request with null request message
ret = rcl_action_process_cancel_request(&this->action_server, nullptr, &cancel_response);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Process cancel request with null response message
ret = rcl_action_process_cancel_request(&this->action_server, &cancel_request, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Process cancel request with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_process_cancel_request(
&invalid_action_server, &cancel_request, &cancel_response);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Process cancel request with valid arguments (but no goals to cancel)
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);
}
TEST_F(TestActionServer, test_action_server_get_goal_status_array)
{
rcl_action_goal_status_array_t status_array =
rcl_action_get_zero_initialized_goal_status_array();
// Get with null action server
rcl_ret_t ret = rcl_action_get_goal_status_array(nullptr, &status_array);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Get with null status array
ret = rcl_action_get_goal_status_array(&this->action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT);
rcl_reset_error();
// Get with invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
ret = rcl_action_get_goal_status_array(&invalid_action_server, nullptr);
EXPECT_EQ(ret, RCL_RET_ACTION_SERVER_INVALID);
rcl_reset_error();
// Get with valid arguments (but not goals being tracked)
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(status_array.msg.status_list.data, nullptr);
EXPECT_EQ(status_array.msg.status_list.size, 0u);
ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK);
// Add a goal before getting the status array
rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
init_test_uuid0(goal_info_in.uuid);
rcl_action_goal_handle_t * goal_handle;
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_NE(status_array.msg.status_list.data, nullptr);
EXPECT_EQ(status_array.msg.status_list.size, 1u);
rcl_action_goal_info_t * goal_info_out = &status_array.msg.status_list.data[0].goal_info;
EXPECT_TRUE(uuidcmp(goal_info_out->uuid, goal_info_in.uuid));
ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK);
// Add nine more goals
for (int i = 1; i < 10; ++i) {
for (int j = 0; j < UUID_SIZE; ++j) {
goal_info_in.uuid[j] = static_cast<uint8_t>(i + j);
}
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
}
ret = rcl_action_get_goal_status_array(&this->action_server, &status_array);
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
EXPECT_NE(status_array.msg.status_list.data, nullptr);
ASSERT_EQ(status_array.msg.status_list.size, 10u);
for (int i = 0; i < 10; ++i) {
goal_info_out = &status_array.msg.status_list.data[i].goal_info;
for (int j = 0; j < UUID_SIZE; ++j) {
EXPECT_EQ(goal_info_out->uuid[j], i + j);
}
}
ret = rcl_action_goal_status_array_fini(&status_array);
ASSERT_EQ(ret, RCL_RET_OK);
}
TEST_F(TestActionServer, test_action_server_get_action_name)
{
// Get action_name for a null action server
const char * action_name = rcl_action_server_get_action_name(nullptr);
EXPECT_EQ(action_name, nullptr);
rcl_reset_error();
// Get action_name for an invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
action_name = rcl_action_server_get_action_name(&invalid_action_server);
EXPECT_EQ(action_name, nullptr);
rcl_reset_error();
// Get action_name for a valid action server
action_name = rcl_action_server_get_action_name(&this->action_server);
ASSERT_NE(action_name, nullptr) << rcl_get_error_string().str;
EXPECT_STREQ(action_name, "test_action_server_name");
}
TEST_F(TestActionServer, test_action_server_get_options)
{
// Get options for a null action server
const rcl_action_server_options_t * options = rcl_action_server_get_options(nullptr);
EXPECT_EQ(options, nullptr);
rcl_reset_error();
// Get options for an invalid action server
rcl_action_server_t invalid_action_server = rcl_action_get_zero_initialized_server();
options = rcl_action_server_get_options(&invalid_action_server);
EXPECT_EQ(options, nullptr);
rcl_reset_error();
// Get options for a valid action server
options = rcl_action_server_get_options(&this->action_server);
EXPECT_NE(options, nullptr) << rcl_get_error_string().str;
}
class TestActionServerCancelPolicy : public TestActionServer
{
protected:
void SetUp() override
{
TestActionServer::SetUp();
// Add several goals
rcl_action_goal_info_t goal_info_in = rcl_action_get_zero_initialized_goal_info();
rcl_action_goal_handle_t * goal_handle;
rcl_ret_t ret;
for (int i = 0; i < NUM_GOALS; ++i) {
for (int j = 0; j < UUID_SIZE; ++j) {
goal_info_in.uuid[j] = static_cast<uint8_t>(i + j);
}
goal_handle = rcl_action_accept_new_goal(&this->action_server, &goal_info_in);
ASSERT_NE(goal_handle, nullptr) << rcl_get_error_string().str;
goal_infos_out[i] = rcl_action_get_zero_initialized_goal_info();
ret = rcl_action_goal_handle_get_info(goal_handle, &goal_infos_out[i]);
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str;
// Sleep so goals have different acceptance times
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
}
void TearDown() override
{
TestActionServer::TearDown();
}
static const int NUM_GOALS = 10;
rcl_action_goal_info_t goal_infos_out[NUM_GOALS];
};
TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_all_goals)
{
// Request to cancel all goals
rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
cancel_request.goal_info.stamp.sec = 0;
cancel_request.goal_info.stamp.nanosec = 0u;
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, (size_t)NUM_GOALS);
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];
for (int j = 0; j < UUID_SIZE; ++j) {
EXPECT_EQ(goal_info_out->uuid[j], static_cast<uint8_t>(i + j));
}
}
}
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.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->uuid, cancel_request.goal_info.uuid));
}
TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time)
{
// Request to cancel all goals at and before a specific time
const size_t time_index = 7;
rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
cancel_request.goal_info = this->goal_infos_out[time_index];
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, time_index + 1); // goals at indices [0, 7]
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];
for (size_t j = 0; j < UUID_SIZE; ++j) {
EXPECT_EQ(goal_info_out->uuid[j], static_cast<uint8_t>(i + j));
}
}
}
TEST_F(TestActionServerCancelPolicy, test_action_process_cancel_request_by_time_and_id)
{
// Request to cancel a specific goal by ID and all goals at and before a specific time
const size_t goal_index = 9;
const size_t time_index = 2;
rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
cancel_request.goal_info = this->goal_infos_out[time_index];
for (int i = 0; i < UUID_SIZE; ++i) {
cancel_request.goal_info.uuid[i] = static_cast<uint8_t>(i + goal_index);
}
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);
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;
for (size_t i = 0; i < num_goals_canceling - 1; ++i) {
goal_info_out = &cancel_response.msg.goals_canceling.data[i];
for (size_t j = 0; j < UUID_SIZE; ++j) {
EXPECT_EQ(goal_info_out->uuid[j], static_cast<uint8_t>(i + j));
}
}
goal_info_out = &cancel_response.msg.goals_canceling.data[num_goals_canceling - 1];
EXPECT_TRUE(uuidcmp(goal_info_out->uuid, cancel_request.goal_info.uuid));
}

15
rcl_action/test/rcl_action/test_names.cpp
View file

@ -19,6 +19,7 @@
#include "rcl_action/names.h"
#include "rcl/allocator.h"
#include "rcl/error_handling.h"
#include "rcl/types.h"
struct ActionDerivedNameTestSubject
@ -62,7 +63,8 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
ret = test_subject.get_action_derived_name(
invalid_action_name, default_allocator,
&action_derived_name);
EXPECT_EQ(RCL_RET_ACTION_NAME_INVALID, ret);
EXPECT_EQ(RCL_RET_ACTION_NAME_INVALID, ret) << rcl_get_error_string().str;
rcl_reset_error();
action_derived_name = NULL;
rcl_allocator_t invalid_allocator =
@ -70,27 +72,30 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
ret = test_subject.get_action_derived_name(
test_subject.action_name, invalid_allocator,
&action_derived_name);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
rcl_reset_error();
action_derived_name = NULL;
char ** invalid_ptr_to_action_derived_name = NULL;
ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator,
invalid_ptr_to_action_derived_name);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
rcl_reset_error();
char dummy_char = '\0';
action_derived_name = &dummy_char;
ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator,
&action_derived_name);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
rcl_reset_error();
action_derived_name = NULL;
ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator,
&action_derived_name);
ASSERT_EQ(RCL_RET_OK, ret);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
EXPECT_STREQ(test_subject.expected_action_derived_name, action_derived_name);
default_allocator.deallocate(action_derived_name, default_allocator.state);
}