Skip to content

Commit

Permalink
Implement send/take functions for action server services
Browse files Browse the repository at this point in the history
  • Loading branch information
@jacobperron
jacobperron committed Nov 9, 2018
1 parent 47d6833 commit c33d94b
Show file tree
Hide file tree
Showing 4 changed files with 245 additions and 18 deletions.
4 changes: 4 additions & 0 deletions rcl_action/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ find_package(rosidl_generator_c REQUIRED)
find_package(action_msgs REQUIRED)
find_package(rcl REQUIRED)
find_package(rcutils REQUIRED)
find_package(rmw REQUIRED)

include_directories(
include
Expand Down Expand Up @@ -53,6 +54,7 @@ add_library(${PROJECT_NAME}
ament_target_dependencies(${PROJECT_NAME}
"rosidl_generator_c"
"action_msgs"
"rmw"
"rcutils"
"rcl"
)
Expand Down Expand Up @@ -145,6 +147,8 @@ ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(ament_cmake)
ament_export_dependencies(rcl)
ament_export_dependencies(rcutils)
ament_export_dependencies(rmw)
ament_export_dependencies(action_msgs)
ament_export_dependencies(rosidl_generator_c)
ament_package()
6 changes: 3 additions & 3 deletions rcl_action/include/rcl_action/action_server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -312,7 +312,7 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_send_goal_response(
const rcl_action_server_t * action_server,
const void * ros_goal_response);
void * ros_goal_response);

/// Accept a new goal using a rcl_action_server_t.
/**
Expand Down Expand Up @@ -542,7 +542,7 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_send_result_response(
const rcl_action_server_t * action_server,
const void * ros_result_response);
void * ros_result_response);

/// Clear all expired goals associated with a rcl_action_server_t.
/**
Expand Down Expand Up @@ -682,7 +682,7 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_send_cancel_response(
const rcl_action_server_t * action_server,
const void * ros_cancel_response);
void * ros_cancel_response);

/// Get the name of the action for a rcl_action_server_t.
/**
Expand Down
58 changes: 43 additions & 15 deletions rcl_action/src/rcl_action/action_server.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@ extern "C"
#include "rcutils/logging_macros.h"
#include "rcutils/strdup.h"

#include "rmw/rmw.h"

/// Internal rcl_action implementation struct.
typedef struct rcl_action_server_impl_t
{
Expand Down Expand Up @@ -236,22 +238,52 @@ rcl_action_server_get_default_options(void)
return default_options;
}

#define TAKE_SERVICE_REQUEST(Type) \
if (!rcl_action_server_is_valid(action_server)) { \
return RCL_RET_ACTION_SERVER_INVALID; /* error already set */ \
} \
RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type ## _request, RCL_RET_INVALID_ARGUMENT); \
rmw_request_id_t request_header; /* ignored */ \
rcl_ret_t ret = rcl_take_request( \
&action_server->impl->Type ## _service, &request_header, ros_ ## Type ## _request); \
if (RCL_RET_OK != ret) { \
if (RCL_RET_BAD_ALLOC == ret) { \
return RCL_RET_BAD_ALLOC; /* error already set */ \
} \
if (RCL_RET_SERVICE_TAKE_FAILED == ret) { \
return RCL_RET_ACTION_SERVER_TAKE_FAILED; \
} \
return RCL_RET_ERROR; /* error already set */ \
} \
return RCL_RET_OK; \

#define SEND_SERVICE_RESPONSE(Type) \
if (!rcl_action_server_is_valid(action_server)) { \
return RCL_RET_ACTION_SERVER_INVALID; /* error already set */ \
} \
RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type ## _response, RCL_RET_INVALID_ARGUMENT); \
rmw_request_id_t request_header; /* ignored */ \
rcl_ret_t ret = rcl_send_response( \
&action_server->impl->Type ## _service, &request_header, ros_ ## Type ## _response); \
if (RCL_RET_OK != ret) { \
return RCL_RET_ERROR; /* error already set */ \
} \
return RCL_RET_OK; \

rcl_ret_t
rcl_action_take_goal_request(
const rcl_action_server_t * action_server,
void * ros_goal_request)
{
// TODO(jacobperron): impl
return RCL_RET_OK;
TAKE_SERVICE_REQUEST(goal);
}

rcl_ret_t
rcl_action_send_goal_response(
const rcl_action_server_t * action_server,
const void * ros_goal_response)
void * ros_goal_response)
{
// TODO(jacobperron): impl
return RCL_RET_OK;
SEND_SERVICE_RESPONSE(goal);
}

rcl_action_goal_handle_t *
Expand Down Expand Up @@ -365,17 +397,15 @@ rcl_action_take_result_request(
const rcl_action_server_t * action_server,
void * ros_result_request)
{
// TODO(jacobperron): impl
return RCL_RET_OK;
TAKE_SERVICE_REQUEST(result);
}

rcl_ret_t
rcl_action_send_result_response(
const rcl_action_server_t * action_server,
const void * ros_result_response)
void * ros_result_response)
{
// TODO(jacobperron): impl
return RCL_RET_OK;
SEND_SERVICE_RESPONSE(result);
}

rcl_ret_t
Expand All @@ -392,8 +422,7 @@ rcl_action_take_cancel_request(
const rcl_action_server_t * action_server,
void * ros_cancel_request)
{
// TODO(jacobperron): impl
return RCL_RET_OK;
TAKE_SERVICE_REQUEST(cancel);
}

rcl_ret_t
Expand All @@ -409,10 +438,9 @@ rcl_action_process_cancel_request(
rcl_ret_t
rcl_action_send_cancel_response(
const rcl_action_server_t * action_server,
const void * ros_cancel_response)
void * ros_cancel_response)
{
// TODO(jacobperron): impl
return RCL_RET_OK;
SEND_SERVICE_RESPONSE(cancel);
}

const char *
Expand Down
195 changes: 195 additions & 0 deletions rcl_action/test/rcl_action/test_action_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -327,3 +327,198 @@ TEST_F(TestActionServer, test_action_server_get_options)
options = rcl_action_server_get_options(&this->action_server);
EXPECT_NE(options, nullptr) << rcl_get_error_string().str;
}

#ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)
#else
# define CLASSNAME(NAME, SUFFIX) NAME
#endif

class CLASSNAME (TestActionServerComm, RMW_IMPLEMENTATION) : public TestActionServer
{
};

TEST_F(CLASSNAME(TestActionServerComm, 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(TestActionServerComm, 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(TestActionServerComm, 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(TestActionServerComm, 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(TestActionServerComm, 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(TestActionServerComm, 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(TestActionServerComm, RMW_IMPLEMENTATION), test_publish_feedback)
{
// TODO(jacobperron): write test
}

TEST_F(CLASSNAME(TestActionServerComm, RMW_IMPLEMENTATION), test_publish_status)
{
// TODO(jacobperron): write test
}

0 comments on commit c33d94b

Please sign in to comment.