diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt
index 79c997b..49922a1 100644
--- a/rcl_action/CMakeLists.txt
+++ b/rcl_action/CMakeLists.txt
@@ -4,8 +4,11 @@ project(rcl_action)
find_package(ament_cmake_ros REQUIRED)
+find_package(rosidl_generator_c REQUIRED)
find_package(action_msgs REQUIRED)
find_package(rcl REQUIRED)
+find_package(rcutils REQUIRED)
+find_package(rmw REQUIRED)
include_directories(
include
@@ -33,6 +36,7 @@ add_executable(test_compile_headers
set(rcl_action_sources
src/${PROJECT_NAME}/action_client.c
+ src/${PROJECT_NAME}/action_server.c
src/${PROJECT_NAME}/goal_handle.c
src/${PROJECT_NAME}/goal_state_machine.c
src/${PROJECT_NAME}/names.c
@@ -49,8 +53,11 @@ add_library(${PROJECT_NAME}
)
ament_target_dependencies(${PROJECT_NAME}
- "rcl"
+ "rosidl_generator_c"
"action_msgs"
+ "rmw"
+ "rcutils"
+ "rcl"
)
# Causes the visibility macros to use dllexport rather than dllimport,
# which is appropriate when building the dll but not consuming it.
@@ -68,6 +75,7 @@ install(TARGETS ${PROJECT_NAME}
)
if(BUILD_TESTING)
+ find_package(test_msgs REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
find_package(test_msgs REQUIRED)
@@ -86,6 +94,34 @@ if(BUILD_TESTING)
)
ament_target_dependencies(test_action_client "rcl" "test_msgs")
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
test/rcl_action/test_goal_handle.cpp
)
@@ -139,5 +175,8 @@ ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(ament_cmake)
ament_export_dependencies(rcl)
+ament_export_dependencies(rcutils)
+ament_export_dependencies(rmw)
ament_export_dependencies(action_msgs)
+ament_export_dependencies(rosidl_generator_c)
ament_package()
diff --git a/rcl_action/include/rcl_action/action_server.h b/rcl_action/include/rcl_action/action_server.h
index 4195b61..47d8790 100644
--- a/rcl_action/include/rcl_action/action_server.h
+++ b/rcl_action/include/rcl_action/action_server.h
@@ -27,6 +27,7 @@ extern "C"
#include "rcl/node.h"
#include "rcl/time.h"
+#include "rosidl_generator_c/action_type_support_struct.h"
/// Internal rcl_action implementation struct.
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.
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 result_service_qos;
rmw_qos_profile_t feedback_topic_qos;
rmw_qos_profile_t status_topic_qos;
/// Custom allocator for the action server, used for incidental allocations.
/** For default behavior (malloc/free), see: rcl_get_default_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.
rcl_duration_t result_timeout;
} rcl_action_server_options_t;
@@ -63,7 +66,7 @@ RCL_WARN_UNUSED
rcl_action_server_t
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
* 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
* only valid as long as the given rcl_node_t remains valid.
*
+ * The give rcl_clock_t must be valid and the resulting rcl_ction_server_t is
+ * only valid as long ast he given rcl_clock_t remains valid.
+ *
* The rosidl_action_type_support_t is obtained on a per .action type basis.
* When the user defines a ROS action, code is generated which provides the
* required rosidl_action_type_support_t object.
@@ -147,9 +153,10 @@ rcl_action_get_zero_initialized_server(void);
* Uses Atomics | No
* 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.
- * \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] action_name the name of the action
* \param[in] options action_server options, including quality of service settings
@@ -165,14 +172,15 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_server_init(
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 char * action_name,
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).
*
* 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[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_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_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:
*
* - goal_service_qos = rmw_qos_profile_services_default;
- * - result_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;
* - status_topic_qos = rcl_action_qos_profile_status_default;
- * - allocator = rcl_get_default_allocator()
- - result_timeout = 9e+11; // 15 minutes
+ * - allocator = rcl_get_default_allocator();
+ * - result_timeout = RCUTILS_S_TO_NS(15 * 60); // 15 minutes
*/
RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
rcl_action_server_options_t
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?
*
@@ -249,7 +257,7 @@ rcl_action_server_get_default_options(void);
* Lock-Free | Yes
* [1] only if required when filling the request, avoided for fixed sizes
*
- * \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
* where the request is copied
* \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,
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.
*
@@ -311,19 +319,27 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_send_goal_response(
const rcl_action_server_t * action_server,
- const void * ros_goal_response);
+ void * ros_goal_response);
-/// Accept a new goal using a rcl_action_server_t.
+/// Accept a new goal using an action server.
/**
* 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.
+ * 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
* rcl_action_take_goal_request() and before sending a response with
* rcl_action_send_goal_response().
*
+ * After calling this function, the action server will start tracking the goal.
+ *
* Example usage:
*
* ```c
@@ -354,9 +370,8 @@ rcl_action_send_goal_response(
* Uses Atomics | No
* 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] error_msg_allocator a valid allocator or `NULL`
* \return a pointer to a new goal handle representing the accepted goal, or
* \return `NULL` if a failure occured.
*/
@@ -364,11 +379,10 @@ RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
rcl_action_goal_handle_t *
rcl_action_accept_new_goal(
- const rcl_action_server_t * action_server,
- const rcl_action_goal_info_t * goal_info,
- rcl_allocator_t * error_msg_allocator);
+ rcl_action_server_t * action_server,
+ const rcl_action_goal_info_t * goal_info);
-/// 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`
* 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,
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
* 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,
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()
*
* A status array message associated with the action server can be created with
@@ -471,9 +485,9 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_publish_status(
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?
*
@@ -511,7 +525,7 @@ rcl_action_take_result_request(
const rcl_action_server_t * action_server,
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.
*
@@ -543,38 +557,54 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_send_result_response(
const rcl_action_server_t * action_server,
- const void * ros_result_response);
+ void * ros_result_response);
-/// Clear all expired goals associated with a rcl_action_server_t.
+/// 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.
* 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.
+ *
*
* Attribute | Adherence
* ------------------ | -------------
- * Allocates Memory | No
+ * Allocates Memory | Maybe[1]
* Thread-Safe | No
* Uses Atomics | No
* Lock-Free | Yes
+ * [1] if one or more goals expires, then the internal goal handle array may be
+ * resized or deallocated
*
* \param[in] action_server handle to the action server from which expired goals
* will be cleared.
- * \param[out] num_expired the number of expired goals cleared. If `NULL` then the
- * number is not set.
+ * \param[out] num_expired the number of expired goals, or set to `NULL` if unused
* \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_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.
*/
RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
rcl_ret_t
-rcl_action_clear_expired_goals(
+rcl_action_expire_goals(
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?
*
@@ -615,7 +645,7 @@ rcl_action_take_cancel_request(
const rcl_action_server_t * action_server,
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.
*
@@ -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] 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
* \return `RCL_RET_OK` if the response was sent successfully, 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,
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.
*
@@ -671,7 +701,7 @@ rcl_action_process_cancel_request(
* Uses Atomics | No
* 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
* \return `RCL_RET_OK` if the request was taken, or
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
@@ -683,14 +713,14 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_send_cancel_response(
const rcl_action_server_t * action_server,
- const void * ros_cancel_response);
+ void * ros_cancel_response);
-/// Get the name of the action for a rcl_action_server_t.
+/// Get the action name for an action server.
/**
* This function returns the action server's internal topic name string.
* This function can fail, and therefore return `NULL`, if the:
* - 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 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
* Lock-Free | Yes
*
- * \param[in] action_server the pointer to the action server
- * \return name string if successful, otherwise `NULL`
+ * \param[in] action_server pointer to the action server
+ * \return name string if successful, or
+ * \return `NULL` otherwise.
*/
RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
const char *
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 can fail, and therefore return `NULL`, if the:
* - 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 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
* Lock-Free | Yes
*
- * \param[in] action_server pointer to the action server
- * \return options struct if successful, otherwise `NULL`
+ * \param[in] action_server handle to the action server
+ * \return options struct if successful, or
+ * \return `NULL` otherwise.
*/
RCL_ACTION_PUBLIC
RCL_WARN_UNUSED
const rcl_action_server_options_t *
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.
- * 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:
- * - action server is `NULL`
- * - action server is invalid (never called init, called fini, or invalid)
- *
- * The returned handle is made invalid if the action server is finalized or if
- * rcl_shutdown() is called.
+ * The returned handle is made invalid if the action server is finalized, if
+ * rcl_shutdown() is called, or if rcl_action_expire_goals() is called and one or more
+ * goals are expired.
* 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.
* 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
* 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,
* 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_WARN_UNUSED
-const rcl_action_goal_handle_t *
+rcl_ret_t
rcl_action_server_get_goal_handles(
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.
- * The bool returned is `true` otherwise.
- * In the case where `false` is to be returned, an
- * error message is set.
+ * Checks whether or not a goal is being tracked in the internal goal array.
+ * The goal state has no effect on the return value.
+ *
+ *
+ * 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.
*
*
@@ -794,15 +850,14 @@ rcl_action_server_get_goal_handles(
* Uses Atomics | No
* Lock-Free | Yes
*
- * \param[in] action_server pointer to the rcl action server
- * \param[in] error_msg_allocator a valid allocator or `NULL`
- * \return `true` if `action_server` is valid, otherwise `false`
+ * \param[in] action_server handle to the action server
+ * \return `true` if `action_server` is valid, or
+ * \return `false` otherwise.
*/
RCL_ACTION_PUBLIC
+RCL_WARN_UNUSED
bool
-rcl_action_server_is_valid(
- const rcl_action_server_t * action_server,
- rcl_allocator_t * error_msg_allocator);
+rcl_action_server_is_valid(const rcl_action_server_t * action_server);
#ifdef __cplusplus
}
diff --git a/rcl_action/include/rcl_action/goal_handle.h b/rcl_action/include/rcl_action/goal_handle.h
index ecd9034..c1410a4 100644
--- a/rcl_action/include/rcl_action/goal_handle.h
+++ b/rcl_action/include/rcl_action/goal_handle.h
@@ -82,7 +82,7 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_action_goal_handle_init(
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);
/// Finalize a rcl_action_goal_handle_t.
diff --git a/rcl_action/include/rcl_action/types.h b/rcl_action/include/rcl_action/types.h
index cb686db..58a300e 100644
--- a/rcl_action/include/rcl_action/types.h
+++ b/rcl_action/include/rcl_action/types.h
@@ -53,6 +53,12 @@ extern "C"
/// Action invalid event return code.
#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
typedef struct rcl_action_server_t rcl_action_server_t;
diff --git a/rcl_action/package.xml b/rcl_action/package.xml
index 158624e..63c3e2f 100644
--- a/rcl_action/package.xml
+++ b/rcl_action/package.xml
@@ -11,9 +11,15 @@
action_msgs
rcl
+ rcutils
+ rmw
+ rosidl_generator_c
- action_msgs
- rcl
+ action_msgs
+ rcl
+ rcutils
+ rmw
+ rosidl_generator_c
ament_cmake_gtest
ament_lint_common
diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c
new file mode 100644
index 0000000..ef9bd6f
--- /dev/null
+++ b/rcl_action/src/rcl_action/action_server.c
@@ -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, ¤t_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
diff --git a/rcl_action/src/rcl_action/goal_handle.c b/rcl_action/src/rcl_action/goal_handle.c
index 1048b1d..903b272 100644
--- a/rcl_action/src/rcl_action/goal_handle.c
+++ b/rcl_action/src/rcl_action/goal_handle.c
@@ -39,7 +39,7 @@ rcl_action_get_zero_initialized_goal_handle(void)
rcl_ret_t
rcl_action_goal_handle_init(
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_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT);
diff --git a/rcl_action/src/rcl_action/types.c b/rcl_action/src/rcl_action/types.c
index 093562f..a4cd86c 100644
--- a/rcl_action/src/rcl_action/types.c
+++ b/rcl_action/src/rcl_action/types.c
@@ -81,11 +81,12 @@ rcl_ret_t
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);
- if (!status_array->msg.status_list.data) {
- return RCL_RET_INVALID_ARGUMENT;
+ if (status_array->msg.status_list.data) {
+ 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;
}
@@ -122,11 +123,12 @@ rcl_ret_t
rcl_action_cancel_response_fini(rcl_action_cancel_response_t * cancel_response)
{
RCL_CHECK_ARGUMENT_FOR_NULL(cancel_response, RCL_RET_INVALID_ARGUMENT);
- if (!cancel_response->msg.goals_canceling.data) {
- return RCL_RET_INVALID_ARGUMENT;
+ if (cancel_response->msg.goals_canceling.data) {
+ 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;
}
diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp
new file mode 100644
index 0000000..8d9bc6c
--- /dev/null
+++ b/rcl_action/test/rcl_action/test_action_communication.cpp
@@ -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
+
+#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;
+}
diff --git a/rcl_action/test/rcl_action/test_action_server.cpp b/rcl_action/test/rcl_action/test_action_server.cpp
new file mode 100644
index 0000000..9c81e91
--- /dev/null
+++ b/rcl_action/test/rcl_action/test_action_server.cpp
@@ -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
+
+#include
+#include
+
+#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(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(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(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(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(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(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));
+}
diff --git a/rcl_action/test/rcl_action/test_names.cpp b/rcl_action/test/rcl_action/test_names.cpp
index d24ef4a..2e42a23 100644
--- a/rcl_action/test/rcl_action/test_names.cpp
+++ b/rcl_action/test/rcl_action/test_names.cpp
@@ -19,6 +19,7 @@
#include "rcl_action/names.h"
#include "rcl/allocator.h"
+#include "rcl/error_handling.h"
#include "rcl/types.h"
struct ActionDerivedNameTestSubject
@@ -62,7 +63,8 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
ret = test_subject.get_action_derived_name(
invalid_action_name, default_allocator,
&action_derived_name);
- EXPECT_EQ(RCL_RET_ACTION_NAME_INVALID, ret);
+ EXPECT_EQ(RCL_RET_ACTION_NAME_INVALID, ret) << rcl_get_error_string().str;
+ rcl_reset_error();
action_derived_name = NULL;
rcl_allocator_t invalid_allocator =
@@ -70,27 +72,30 @@ TEST_P(TestActionDerivedName, validate_action_derived_getter)
ret = test_subject.get_action_derived_name(
test_subject.action_name, invalid_allocator,
&action_derived_name);
- EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+ rcl_reset_error();
action_derived_name = NULL;
char ** invalid_ptr_to_action_derived_name = NULL;
ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator,
invalid_ptr_to_action_derived_name);
- EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+ rcl_reset_error();
char dummy_char = '\0';
action_derived_name = &dummy_char;
ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator,
&action_derived_name);
- EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret);
+ EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str;
+ rcl_reset_error();
action_derived_name = NULL;
ret = test_subject.get_action_derived_name(
test_subject.action_name, default_allocator,
&action_derived_name);
- ASSERT_EQ(RCL_RET_OK, ret);
+ ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
EXPECT_STREQ(test_subject.expected_action_derived_name, action_derived_name);
default_allocator.deallocate(action_derived_name, default_allocator.state);
}