From 58c7676155e9d3d61161f29bd2bce089ce68b21d Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 20 Nov 2018 14:47:18 -0800 Subject: [PATCH 1/2] [rcl_action] Implement wait set functions for action server --- rcl_action/src/rcl_action/action_server.c | 96 +++++++++++++++++++++++ 1 file changed, 96 insertions(+) diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index ef9bd6fd8..768d11bc5 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/rcl_action/src/rcl_action/action_server.c @@ -23,6 +23,7 @@ extern "C" #include "rcl_action/goal_handle.h" #include "rcl_action/names.h" #include "rcl_action/types.h" +#include "rcl_action/wait.h" #include "rcl/error_handling.h" #include "rcl/rcl.h" @@ -48,6 +49,10 @@ typedef struct rcl_action_server_impl_t size_t num_goal_handles; // Clock rcl_clock_t clock; + // Wait set records + size_t wait_set_goal_service_index; + size_t wait_set_cancel_service_index; + size_t wait_set_result_service_index; } rcl_action_server_impl_t; rcl_action_server_t @@ -783,6 +788,97 @@ rcl_action_server_is_valid(const rcl_action_server_t * action_server) return true; } +rcl_ret_t +rcl_action_wait_set_add_action_server( + rcl_wait_set_t * wait_set, + const rcl_action_server_t * action_server, + size_t * service_index) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID); + if (!rcl_action_server_is_valid(action_server)) { + return RCL_RET_ACTION_SERVER_INVALID; // error already set + } + + rcl_ret_t ret = rcl_wait_set_add_service( + wait_set, + &action_server->impl->goal_service, + &action_server->impl->wait_set_goal_service_index); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_wait_set_add_service( + wait_set, + &action_server->impl->cancel_service, + &action_server->impl->wait_set_cancel_service_index); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_wait_set_add_service( + wait_set, + &action_server->impl->result_service, + &action_server->impl->wait_set_result_service_index); + if (RCL_RET_OK != ret) { + return ret; + } + + if (NULL != service_index) { + // The goal service was the first added + *service_index = action_server->impl->wait_set_goal_service_index; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_action_server_wait_set_get_num_entities( + const rcl_action_server_t * action_server, + size_t * num_subscriptions, + size_t * num_guard_conditions, + size_t * num_timers, + size_t * num_clients, + size_t * num_services) +{ + if (!rcl_action_server_is_valid(action_server)) { + return RCL_RET_ACTION_SERVER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(num_subscriptions, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(num_guard_conditions, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(num_timers, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(num_clients, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(num_services, RCL_RET_INVALID_ARGUMENT); + *num_subscriptions = 0u; + *num_guard_conditions = 0u; + *num_timers = 0u; + *num_clients = 0u; + *num_services = 3u; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_action_server_wait_set_get_entities_ready( + const rcl_wait_set_t * wait_set, + const rcl_action_server_t * action_server, + bool * is_goal_request_ready, + bool * is_cancel_request_ready, + bool * is_result_request_ready) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID); + if (!rcl_action_server_is_valid(action_server)) { + return RCL_RET_ACTION_SERVER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(is_goal_request_ready, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_cancel_request_ready, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_result_request_ready, RCL_RET_INVALID_ARGUMENT); + + const rcl_action_server_impl_t * impl = action_server->impl; + const rcl_service_t * goal_service = wait_set->services[impl->wait_set_goal_service_index]; + const rcl_service_t * cancel_service = wait_set->services[impl->wait_set_cancel_service_index]; + const rcl_service_t * result_service = wait_set->services[impl->wait_set_result_service_index]; + *is_goal_request_ready = (&impl->goal_service == goal_service); + *is_cancel_request_ready = (&impl->cancel_service == cancel_service); + *is_result_request_ready = (&impl->result_service == result_service); + return RCL_RET_OK; +} + #ifdef __cplusplus } #endif From 1b9526afdda9027b511e2f6f904e317b141a1479 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 20 Nov 2018 15:16:10 -0800 Subject: [PATCH 2/2] [rcl_action] Clean up CMakeLists.txt --- rcl_action/CMakeLists.txt | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt index 49922a14c..def67842c 100644 --- a/rcl_action/CMakeLists.txt +++ b/rcl_action/CMakeLists.txt @@ -53,11 +53,11 @@ add_library(${PROJECT_NAME} ) ament_target_dependencies(${PROJECT_NAME} - "rosidl_generator_c" "action_msgs" - "rmw" - "rcutils" "rcl" + "rcutils" + "rmw" + "rosidl_generator_c" ) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. @@ -75,7 +75,6 @@ install(TARGETS ${PROJECT_NAME} ) if(BUILD_TESTING) - find_package(test_msgs REQUIRED) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) find_package(test_msgs REQUIRED) @@ -101,7 +100,6 @@ if(BUILD_TESTING) target_include_directories(test_action_communication PUBLIC include ${rcl_INCLUDE_DIRS} - ${test_msgs_INCLUDE_DIRS} ) target_link_libraries(test_action_communication ${PROJECT_NAME} @@ -115,7 +113,6 @@ if(BUILD_TESTING) target_include_directories(test_action_server PUBLIC include ${rcl_INCLUDE_DIRS} - ${test_msgs_INCLUDE_DIRS} ) target_link_libraries(test_action_server ${PROJECT_NAME}