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

41
rcl_action/CMakeLists.txt
View file

@ -4,8 +4,11 @@ project(rcl_action)
find_package(ament_cmake_ros REQUIRED) find_package(ament_cmake_ros REQUIRED)
find_package(rosidl_generator_c REQUIRED)
find_package(action_msgs REQUIRED) find_package(action_msgs REQUIRED)
find_package(rcl REQUIRED) find_package(rcl REQUIRED)
find_package(rcutils REQUIRED)
find_package(rmw REQUIRED)
include_directories( include_directories(
include include
@ -33,6 +36,7 @@ add_executable(test_compile_headers
set(rcl_action_sources set(rcl_action_sources
src/${PROJECT_NAME}/action_client.c src/${PROJECT_NAME}/action_client.c
src/${PROJECT_NAME}/action_server.c
src/${PROJECT_NAME}/goal_handle.c src/${PROJECT_NAME}/goal_handle.c
src/${PROJECT_NAME}/goal_state_machine.c src/${PROJECT_NAME}/goal_state_machine.c
src/${PROJECT_NAME}/names.c src/${PROJECT_NAME}/names.c
@ -49,8 +53,11 @@ add_library(${PROJECT_NAME}
) )
ament_target_dependencies(${PROJECT_NAME} ament_target_dependencies(${PROJECT_NAME}
"rcl" "rosidl_generator_c"
"action_msgs" "action_msgs"
"rmw"
"rcutils"
"rcl"
) )
# Causes the visibility macros to use dllexport rather than dllimport, # Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it. # which is appropriate when building the dll but not consuming it.
@ -68,6 +75,7 @@ install(TARGETS ${PROJECT_NAME}
) )
if(BUILD_TESTING) if(BUILD_TESTING)
find_package(test_msgs REQUIRED)
find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED) find_package(ament_lint_auto REQUIRED)
find_package(test_msgs REQUIRED) find_package(test_msgs REQUIRED)
@ -86,6 +94,34 @@ if(BUILD_TESTING)
) )
ament_target_dependencies(test_action_client "rcl" "test_msgs") ament_target_dependencies(test_action_client "rcl" "test_msgs")
endif() endif()
ament_add_gtest(test_action_communication
test/rcl_action/test_action_communication.cpp
)
if(TARGET test_action_communication)
target_include_directories(test_action_communication PUBLIC
include
${rcl_INCLUDE_DIRS}
${test_msgs_INCLUDE_DIRS}
)
target_link_libraries(test_action_communication
${PROJECT_NAME}
)
ament_target_dependencies(test_action_communication "test_msgs")
endif()
ament_add_gtest(test_action_server
test/rcl_action/test_action_server.cpp
)
if(TARGET test_action_server)
target_include_directories(test_action_server PUBLIC
include
${rcl_INCLUDE_DIRS}
${test_msgs_INCLUDE_DIRS}
)
target_link_libraries(test_action_server
${PROJECT_NAME}
)
ament_target_dependencies(test_action_server "test_msgs")
endif()
ament_add_gtest(test_goal_handle ament_add_gtest(test_goal_handle
test/rcl_action/test_goal_handle.cpp test/rcl_action/test_goal_handle.cpp
) )
@ -139,5 +175,8 @@ ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME}) ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(ament_cmake) ament_export_dependencies(ament_cmake)
ament_export_dependencies(rcl) ament_export_dependencies(rcl)
ament_export_dependencies(rcutils)
ament_export_dependencies(rmw)
ament_export_dependencies(action_msgs) ament_export_dependencies(action_msgs)
ament_export_dependencies(rosidl_generator_c)
ament_package() ament_package()

207
rcl_action/include/rcl_action/action_server.h
View file

@ -27,6 +27,7 @@ extern "C"
#include "rcl/node.h" #include "rcl/node.h"
#include "rcl/time.h" #include "rcl/time.h"
#include "rosidl_generator_c/action_type_support_struct.h"
/// Internal rcl_action implementation struct. /// Internal rcl_action implementation struct.
struct rcl_action_server_impl_t; struct rcl_action_server_impl_t;
@ -42,13 +43,15 @@ typedef struct rcl_action_server_options_t
{ {
/// Middleware quality of service settings for the action server. /// Middleware quality of service settings for the action server.
rmw_qos_profile_t goal_service_qos; rmw_qos_profile_t goal_service_qos;
rmw_qos_profile_t result_service_qos;
rmw_qos_profile_t cancel_service_qos; rmw_qos_profile_t cancel_service_qos;
rmw_qos_profile_t result_service_qos;
rmw_qos_profile_t feedback_topic_qos; rmw_qos_profile_t feedback_topic_qos;
rmw_qos_profile_t status_topic_qos; rmw_qos_profile_t status_topic_qos;
/// Custom allocator for the action server, used for incidental allocations. /// Custom allocator for the action server, used for incidental allocations.
/** For default behavior (malloc/free), see: rcl_get_default_allocator() */ /** For default behavior (malloc/free), see: rcl_get_default_allocator() */
rcl_allocator_t allocator; rcl_allocator_t allocator;
/// Clock type used for checking result timeouts.
rcl_clock_type_t clock_type;
/// Goal handles that have results longer than this time are deallocated. /// Goal handles that have results longer than this time are deallocated.
rcl_duration_t result_timeout; rcl_duration_t result_timeout;
} rcl_action_server_options_t; } rcl_action_server_options_t;
@ -63,7 +66,7 @@ RCL_WARN_UNUSED
rcl_action_server_t rcl_action_server_t
rcl_action_get_zero_initialized_server(void); rcl_action_get_zero_initialized_server(void);
/// Initialize a rcl_action_server_t. /// Initialize an action server.
/** /**
* After calling this function on a rcl_action_server_t, it can be used to take * After calling this function on a rcl_action_server_t, it can be used to take
* goals of the given type for the given action name using rcl_action_take_goal_request() * goals of the given type for the given action name using rcl_action_take_goal_request()
@ -78,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 * 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. * 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. * 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 * When the user defines a ROS action, code is generated which provides the
* required rosidl_action_type_support_t object. * required rosidl_action_type_support_t object.
@ -147,9 +153,10 @@ rcl_action_get_zero_initialized_server(void);
* Uses Atomics | No * Uses Atomics | No
* Lock-Free | Yes * Lock-Free | Yes
* *
* \param[out] action_server a preallocated, zero-initialized action server structure * \param[out] action_server handle to a preallocated, zero-initialized action server structure
* to be initialized. * to be initialized.
* \param[in] node valid rcl node handle * \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] type_support type support object for the action's type
* \param[in] action_name the name of the action * \param[in] action_name the name of the action
* \param[in] options action_server options, including quality of service settings * \param[in] options action_server options, including quality of service settings
@ -165,14 +172,15 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_action_server_init( rcl_action_server_init(
rcl_action_server_t * action_server, rcl_action_server_t * action_server,
const rcl_node_t * node, rcl_node_t * node,
rcl_clock_t * clock,
const rosidl_action_type_support_t * type_support, const rosidl_action_type_support_t * type_support,
const char * action_name, const char * action_name,
const rcl_action_server_options_t * options); const rcl_action_server_options_t * options);
/// Finalize a rcl_action_server_t. /// Finalize an action server.
/** /**
* After calling, the node will no longer listen for goals for this action server. * After calling, the node will no longer listen to services and topics for this action server.
* (assuming this is the only action server of this type in this node). * (assuming this is the only action server of this type in this node).
* *
* After calling, calls to rcl_wait(), rcl_action_take_goal_request(), * After calling, calls to rcl_wait(), rcl_action_take_goal_request(),
@ -193,7 +201,7 @@ rcl_action_server_init(
* \param[inout] action_server handle to the action_server to be deinitialized * \param[inout] action_server handle to the action_server to be deinitialized
* \param[in] node handle to the node used to create the action server * \param[in] node handle to the node used to create the action server
* \return `RCL_RET_OK` if the action server was deinitialized successfully, or * \return `RCL_RET_OK` if the action server was deinitialized successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_ACTION_SERVER_INVALID` if the action server pointer is null, or
* \return `RCL_RET_NODE_INVALID` if the node is invalid, or * \return `RCL_RET_NODE_INVALID` if the node is invalid, or
* \return `RCL_RET_ERROR` if an unspecified error occurs. * \return `RCL_RET_ERROR` if an unspecified error occurs.
*/ */
@ -207,19 +215,19 @@ rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node);
* The defaults are: * The defaults are:
* *
* - goal_service_qos = rmw_qos_profile_services_default; * - goal_service_qos = rmw_qos_profile_services_default;
* - result_service_qos = rmw_qos_profile_services_default;
* - cancel_service_qos = rmw_qos_profile_services_default; * - cancel_service_qos = rmw_qos_profile_services_default;
* - result_service_qos = rmw_qos_profile_services_default;
* - feedback_topic_qos = rmw_qos_profile_default; * - feedback_topic_qos = rmw_qos_profile_default;
* - status_topic_qos = rcl_action_qos_profile_status_default; * - status_topic_qos = rcl_action_qos_profile_status_default;
* - allocator = rcl_get_default_allocator() * - allocator = rcl_get_default_allocator();
- result_timeout = 9e+11; // 15 minutes * - result_timeout = RCUTILS_S_TO_NS(15 * 60); // 15 minutes
*/ */
RCL_ACTION_PUBLIC RCL_ACTION_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_action_server_options_t rcl_action_server_options_t
rcl_action_server_get_default_options(void); rcl_action_server_get_default_options(void);
/// Take a pending ROS goal using a rcl_action_server_t. /// Take a pending ROS goal using an action server.
/** /**
* \todo TODO(jacobperron) blocking of take? * \todo TODO(jacobperron) blocking of take?
* *
@ -249,7 +257,7 @@ rcl_action_server_get_default_options(void);
* Lock-Free | Yes * Lock-Free | Yes
* <i>[1] only if required when filling the request, avoided for fixed sizes</i> * <i>[1] only if required when filling the request, avoided for fixed sizes</i>
* *
* \param[in] action_server the action server that will take the request * \param[in] action_server handle to the action server that will take the request
* \param[out] ros_goal_request a preallocated, zero-initialized, ROS goal request message * \param[out] ros_goal_request a preallocated, zero-initialized, ROS goal request message
* where the request is copied * where the request is copied
* \return `RCL_RET_OK` if the request was taken, or * \return `RCL_RET_OK` if the request was taken, or
@ -267,7 +275,7 @@ rcl_action_take_goal_request(
const rcl_action_server_t * action_server, const rcl_action_server_t * action_server,
void * ros_goal_request); void * ros_goal_request);
/// Send a response for a goal request to an action client using a rcl_action_server_t. /// Send a response for a goal request to an action client using an action server.
/** /**
* This is a non-blocking call. * This is a non-blocking call.
* *
@ -311,19 +319,27 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_action_send_goal_response( rcl_action_send_goal_response(
const rcl_action_server_t * action_server, 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. /// Accept a new goal using an action server.
/** /**
* This is a non-blocking call. * This is a non-blocking call.
* *
* Creates and returns a pointer to a goal handle for a newly accepted goal. * Creates and returns a new goal handle.
* The action server starts tracking it internally.
* If a failure occurs, `NULL` is returned and an error message is set. * If a failure occurs, `NULL` is returned and an error message is set.
* Possible reasons for failure:
* - action server is invalid
* - goal info is invalid
* - goal ID is already being tracked by the action server
* - memory allocation failure
* *
* This function should be called after receiving a new goal request with * This function should be called after receiving a new goal request with
* rcl_action_take_goal_request() and before sending a response with * rcl_action_take_goal_request() and before sending a response with
* rcl_action_send_goal_response(). * rcl_action_send_goal_response().
* *
* After calling this function, the action server will start tracking the goal.
*
* Example usage: * Example usage:
* *
* ```c * ```c
@ -354,9 +370,8 @@ rcl_action_send_goal_response(
* Uses Atomics | No * Uses Atomics | No
* Lock-Free | Yes * Lock-Free | Yes
* *
* \param[in] action_server the action server that is accepting the goal * \param[in] action_server handle to the action server that is accepting the goal
* \param[in] goal_info a message containing info about the goal being accepted * \param[in] goal_info a message containing info about the goal being accepted
* \param[in] error_msg_allocator a valid allocator or `NULL`
* \return a pointer to a new goal handle representing the accepted goal, or * \return a pointer to a new goal handle representing the accepted goal, or
* \return `NULL` if a failure occured. * \return `NULL` if a failure occured.
*/ */
@ -364,11 +379,10 @@ RCL_ACTION_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_action_goal_handle_t * rcl_action_goal_handle_t *
rcl_action_accept_new_goal( rcl_action_accept_new_goal(
const rcl_action_server_t * action_server, rcl_action_server_t * action_server,
const rcl_action_goal_info_t * goal_info, const rcl_action_goal_info_t * goal_info);
rcl_allocator_t * error_msg_allocator);
/// Publish a ROS feedback message for an active goal using a rcl_action_server_t. /// Publish a ROS feedback message for an active goal using an action server.
/** /**
* The caller is responsible for ensuring that the type of `ros_feedback` * The caller is responsible for ensuring that the type of `ros_feedback`
* and the type associate with the client (via the type support) match. * and the type associate with the client (via the type support) match.
@ -416,7 +430,7 @@ rcl_action_publish_feedback(
const rcl_action_server_t * action_server, const rcl_action_server_t * action_server,
void * ros_feedback); void * ros_feedback);
/// Get a status array message for accepted goals associated with a rcl_action_server_t. /// Get a status array message for accepted goals associated with an action server.
/** /**
* The provided `status_message` should be zero-initialized with * The provided `status_message` should be zero-initialized with
* rcl_action_get_zero_initialized_goal_status_array() before calling this function. * rcl_action_get_zero_initialized_goal_status_array() before calling this function.
@ -443,9 +457,9 @@ rcl_action_get_goal_status_array(
const rcl_action_server_t * action_server, const rcl_action_server_t * action_server,
rcl_action_goal_status_array_t * status_message); rcl_action_goal_status_array_t * status_message);
/// Publish a status array message for accepted goals associated with a rcl_action_server_t. /// Publish a status array message for accepted goals associated with an action server.
/** /**
* This function acts like a ROS publisher and is potntially a blocking call. * This function acts like a ROS publisher and is potentially a blocking call.
* \see rcl_publish() * \see rcl_publish()
* *
* A status array message associated with the action server can be created with * A status array message associated with the action server can be created with
@ -471,9 +485,9 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_action_publish_status( rcl_action_publish_status(
const rcl_action_server_t * action_server, const rcl_action_server_t * action_server,
const rcl_action_goal_status_array_t * status_message); const void * status_message);
/// Take a pending result request using a rcl_action_server_t. /// Take a pending result request using an action server.
/** /**
* \todo TODO(jacobperron) blocking of take? * \todo TODO(jacobperron) blocking of take?
* *
@ -511,7 +525,7 @@ rcl_action_take_result_request(
const rcl_action_server_t * action_server, const rcl_action_server_t * action_server,
void * ros_result_request); void * ros_result_request);
/// Send a result response using a rcl_action_server_t. /// Send a result response using an action server.
/** /**
* This is a non-blocking call. * This is a non-blocking call.
* *
@ -543,38 +557,54 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_action_send_result_response( rcl_action_send_result_response(
const rcl_action_server_t * action_server, 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. /// Expires goals associated with an action server.
/** /**
* A goal is 'expired' if it has been in a terminal state (has a result) for more * A goal is 'expired' if it has been in a terminal state (has a result) for longer
* than some duration. * than some duration.
* The timeout duration is set as part of the action server options. * The timeout duration is set as part of the action server options.
* *
* If a negative timeout value if provided, then goal results never expire (kept forever).
* If a timeout of zero is set, then goal results are discarded immediately (ie. goal
* results are discarded whenever this function is called).
*
* Expired goals are removed from the internal array of goal handles.
* rcl_action_server_goal_exists() will return false for any goals that have expired.
*
* \attention If one or more goals are expired then a previously returned goal handle
* array from rcl_action_server_get_goal_handles() becomes invalid.
*
* `num_expired` is an optional argument. If it is not `NULL`, then it is set to the
* number of goals that were expired.
*
* <hr> * <hr>
* Attribute | Adherence * Attribute | Adherence
* ------------------ | ------------- * ------------------ | -------------
* Allocates Memory | No * Allocates Memory | Maybe[1]
* Thread-Safe | No * Thread-Safe | No
* Uses Atomics | No * Uses Atomics | No
* Lock-Free | Yes * Lock-Free | Yes
* <i>[1] if one or more goals expires, then the internal goal handle array may be
* resized or deallocated</i>
* *
* \param[in] action_server handle to the action server from which expired goals * \param[in] action_server handle to the action server from which expired goals
* will be cleared. * will be cleared.
* \param[out] num_expired the number of expired goals cleared. If `NULL` then the * \param[out] num_expired the number of expired goals, or set to `NULL` if unused
* number is not set.
* \return `RCL_RET_OK` if the response was sent successfully, or * \return `RCL_RET_OK` if the response was sent successfully, or
* \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or * \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
* \return `RCL_RET_ERROR` if an unspecified error occurs. * \return `RCL_RET_ERROR` if an unspecified error occurs.
*/ */
RCL_ACTION_PUBLIC RCL_ACTION_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_action_clear_expired_goals( rcl_action_expire_goals(
const rcl_action_server_t * action_server, const rcl_action_server_t * action_server,
uint32_t * num_expired); size_t * num_expired);
/// Take a pending cancel request using a rcl_action_server_t. /// Take a pending cancel request using an action server.
/** /**
* \todo TODO(jacobperron) blocking of take? * \todo TODO(jacobperron) blocking of take?
* *
@ -615,7 +645,7 @@ rcl_action_take_cancel_request(
const rcl_action_server_t * action_server, const rcl_action_server_t * action_server,
void * ros_cancel_request); void * ros_cancel_request);
/// Process a cancel request using a rcl_action_server_t. /// Process a cancel request using an action server.
/** /**
* This is a non-blocking call. * This is a non-blocking call.
* *
@ -641,7 +671,7 @@ rcl_action_take_cancel_request(
* *
* \param[in] action_server handle to the action server that will process the cancel request * \param[in] action_server handle to the action server that will process the cancel request
* \param[in] cancel_request a C-typed ROS cancel request to process * \param[in] cancel_request a C-typed ROS cancel request to process
* \param[out] cancel_reponse a preallocated, zero-initialized, C-typed ROS cancel response * \param[out] cancel_reponse a zero-initialized cancel response struct
* where the response is copied * where the response is copied
* \return `RCL_RET_OK` if the response was sent successfully, or * \return `RCL_RET_OK` if the response was sent successfully, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@ -659,7 +689,7 @@ rcl_action_process_cancel_request(
const rcl_action_cancel_request_t * cancel_request, const rcl_action_cancel_request_t * cancel_request,
rcl_action_cancel_response_t * cancel_response); rcl_action_cancel_response_t * cancel_response);
/// Send a cancel response using a rcl action server. /// Send a cancel response using an action server.
/** /**
* This is a non-blocking call. * This is a non-blocking call.
* *
@ -671,7 +701,7 @@ rcl_action_process_cancel_request(
* Uses Atomics | No * Uses Atomics | No
* Lock-Free | Yes * Lock-Free | Yes
* *
* \param[in] action_server the handle to the action server that will send the cancel response * \param[in] action_server handle to the action server that will send the cancel response
* \param[in] ros_cancel_response a ROS cancel response to send * \param[in] ros_cancel_response a ROS cancel response to send
* \return `RCL_RET_OK` if the request was taken, or * \return `RCL_RET_OK` if the request was taken, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@ -683,14 +713,14 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_action_send_cancel_response( rcl_action_send_cancel_response(
const rcl_action_server_t * action_server, 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. /// Get the action name for an action server.
/** /**
* This function returns the action server's internal topic name string. * This function returns the action server's internal topic name string.
* This function can fail, and therefore return `NULL`, if the: * This function can fail, and therefore return `NULL`, if the:
* - action server is `NULL` * - action server is `NULL`
* - action server is invalid (never called init, called fini, or invalid) * - action server is invalid (e.g. never called init or called fini)
* *
* The returned string is only valid as long as the action server is valid. * The returned string is only valid as long as the action server is valid.
* The value of the string may change if the topic name changes, and therefore * The value of the string may change if the topic name changes, and therefore
@ -704,20 +734,21 @@ rcl_action_send_cancel_response(
* Uses Atomics | No * Uses Atomics | No
* Lock-Free | Yes * Lock-Free | Yes
* *
* \param[in] action_server the pointer to the action server * \param[in] action_server pointer to the action server
* \return name string if successful, otherwise `NULL` * \return name string if successful, or
* \return `NULL` otherwise.
*/ */
RCL_ACTION_PUBLIC RCL_ACTION_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
const char * const char *
rcl_action_server_get_action_name(const rcl_action_server_t * action_server); rcl_action_server_get_action_name(const rcl_action_server_t * action_server);
/// Return the rcl_action_server_options_t for a rcl_action_server_t. /// Return the rcl_action_server_options_t for an action server.
/** /**
* This function returns the action server's internal options struct. * This function returns the action server's internal options struct.
* This function can fail, and therefore return `NULL`, if the: * This function can fail, and therefore return `NULL`, if the:
* - action server is `NULL` * - action server is `NULL`
* - action server is invalid (never called init, called fini, or invalid) * - action server is invalid (e.g. never called init or called fini)
* *
* The returned struct is only valid as long as the action server is valid. * The returned struct is only valid as long as the action server is valid.
* The values in the struct may change if the action server's options change, * The values in the struct may change if the action server's options change,
@ -731,27 +762,23 @@ rcl_action_server_get_action_name(const rcl_action_server_t * action_server);
* Uses Atomics | No * Uses Atomics | No
* Lock-Free | Yes * Lock-Free | Yes
* *
* \param[in] action_server pointer to the action server * \param[in] action_server handle to the action server
* \return options struct if successful, otherwise `NULL` * \return options struct if successful, or
* \return `NULL` otherwise.
*/ */
RCL_ACTION_PUBLIC RCL_ACTION_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
const rcl_action_server_options_t * const rcl_action_server_options_t *
rcl_action_server_get_options(const rcl_action_server_t * action_server); rcl_action_server_get_options(const rcl_action_server_t * action_server);
/// Return the goal handles for all active or terminated goals. /// Get the goal handles for all goals an action server is tracking.
/** /**
* A pointer to the internally held array of goal handle structs is returned * A pointer to the internally held array of pointers to goal handle structs is returned
* along with the number of items in the array. * along with the number of items in the array.
* Goals that have terminated, successfully responded to a client with a
* result, and have expired (timed out) are not present in the array.
* *
* This function can fail, and therefore return `NULL`, if the: * The returned handle is made invalid if the action server is finalized, if
* - action server is `NULL` * rcl_shutdown() is called, or if rcl_action_expire_goals() is called and one or more
* - action server is invalid (never called init, called fini, or invalid) * goals are expired.
*
* The returned handle is made invalid if the action server is finalized or if
* rcl_shutdown() is called.
* The returned handle is not guaranteed to be valid for the life time of the * The returned handle is not guaranteed to be valid for the life time of the
* action server as it may be finalized and recreated itself. * action server as it may be finalized and recreated itself.
* Therefore, it is recommended to get the handle from the action server using * Therefore, it is recommended to get the handle from the action server using
@ -766,24 +793,53 @@ rcl_action_server_get_options(const rcl_action_server_t * action_server);
* Uses Atomics | No * Uses Atomics | No
* Lock-Free | Yes * Lock-Free | Yes
* *
* \param[in] action_server pointer to the rcl action server * \param[in] action_server handle to the action server
* \param[out] goal_handles is set to the array of pointers to goal handles if successful.
* \param[out] num_goals is set to the number of goals in the returned array if successful, * \param[out] num_goals is set to the number of goals in the returned array if successful,
* not set otherwise. * not set otherwise.
* \return pointer to an array goal handles if successful, otherwise `NULL` * \return `RCL_RET_OK` if successful, or
* \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
* \return `RCL_RET_ERROR` if an unspecified error occurs.
*/ */
RCL_ACTION_PUBLIC RCL_ACTION_PUBLIC
RCL_WARN_UNUSED RCL_WARN_UNUSED
const rcl_action_goal_handle_t * rcl_ret_t
rcl_action_server_get_goal_handles( rcl_action_server_get_goal_handles(
const rcl_action_server_t * action_server, const rcl_action_server_t * action_server,
uint32_t * num_goals); rcl_action_goal_handle_t *** goal_handles,
size_t * num_goals);
/// Check that the action server is valid. /// Check if a goal is already being tracked by an action server.
/** /**
* The bool returned is `false` if `action_server` is invalid. * Checks whether or not a goal is being tracked in the internal goal array.
* The bool returned is `true` otherwise. * The goal state has no effect on the return value.
* In the case where `false` is to be returned, an *
* error message is set. * <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Yes
*
* \param[in] action_server handle to the action server
* \param[in] goal_info handle to a struct containing the goal ID to check for
* \return `true` if `action_server` is currently tracking a goal with the provided goal ID, or
* \return `false` otherwise.
*/
RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
bool
rcl_action_server_goal_exists(
const rcl_action_server_t * action_server,
const rcl_action_goal_info_t * goal_info);
/// Check if an action server is valid.
/**
* In the case where `false` is returned (ie. the action server is invalid),
* an error message is set.
*
* This function cannot fail. * This function cannot fail.
* *
* <hr> * <hr>
@ -794,15 +850,14 @@ rcl_action_server_get_goal_handles(
* Uses Atomics | No * Uses Atomics | No
* Lock-Free | Yes * Lock-Free | Yes
* *
* \param[in] action_server pointer to the rcl action server * \param[in] action_server handle to the action server
* \param[in] error_msg_allocator a valid allocator or `NULL` * \return `true` if `action_server` is valid, or
* \return `true` if `action_server` is valid, otherwise `false` * \return `false` otherwise.
*/ */
RCL_ACTION_PUBLIC RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
bool bool
rcl_action_server_is_valid( rcl_action_server_is_valid(const rcl_action_server_t * action_server);
const rcl_action_server_t * action_server,
rcl_allocator_t * error_msg_allocator);
#ifdef __cplusplus #ifdef __cplusplus
} }

2
rcl_action/include/rcl_action/goal_handle.h
View file

@ -82,7 +82,7 @@ RCL_WARN_UNUSED
rcl_ret_t rcl_ret_t
rcl_action_goal_handle_init( rcl_action_goal_handle_init(
rcl_action_goal_handle_t * goal_handle, rcl_action_goal_handle_t * goal_handle,
rcl_action_goal_info_t * goal_info, const rcl_action_goal_info_t * goal_info,
rcl_allocator_t allocator); rcl_allocator_t allocator);
/// Finalize a rcl_action_goal_handle_t. /// Finalize a rcl_action_goal_handle_t.

6
rcl_action/include/rcl_action/types.h
View file

@ -53,6 +53,12 @@ extern "C"
/// Action invalid event return code. /// Action invalid event return code.
#define RCL_RET_ACTION_GOAL_EVENT_INVALID 2301 #define RCL_RET_ACTION_GOAL_EVENT_INVALID 2301
// TODO(jacobperron): Move these to a common place for UUIDs
#define UUID_SIZE 16
#define uuidcmp(uuid0, uuid1) (0 == memcmp(uuid0, uuid1, UUID_SIZE))
#define zerouuid (uint8_t[UUID_SIZE]) {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#define uuidcmpzero(uuid) uuidcmp(uuid, zerouuid)
// Forward declare // Forward declare
typedef struct rcl_action_server_t rcl_action_server_t; typedef struct rcl_action_server_t rcl_action_server_t;

10
rcl_action/package.xml
View file

@ -11,9 +11,15 @@
<build_depend>action_msgs</build_depend> <build_depend>action_msgs</build_depend>
<build_depend>rcl</build_depend> <build_depend>rcl</build_depend>
<build_depend>rcutils</build_depend>
<build_depend>rmw</build_depend>
<build_depend>rosidl_generator_c</build_depend>
<exec_depend>action_msgs</exec_depend> <build_export_depend>action_msgs</build_export_depend>
<exec_depend>rcl</exec_depend> <build_export_depend>rcl</build_export_depend>
<build_export_depend>rcutils</build_export_depend>
<build_export_depend>rmw</build_export_depend>
<build_export_depend>rosidl_generator_c</build_export_depend>
<test_depend>ament_cmake_gtest</test_depend> <test_depend>ament_cmake_gtest</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

788
rcl_action/src/rcl_action/action_server.c Normal file
View file

@ -0,0 +1,788 @@
// 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.
#ifdef __cplusplus
extern "C"
{
#endif
#include "rcl_action/action_server.h"
#include "rcl_action/default_qos.h"
#include "rcl_action/goal_handle.h"
#include "rcl_action/names.h"
#include "rcl_action/types.h"
#include "rcl/error_handling.h"
#include "rcl/rcl.h"
#include "rcl/time.h"
#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
{
rcl_service_t goal_service;
rcl_service_t cancel_service;
rcl_service_t result_service;
rcl_publisher_t feedback_publisher;
rcl_publisher_t status_publisher;
char * action_name;
rcl_action_server_options_t options;
// Array of goal handles
rcl_action_goal_handle_t ** goal_handles;
size_t num_goal_handles;
// Clock
rcl_clock_t clock;
} rcl_action_server_impl_t;
rcl_action_server_t
rcl_action_get_zero_initialized_server(void)
{
static rcl_action_server_t null_action_server = {0};
return null_action_server;
}
#define SERVICE_INIT(Type) \
char * Type ## _service_name = NULL; \
ret = rcl_action_get_ ## Type ## _service_name(action_name, allocator, &Type ## _service_name); \
if (RCL_RET_OK != ret) { \
if (RCL_RET_BAD_ALLOC == ret) { \
ret = RCL_RET_BAD_ALLOC; \
} else if (RCL_RET_ACTION_NAME_INVALID == ret) { \
ret = RCL_RET_ACTION_NAME_INVALID; \
} else { \
ret = RCL_RET_ERROR; \
} \
goto fail; \
} \
rcl_service_options_t Type ## _service_options = { \
.qos = options->Type ## _service_qos, .allocator = allocator \
}; \
ret = rcl_service_init( \
&action_server->impl->Type ## _service, \
node, \
type_support->Type ## _service_type_support, \
Type ## _service_name, \
&Type ## _service_options); \
allocator.deallocate(Type ## _service_name, allocator.state); \
if (RCL_RET_OK != ret) { \
if (RCL_RET_BAD_ALLOC == ret) { \
ret = RCL_RET_BAD_ALLOC; \
} else if (RCL_RET_SERVICE_NAME_INVALID == ret) { \
ret = RCL_RET_ACTION_NAME_INVALID; \
} else { \
ret = RCL_RET_ERROR; \
} \
goto fail; \
}
#define PUBLISHER_INIT(Type) \
char * Type ## _topic_name = NULL; \
ret = rcl_action_get_ ## Type ## _topic_name(action_name, allocator, &Type ## _topic_name); \
if (RCL_RET_OK != ret) { \
if (RCL_RET_BAD_ALLOC == ret) { \
ret = RCL_RET_BAD_ALLOC; \
} else if (RCL_RET_ACTION_NAME_INVALID == ret) { \
ret = RCL_RET_ACTION_NAME_INVALID; \
} else { \
ret = RCL_RET_ERROR; \
} \
goto fail; \
} \
rcl_publisher_options_t Type ## _publisher_options = { \
.qos = options->Type ## _topic_qos, .allocator = allocator \
}; \
ret = rcl_publisher_init( \
&action_server->impl->Type ## _publisher, \
node, \
type_support->Type ## _message_type_support, \
Type ## _topic_name, \
&Type ## _publisher_options); \
allocator.deallocate(Type ## _topic_name, allocator.state); \
if (RCL_RET_OK != ret) { \
if (RCL_RET_BAD_ALLOC == ret) { \
ret = RCL_RET_BAD_ALLOC; \
} else if (RCL_RET_TOPIC_NAME_INVALID == ret) { \
ret = RCL_RET_ACTION_NAME_INVALID; \
} else { \
ret = RCL_RET_ERROR; \
} \
goto fail; \
}
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)
{
RCL_CHECK_ARGUMENT_FOR_NULL(action_server, RCL_RET_INVALID_ARGUMENT);
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);
rcl_allocator_t allocator = options->allocator;
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
RCUTILS_LOG_DEBUG_NAMED(
ROS_PACKAGE_NAME, "Initializing action server for action name '%s'", action_name);
if (action_server->impl) {
RCL_SET_ERROR_MSG("action server already initialized, or memory was unintialized");
return RCL_RET_ALREADY_INIT;
}
// Allocate for action server
action_server->impl = (rcl_action_server_impl_t *)allocator.allocate(
sizeof(rcl_action_server_impl_t), allocator.state);
RCL_CHECK_FOR_NULL_WITH_MSG(
action_server->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
// Zero initialization
action_server->impl->goal_service = rcl_get_zero_initialized_service();
action_server->impl->cancel_service = rcl_get_zero_initialized_service();
action_server->impl->result_service = rcl_get_zero_initialized_service();
action_server->impl->feedback_publisher = rcl_get_zero_initialized_publisher();
action_server->impl->status_publisher = rcl_get_zero_initialized_publisher();
action_server->impl->action_name = NULL;
action_server->impl->options = *options; // copy options
action_server->impl->goal_handles = NULL;
action_server->impl->num_goal_handles = 0u;
action_server->impl->clock.type = RCL_CLOCK_UNINITIALIZED;
rcl_ret_t ret = RCL_RET_OK;
// Initialize services
SERVICE_INIT(goal);
SERVICE_INIT(cancel);
SERVICE_INIT(result);
// Initialize publishers
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) {
ret = RCL_RET_BAD_ALLOC;
goto fail;
}
return ret;
fail:
{
// Finalize any services/publishers that were initialized and deallocate action_server->impl
rcl_ret_t ret_throwaway = rcl_action_server_fini(action_server, node);
// Since there is already a failure, it is likely that finalize will error on one or more of
// the action servers members
(void)ret_throwaway;
}
return ret;
}
rcl_ret_t
rcl_action_server_fini(rcl_action_server_t * action_server, rcl_node_t * node)
{
RCL_CHECK_ARGUMENT_FOR_NULL(action_server, RCL_RET_ACTION_SERVER_INVALID);
if (!rcl_node_is_valid(node)) {
return RCL_RET_NODE_INVALID; // error already set
}
rcl_ret_t ret = RCL_RET_OK;
if (action_server->impl) {
// Finalize services
if (rcl_service_fini(&action_server->impl->goal_service, node) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
}
if (rcl_service_fini(&action_server->impl->cancel_service, node) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
}
if (rcl_service_fini(&action_server->impl->result_service, node) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
}
// Finalize publishers
if (rcl_publisher_fini(&action_server->impl->feedback_publisher, node) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
}
if (rcl_publisher_fini(&action_server->impl->status_publisher, node) != RCL_RET_OK) {
ret = RCL_RET_ERROR;
}
// Deallocate action name
rcl_allocator_t allocator = action_server->impl->options.allocator;
if (action_server->impl->action_name) {
allocator.deallocate(action_server->impl->action_name, allocator.state);
action_server->impl->action_name = NULL;
}
// Deallocate struct
allocator.deallocate(action_server->impl, allocator.state);
action_server->impl = NULL;
}
return ret;
}
rcl_action_server_options_t
rcl_action_server_get_default_options(void)
{
// !!! MAKE SURE THAT CHANGES TO THESE DEFAULTS ARE REFLECTED IN THE HEADER DOC STRING
static rcl_action_server_options_t default_options;
default_options.goal_service_qos = rmw_qos_profile_services_default;
default_options.cancel_service_qos = rmw_qos_profile_services_default;
default_options.result_service_qos = rmw_qos_profile_services_default;
default_options.feedback_topic_qos = rmw_qos_profile_default;
default_options.status_topic_qos = rcl_action_qos_profile_status_default;
default_options.allocator = rcl_get_default_allocator();
default_options.result_timeout.nanoseconds = RCUTILS_S_TO_NS(15 * 60); // 15 minutes
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)
{
TAKE_SERVICE_REQUEST(goal);
}
rcl_ret_t
rcl_action_send_goal_response(
const rcl_action_server_t * action_server,
void * ros_goal_response)
{
SEND_SERVICE_RESPONSE(goal);
}
// Implementation only
static int64_t
_goal_info_stamp_to_nanosec(const rcl_action_goal_info_t * goal_info)
{
assert(goal_info);
return RCUTILS_S_TO_NS(goal_info->stamp.sec) + (int64_t)goal_info->stamp.nanosec;
}
// Implementation only
static void
_nanosec_to_goal_info_stamp(const int64_t * nanosec, rcl_action_goal_info_t * goal_info)
{
assert(nanosec);
assert(goal_info);
goal_info->stamp.sec = (int32_t)RCUTILS_NS_TO_S(*nanosec);
goal_info->stamp.nanosec = *nanosec % RCUTILS_S_TO_NS(1);
}
rcl_action_goal_handle_t *
rcl_action_accept_new_goal(
rcl_action_server_t * action_server,
const rcl_action_goal_info_t * goal_info)
{
if (!rcl_action_server_is_valid(action_server)) {
return NULL; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(goal_info, NULL);
// Check if goal with same ID already exists
if (rcl_action_server_goal_exists(action_server, goal_info)) {
RCL_SET_ERROR_MSG("goal ID already exists");
return NULL;
}
// Allocate space in the goal handle pointer array
rcl_allocator_t allocator = action_server->impl->options.allocator;
rcl_action_goal_handle_t ** goal_handles = action_server->impl->goal_handles;
const size_t num_goal_handles = action_server->impl->num_goal_handles;
// TODO(jacobperron): Don't allocate for every accepted goal handle,
// instead double the memory allocated if needed.
const size_t new_num_goal_handles = num_goal_handles + 1u;
void * tmp_ptr = allocator.reallocate(
goal_handles, new_num_goal_handles * sizeof(rcl_action_goal_handle_t *), allocator.state);
if (!tmp_ptr) {
RCL_SET_ERROR_MSG("memory allocation failed for goal handle pointer");
return NULL;
}
goal_handles = (rcl_action_goal_handle_t **)tmp_ptr;
// Allocate space for a new goal handle
tmp_ptr = allocator.allocate(sizeof(rcl_action_goal_handle_t), allocator.state);
if (!tmp_ptr) {
RCL_SET_ERROR_MSG("memory allocation failed for new goal handle");
return NULL;
}
goal_handles[num_goal_handles] = (rcl_action_goal_handle_t *) tmp_ptr;
// Re-stamp goal info with current time
rcl_action_goal_info_t goal_info_stamp_now = rcl_action_get_zero_initialized_goal_info();
goal_info_stamp_now = *goal_info;
rcl_time_point_value_t now_time_point;
rcl_ret_t ret = rcl_clock_get_now(&action_server->impl->clock, &now_time_point);
if (RCL_RET_OK != ret) {
return NULL; // Error already set
}
_nanosec_to_goal_info_stamp(&now_time_point, &goal_info_stamp_now);
// Create a new goal handle
*goal_handles[num_goal_handles] = rcl_action_get_zero_initialized_goal_handle();
ret = rcl_action_goal_handle_init(
goal_handles[num_goal_handles], &goal_info_stamp_now, allocator);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("failed to initialize goal handle");
return NULL;
}
action_server->impl->goal_handles = goal_handles;
action_server->impl->num_goal_handles = new_num_goal_handles;
return goal_handles[num_goal_handles];
}
rcl_ret_t
rcl_action_publish_feedback(
const rcl_action_server_t * action_server,
void * ros_feedback)
{
if (!rcl_action_server_is_valid(action_server)) {
return RCL_RET_ACTION_SERVER_INVALID; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(ros_feedback, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = rcl_publish(&action_server->impl->feedback_publisher, ros_feedback);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR; // error already set
}
return RCL_RET_OK;
}
rcl_ret_t
rcl_action_get_goal_status_array(
const rcl_action_server_t * action_server,
rcl_action_goal_status_array_t * status_message)
{
if (!rcl_action_server_is_valid(action_server)) {
return RCL_RET_ACTION_SERVER_INVALID; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(status_message, RCL_RET_INVALID_ARGUMENT);
// If number of goals is zero, then we don't need to do any allocation
size_t num_goals = action_server->impl->num_goal_handles;
if (0u == num_goals) {
status_message->msg.status_list.size = 0u;
return RCL_RET_OK;
}
// Allocate status array
rcl_allocator_t allocator = action_server->impl->options.allocator;
rcl_ret_t ret = rcl_action_goal_status_array_init(status_message, num_goals, allocator);
if (RCL_RET_OK != ret) {
if (RCL_RET_BAD_ALLOC == ret) {
return RCL_RET_BAD_ALLOC;
}
return RCL_RET_ERROR;
}
// Populate status array
for (size_t i = 0u; i < num_goals; ++i) {
ret = rcl_action_goal_handle_get_info(
action_server->impl->goal_handles[i], &status_message->msg.status_list.data[i].goal_info);
if (RCL_RET_OK != ret) {
ret = RCL_RET_ERROR;
goto fail;
}
ret = rcl_action_goal_handle_get_status(
action_server->impl->goal_handles[i], &status_message->msg.status_list.data[i].status);
if (RCL_RET_OK != ret) {
ret = RCL_RET_ERROR;
goto fail;
}
}
return RCL_RET_OK;
fail:
{
rcl_ret_t ret_throwaway = rcl_action_goal_status_array_fini(status_message);
(void)ret_throwaway;
return ret;
}
}
rcl_ret_t
rcl_action_publish_status(
const rcl_action_server_t * action_server,
const void * status_message)
{
if (!rcl_action_server_is_valid(action_server)) {
return RCL_RET_ACTION_SERVER_INVALID; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(status_message, RCL_RET_INVALID_ARGUMENT);
rcl_ret_t ret = rcl_publish(&action_server->impl->status_publisher, status_message);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR; // error already set
}
return RCL_RET_OK;
}
rcl_ret_t
rcl_action_take_result_request(
const rcl_action_server_t * action_server,
void * ros_result_request)
{
TAKE_SERVICE_REQUEST(result);
}
rcl_ret_t
rcl_action_send_result_response(
const rcl_action_server_t * action_server,
void * ros_result_response)
{
SEND_SERVICE_RESPONSE(result);
}
rcl_ret_t
rcl_action_expire_goals(
const rcl_action_server_t * action_server,
size_t * num_expired)
{
if (!rcl_action_server_is_valid(action_server)) {
return RCL_RET_ACTION_SERVER_INVALID;
}
// Get current time (nanosec)
int64_t current_time;
rcl_ret_t ret = rcl_clock_get_now(&action_server->impl->clock, &current_time);
if (RCL_RET_OK != ret) {
return RCL_RET_ERROR;
}
// Used for shrinking goal handle array
rcl_allocator_t allocator = action_server->impl->options.allocator;
size_t num_goals_expired = 0u;
rcl_ret_t ret_final = RCL_RET_OK;
const int64_t timeout = (int64_t)action_server->impl->options.result_timeout.nanoseconds;
rcl_action_goal_handle_t * goal_handle;
rcl_action_goal_info_t goal_info;
int64_t goal_time;
size_t num_goal_handles = action_server->impl->num_goal_handles;
for (size_t i = 0u; i < num_goal_handles; ++i) {
goal_handle = action_server->impl->goal_handles[i];
// Expiration only applys to terminated goals
if (rcl_action_goal_handle_is_active(goal_handle)) {
continue;
}
ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
if (RCL_RET_OK != ret) {
ret_final = RCL_RET_ERROR;
continue;
}
goal_time = _goal_info_stamp_to_nanosec(&goal_info);
assert(current_time > goal_time);
if ((current_time - goal_time) > timeout) {
// Stop tracking goal handle
// Fill in any gaps left in the array with pointers from the end
action_server->impl->goal_handles[i] = action_server->impl->goal_handles[num_goal_handles];
action_server->impl->goal_handles[num_goal_handles--] = NULL;
++num_goals_expired;
}
}
// Shrink goal handle array if some goals expired
if (num_goals_expired > 0u) {
if (0u == num_goal_handles) {
allocator.deallocate(action_server->impl->goal_handles, allocator.state);
} else {
void * tmp_ptr = allocator.reallocate(
action_server->impl->goal_handles,
num_goal_handles * sizeof(rcl_action_goal_handle_t *),
allocator.state);
if (!tmp_ptr) {
RCL_SET_ERROR_MSG("failed to shrink size of goal handle array");
ret_final = RCL_RET_ERROR;
} else {
action_server->impl->goal_handles = (rcl_action_goal_handle_t **)tmp_ptr;
action_server->impl->num_goal_handles = num_goal_handles;
}
}
}
// If argument is not null, then set it
if (NULL != num_expired) {
(*num_expired) = num_goals_expired;
}
return ret_final;
}
rcl_ret_t
rcl_action_take_cancel_request(
const rcl_action_server_t * action_server,
void * ros_cancel_request)
{
TAKE_SERVICE_REQUEST(cancel);
}
rcl_ret_t
rcl_action_process_cancel_request(
const rcl_action_server_t * action_server,
const rcl_action_cancel_request_t * cancel_request,
rcl_action_cancel_response_t * cancel_response)
{
if (!rcl_action_server_is_valid(action_server)) {
return RCL_RET_ACTION_SERVER_INVALID; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(cancel_request, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(cancel_response, RCL_RET_INVALID_ARGUMENT);
rcl_allocator_t allocator = action_server->impl->options.allocator;
const size_t total_num_goals = action_server->impl->num_goal_handles;
// Storage for pointers to active goals handles that will be transitioned to canceling
// Note, we need heap allocation for MSVC support
rcl_action_goal_handle_t ** goal_handles_to_cancel =
(rcl_action_goal_handle_t **)allocator.allocate(
sizeof(rcl_action_goal_handle_t *) * total_num_goals, allocator.state);
if (!goal_handles_to_cancel) {
RCL_SET_ERROR_MSG("allocation failed for temporary goal handle array");
return RCL_RET_BAD_ALLOC;
}
size_t num_goals_to_cancel = 0u;
// Request data
const rcl_action_goal_info_t * request_goal_info = &cancel_request->goal_info;
const uint8_t * request_uuid = request_goal_info->uuid;
int64_t request_nanosec = _goal_info_stamp_to_nanosec(request_goal_info);
rcl_ret_t ret_final = RCL_RET_OK;
// Determine how many goals should transition to canceling
if (!uuidcmpzero(request_uuid) && (0u == request_nanosec)) {
// UUID is not zero and timestamp is zero; cancel exactly one goal (if it exists)
rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
rcl_action_goal_handle_t * goal_handle;
for (size_t i = 0u; i < total_num_goals; ++i) {
goal_handle = action_server->impl->goal_handles[i];
rcl_ret_t ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
if (RCL_RET_OK != ret) {
ret_final = RCL_RET_ERROR;
continue;
}
if (uuidcmp(request_uuid, goal_info.uuid)) {
if (rcl_action_goal_handle_is_cancelable(goal_handle)) {
goal_handles_to_cancel[num_goals_to_cancel++] = goal_handle;
}
break;
}
}
} else {
if (uuidcmpzero(request_uuid) && (0u == request_nanosec)) {
// UUID and timestamp are both zero; cancel all goals
// Set timestamp to max to cancel all active goals in the following for-loop
request_nanosec = INT64_MAX;
}
// Cancel all active goals at or before the timestamp
// Also cancel any goal matching the UUID in the cancel request
rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
rcl_action_goal_handle_t * goal_handle;
for (size_t i = 0u; i < total_num_goals; ++i) {
goal_handle = action_server->impl->goal_handles[i];
rcl_ret_t ret = rcl_action_goal_handle_get_info(goal_handle, &goal_info);
if (RCL_RET_OK != ret) {
ret_final = RCL_RET_ERROR;
continue;
}
const int64_t goal_nanosec = _goal_info_stamp_to_nanosec(&goal_info);
if (rcl_action_goal_handle_is_cancelable(goal_handle) &&
((goal_nanosec <= request_nanosec) || uuidcmp(request_uuid, goal_info.uuid)))
{
goal_handles_to_cancel[num_goals_to_cancel++] = goal_handle;
}
}
}
if (0u == num_goals_to_cancel) {
cancel_response->msg.goals_canceling.data = NULL;
cancel_response->msg.goals_canceling.size = 0u;
goto cleanup;
}
// Allocate space in response
rcl_ret_t ret = rcl_action_cancel_response_init(
cancel_response, num_goals_to_cancel, allocator);
if (RCL_RET_OK != ret) {
if (RCL_RET_BAD_ALLOC == ret) {
ret_final = RCL_RET_BAD_ALLOC; // error already set
}
ret_final = RCL_RET_ERROR; // error already set
goto cleanup;
}
// Transition goals to canceling and add to response
rcl_action_goal_handle_t * goal_handle;
for (size_t i = 0u; i < num_goals_to_cancel; ++i) {
goal_handle = goal_handles_to_cancel[i];
ret = rcl_action_update_goal_state(goal_handle, GOAL_EVENT_CANCEL);
if (RCL_RET_OK == ret) {
ret = rcl_action_goal_handle_get_info(
goal_handle, &cancel_response->msg.goals_canceling.data[i]);
}
if (RCL_RET_OK != ret) {
ret_final = RCL_RET_ERROR; // error already set
}
}
cleanup:
allocator.deallocate(goal_handles_to_cancel, allocator.state);
return ret_final;
}
rcl_ret_t
rcl_action_send_cancel_response(
const rcl_action_server_t * action_server,
void * ros_cancel_response)
{
SEND_SERVICE_RESPONSE(cancel);
}
const char *
rcl_action_server_get_action_name(const rcl_action_server_t * action_server)
{
if (!rcl_action_server_is_valid(action_server)) {
return NULL; // error already set
}
return action_server->impl->action_name;
}
const rcl_action_server_options_t *
rcl_action_server_get_options(const rcl_action_server_t * action_server)
{
if (!rcl_action_server_is_valid(action_server)) {
return NULL; // error already set
}
return &action_server->impl->options;
}
rcl_ret_t
rcl_action_server_get_goal_handles(
const rcl_action_server_t * action_server,
rcl_action_goal_handle_t *** goal_handles,
size_t * num_goals)
{
if (!rcl_action_server_is_valid(action_server)) {
return RCL_RET_ACTION_SERVER_INVALID; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(goal_handles, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(num_goals, RCL_RET_INVALID_ARGUMENT);
*goal_handles = action_server->impl->goal_handles;
*num_goals = action_server->impl->num_goal_handles;
return RCL_RET_OK;
}
bool
rcl_action_server_goal_exists(
const rcl_action_server_t * action_server,
const rcl_action_goal_info_t * goal_info)
{
if (!rcl_action_server_is_valid(action_server)) {
return false; // error already set
}
RCL_CHECK_ARGUMENT_FOR_NULL(goal_info, false);
rcl_action_goal_info_t gh_goal_info = rcl_action_get_zero_initialized_goal_info();
rcl_ret_t ret;
for (size_t i = 0u; i < action_server->impl->num_goal_handles; ++i) {
ret = rcl_action_goal_handle_get_info(action_server->impl->goal_handles[i], &gh_goal_info);
if (RCL_RET_OK != ret) {
RCL_SET_ERROR_MSG("failed to get info for goal handle");
return false;
}
// Compare UUIDs
if (uuidcmp(gh_goal_info.uuid, goal_info->uuid)) {
return true;
}
}
return false;
}
bool
rcl_action_server_is_valid(const rcl_action_server_t * action_server)
{
RCL_CHECK_FOR_NULL_WITH_MSG(action_server, "action server pointer is invalid", return false);
RCL_CHECK_FOR_NULL_WITH_MSG(
action_server->impl, "action server implementation is invalid", return false);
if (!rcl_service_is_valid(&action_server->impl->goal_service)) {
RCL_SET_ERROR_MSG("goal service is invalid");
return false;
}
if (!rcl_service_is_valid(&action_server->impl->cancel_service)) {
RCL_SET_ERROR_MSG("cancel service is invalid");
return false;
}
if (!rcl_service_is_valid(&action_server->impl->result_service)) {
RCL_SET_ERROR_MSG("result service is invalid");
return false;
}
if (!rcl_publisher_is_valid(&action_server->impl->feedback_publisher)) {
RCL_SET_ERROR_MSG("feedback publisher is invalid");
return false;
}
if (!rcl_publisher_is_valid(&action_server->impl->status_publisher)) {
RCL_SET_ERROR_MSG("status publisher is invalid");
return false;
}
return true;
}
#ifdef __cplusplus
}
#endif

2
rcl_action/src/rcl_action/goal_handle.c
View file

@ -39,7 +39,7 @@ rcl_action_get_zero_initialized_goal_handle(void)
rcl_ret_t rcl_ret_t
rcl_action_goal_handle_init( rcl_action_goal_handle_init(
rcl_action_goal_handle_t * goal_handle, rcl_action_goal_handle_t * goal_handle,
rcl_action_goal_info_t * goal_info, const rcl_action_goal_info_t * goal_info,
rcl_allocator_t allocator) rcl_allocator_t allocator)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT);

18
rcl_action/src/rcl_action/types.c
View file

@ -81,11 +81,12 @@ rcl_ret_t
rcl_action_goal_status_array_fini(rcl_action_goal_status_array_t * status_array) rcl_action_goal_status_array_fini(rcl_action_goal_status_array_t * status_array)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(status_array, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(status_array, RCL_RET_INVALID_ARGUMENT);
if (!status_array->msg.status_list.data) { if (status_array->msg.status_list.data) {
return RCL_RET_INVALID_ARGUMENT; status_array->allocator.deallocate(
status_array->msg.status_list.data, status_array->allocator.state);
status_array->msg.status_list.data = NULL;
status_array->msg.status_list.size = 0u;
} }
status_array->allocator.deallocate(
status_array->msg.status_list.data, status_array->allocator.state);
return RCL_RET_OK; return RCL_RET_OK;
} }
@ -122,11 +123,12 @@ rcl_ret_t
rcl_action_cancel_response_fini(rcl_action_cancel_response_t * cancel_response) rcl_action_cancel_response_fini(rcl_action_cancel_response_t * cancel_response)
{ {
RCL_CHECK_ARGUMENT_FOR_NULL(cancel_response, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(cancel_response, RCL_RET_INVALID_ARGUMENT);
if (!cancel_response->msg.goals_canceling.data) { if (cancel_response->msg.goals_canceling.data) {
return RCL_RET_INVALID_ARGUMENT; cancel_response->allocator.deallocate(
cancel_response->msg.goals_canceling.data, cancel_response->allocator.state);
cancel_response->msg.goals_canceling.data = NULL;
cancel_response->msg.goals_canceling.size = 0u;
} }
cancel_response->allocator.deallocate(
cancel_response->msg.goals_canceling.data, cancel_response->allocator.state);
return RCL_RET_OK; return RCL_RET_OK;
} }

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_action/names.h"
#include "rcl/allocator.h" #include "rcl/allocator.h"
#include "rcl/error_handling.h"
#include "rcl/types.h" #include "rcl/types.h"
struct ActionDerivedNameTestSubject struct ActionDerivedNameTestSubject
@ -62,7 +63,8 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
ret = test_subject.get_action_derived_name( ret = test_subject.get_action_derived_name(
invalid_action_name, default_allocator, invalid_action_name, default_allocator,
&action_derived_name); &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; action_derived_name = NULL;
rcl_allocator_t invalid_allocator = rcl_allocator_t invalid_allocator =
@ -70,27 +72,30 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
ret = test_subject.get_action_derived_name( ret = test_subject.get_action_derived_name(
test_subject.action_name, invalid_allocator, test_subject.action_name, invalid_allocator,
&action_derived_name); &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; action_derived_name = NULL;
char ** invalid_ptr_to_action_derived_name = NULL; char ** invalid_ptr_to_action_derived_name = NULL;
ret = test_subject.get_action_derived_name( ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator, test_subject.action_name, default_allocator,
invalid_ptr_to_action_derived_name); 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'; char dummy_char = '\0';
action_derived_name = &dummy_char; action_derived_name = &dummy_char;
ret = test_subject.get_action_derived_name( ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator, test_subject.action_name, default_allocator,
&action_derived_name); &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; action_derived_name = NULL;
ret = test_subject.get_action_derived_name( ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator, test_subject.action_name, default_allocator,
&action_derived_name); &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); EXPECT_STREQ(test_subject.expected_action_derived_name, action_derived_name);
default_allocator.deallocate(action_derived_name, default_allocator.state); default_allocator.deallocate(action_derived_name, default_allocator.state);
} }