Skip to content

Commit

Permalink
Pass in rcl_clock_t to action server
Browse files Browse the repository at this point in the history 
Rather than initializing internally.
  • Loading branch information
@jacobperron
jacobperron committed Nov 16, 2018
1 parent 06df1fd commit 4eb7fdb
Show file tree
Hide file tree
Showing 4 changed files with 61 additions and 25 deletions.
5 changes: 5 additions & 0 deletions rcl_action/include/rcl_action/action_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,9 @@ rcl_action_get_zero_initialized_server(void);
* The given rcl_node_t must be valid and the resulting rcl_action_server_t is
* only valid as long as the given rcl_node_t remains valid.
*
* The give rcl_clock_t must be valid and the resulting rcl_ction_server_t is
* only valid as long ast he given rcl_clock_t remains valid.
*
* The rosidl_action_type_support_t is obtained on a per .action type basis.
* When the user defines a ROS action, code is generated which provides the
* required rosidl_action_type_support_t object.
Expand Down Expand Up @@ -153,6 +156,7 @@ rcl_action_get_zero_initialized_server(void);
* \param[out] action_server handle to a preallocated, zero-initialized action server structure
* to be initialized.
* \param[in] node valid node handle
* \param[in] clock valid clock handle
* \param[in] type_support type support object for the action's type
* \param[in] action_name the name of the action
* \param[in] options action_server options, including quality of service settings
Expand All @@ -169,6 +173,7 @@ rcl_ret_t
rcl_action_server_init(
rcl_action_server_t * action_server,
rcl_node_t * node,
rcl_clock_t * clock,
const rosidl_action_type_support_t * type_support,
const char * action_name,
const rcl_action_server_options_t * options);
Expand Down
22 changes: 10 additions & 12 deletions rcl_action/src/rcl_action/action_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,6 +129,7 @@ rcl_ret_t
rcl_action_server_init(
rcl_action_server_t * action_server,
rcl_node_t * node,
rcl_clock_t * clock,
const rosidl_action_type_support_t * type_support,
const char * action_name,
const rcl_action_server_options_t * options)
Expand All @@ -137,6 +138,10 @@ rcl_action_server_init(
if (!rcl_node_is_valid(node)) {
return RCL_RET_NODE_INVALID; // error already set
}
if (!rcl_clock_valid(clock)) {
RCL_SET_ERROR_MSG("invalid clock");
return RCL_RET_INVALID_ARGUMENT;
}
RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(action_name, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
Expand All @@ -156,13 +161,7 @@ rcl_action_server_init(
RCL_CHECK_FOR_NULL_WITH_MSG(
action_server->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);

// Initialize clock
rcl_ret_t ret = rcl_clock_init(options->clock_type, &action_server->impl->clock, &allocator);
if (RCL_RET_OK != ret) {
ret = RCL_RET_ERROR;
goto fail;
}

rcl_ret_t ret = RCL_RET_OK;
// Initialize services
SERVICE_INIT(goal);
SERVICE_INIT(cancel);
Expand All @@ -172,6 +171,9 @@ rcl_action_server_init(
PUBLISHER_INIT(feedback);
PUBLISHER_INIT(status);

// Copy clock
action_server->impl->clock = *clock;

// Copy action name
action_server->impl->action_name = rcutils_strdup(action_name, allocator);
if (NULL == action_server->impl->action_name) {
Expand All @@ -186,7 +188,7 @@ rcl_action_server_init(
action_server->impl->goal_handles = NULL;
action_server->impl->num_goal_handles = 0;

return RCL_RET_OK;
return ret;
fail:
{
// Finalize any services/publishers that were initialized and deallocate action_server->impl
Expand All @@ -208,10 +210,6 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node)

rcl_ret_t ret = RCL_RET_OK;
if (action_server->impl) {
// Finalize clock
if (rcl_clock_fini(&action_server->impl->clock) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
}
// Finalize services
if (rcl_service_fini(&action_server->impl->goal_service, node) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
Expand Down
10 changes: 8 additions & 2 deletions rcl_action/test/rcl_action/test_action_communication.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,18 +33,21 @@ class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing
protected:
void SetUp() override
{
rcl_ret_t ret = rcl_init(0, nullptr, rcl_get_default_allocator());
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, ts, action_name, &options);
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;
}

Expand All @@ -53,6 +56,8 @@ class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing
// 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();
Expand All @@ -75,6 +80,7 @@ class CLASSNAME (TestActionCommunication, RMW_IMPLEMENTATION) : public ::testing

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)
Expand Down
49 changes: 38 additions & 11 deletions rcl_action/test/rcl_action/test_action_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,55 +25,71 @@

TEST(TestActionServerInitFini, test_action_server_init_fini)
{
rcl_ret_t ret = rcl_init(0, nullptr, rcl_get_default_allocator());
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, ts, action_name, &options);
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, ts, action_name, &options);
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, ts, action_name, &options);
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, nullptr, action_name, &options);
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, ts, nullptr, &options);
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 a null options
ret = rcl_action_server_init(&action_server, &node, ts, action_name, nullptr);
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, ts, action_name, &options);
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, ts, action_name, &options);
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();

Expand Down Expand Up @@ -104,6 +120,10 @@ TEST(TestActionServerInitFini, test_action_server_init_fini)
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;
Expand All @@ -117,18 +137,22 @@ class TestActionServer : public ::testing::Test
protected:
void SetUp() override
{
rcl_ret_t ret = rcl_init(0, nullptr, rcl_get_default_allocator());
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, ts, action_name, &options);
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;
}

Expand All @@ -137,6 +161,8 @@ class TestActionServer : public ::testing::Test
// 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();
Expand All @@ -159,6 +185,7 @@ class TestActionServer : public ::testing::Test

rcl_action_server_t action_server;
rcl_node_t node;
rcl_clock_t clock;
}; // class TestActionServer

TEST_F(TestActionServer, test_action_server_is_valid)
Expand Down

0 comments on commit 4eb7fdb

Please sign in to comment.