[rcl_action] Implement init/fini functions for types (#312)
* Refactored API. Removed init/fini functions for types that do not necessarily need allocation on the heap. * Added unit tests for implementation.
This commit is contained in:
parent
29e7dbe156
commit
f9dfc5ddd1
4 changed files with 356 additions and 133 deletions
|
@ -27,6 +27,7 @@ extern "C"
|
|||
#include "action_msgs/msg/goal_status_array.h"
|
||||
#include "action_msgs/srv/cancel_goal.h"
|
||||
|
||||
#include "rcl/allocator.h"
|
||||
#include "rcl/macros.h"
|
||||
#include "rcl/types.h"
|
||||
|
||||
|
@ -56,6 +57,7 @@ typedef struct rcl_action_server_t rcl_action_server_t;
|
|||
|
||||
// Typedef generated messages for convenience
|
||||
typedef action_msgs__msg__GoalInfo rcl_action_goal_info_t;
|
||||
typedef action_msgs__msg__GoalStatus rcl_action_goal_status_t;
|
||||
typedef action_msgs__msg__GoalStatusArray rcl_action_goal_status_array_t;
|
||||
typedef action_msgs__srv__CancelGoal_Request rcl_action_cancel_request_t;
|
||||
typedef action_msgs__srv__CancelGoal_Response rcl_action_cancel_response_t;
|
||||
|
@ -95,79 +97,6 @@ RCL_WARN_UNUSED
|
|||
rcl_action_goal_info_t
|
||||
rcl_action_get_zero_initialized_goal_info(void);
|
||||
|
||||
/// Initialize a rcl_action_goal_info_t.
|
||||
/**
|
||||
* After calling this function on a rcl_action_goal_info_t, it can be populated
|
||||
* and used to accept goal requests with rcl_action_accept_new_goal().
|
||||
*
|
||||
* The given rcl_action_server_t must be valid and the resulting
|
||||
* rcl_action_goal_info_t is only valid as long as the given rcl_action_server_t
|
||||
* remains valid.
|
||||
*
|
||||
* Expected usage (for C action servers):
|
||||
*
|
||||
* ```c
|
||||
* #include <rcl_action/rcl_action.h>
|
||||
*
|
||||
* // ... init action server
|
||||
* rcl_action_goal_info_t goal_info = rcl_action_get_zero_initialized_goal_info();
|
||||
* ret = rcl_action_goal_info_init(&goal_info, &action_server);
|
||||
* // ... error handling, and when done with the goal info message, finalize
|
||||
* ret = rcl_action_goal_info_fini(&goal_info, &action_server);
|
||||
* // ... error handling
|
||||
* ```
|
||||
*
|
||||
* <hr>
|
||||
* Attribute | Adherence
|
||||
* ------------------ | -------------
|
||||
* Allocates Memory | Yes
|
||||
* Thread-Safe | No
|
||||
* Uses Atomics | No
|
||||
* Lock-Free | Yes
|
||||
*
|
||||
* \param[out] goal_info a preallocated, zero-initialized, goal info message
|
||||
* to be initialized.
|
||||
* \param[in] action_server a valid action server handle
|
||||
* \return `RCL_RET_OK` if goal info was initialized successfully, or
|
||||
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||
* \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is 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_goal_info_init(
|
||||
rcl_action_goal_info_t * goal_info,
|
||||
const rcl_action_server_t * action_server);
|
||||
|
||||
/// Finalize a rcl_action_goal_info_t.
|
||||
/**
|
||||
* After calling, the goal info message will no longer be valid.
|
||||
* However, the given action server handle is still valid.
|
||||
*
|
||||
* <hr>
|
||||
* Attribute | Adherence
|
||||
* ------------------ | -------------
|
||||
* Allocates Memory | Yes
|
||||
* Thread-Safe | No
|
||||
* Uses Atomics | No
|
||||
* Lock-Free | Yes
|
||||
*
|
||||
* \param[inout] goal_info the goal info message to be deinitialized
|
||||
* \param[in] action_server handle to the action sever used to create the goal info message
|
||||
* \return `RCL_RET_OK` if the goal info message 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 is invalid, or
|
||||
* \return `RCL_RET_ERROR` if an unspecified error occurs.
|
||||
*/
|
||||
RCL_ACTION_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_action_goal_info_fini(
|
||||
rcl_action_goal_info_t * goal_info,
|
||||
const rcl_action_server_t * action_server);
|
||||
|
||||
/// Return a rcl_action_goal_status_array_t with members set to `NULL`.
|
||||
/**
|
||||
* Should be called to get a null rcl_action_goal_status_array_t before passing to
|
||||
|
@ -189,27 +118,37 @@ RCL_WARN_UNUSED
|
|||
rcl_action_cancel_request_t
|
||||
rcl_action_get_zero_initialized_cancel_request(void);
|
||||
|
||||
/// Initialize a rcl_action_cancel_request_t.
|
||||
/// Return a rcl_action_cancel_response_t with members set to `NULL`.
|
||||
/**
|
||||
* After calling this function on a rcl_action_cancel_request_t, it can be populated
|
||||
* and used to process cancel requests with an action server using
|
||||
* rcl_action_process_cancel_request().
|
||||
* Should be called to get a null rcl_action_cancel_response_t before passing to
|
||||
* rcl_action_cancel_response_init().
|
||||
*/
|
||||
RCL_ACTION_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_action_cancel_response_t
|
||||
rcl_action_get_zero_initialized_cancel_response(void);
|
||||
|
||||
/// Initialize a rcl_action_goal_status_array_t.
|
||||
/**
|
||||
* After calling this function on a rcl_action_goal_status_array_t, it can be populated
|
||||
* and used to get and send status array messages with an action server using
|
||||
* rcl_action_get_goal_status_array() and rcl_action_publish_status() respectively.
|
||||
*
|
||||
* The given rcl_action_server_t must be valid and the resulting
|
||||
* rcl_action_cancel_request_t is only valid as long as the given rcl_action_server_t
|
||||
* remains valid.
|
||||
*
|
||||
* Expected usage (for C action servers):
|
||||
* Example usage:
|
||||
*
|
||||
* ```c
|
||||
* #include <rcl/rcl.h>
|
||||
* #include <rcl_action/rcl_action.h>
|
||||
*
|
||||
* // ... init action server
|
||||
* rcl_action_cancel_request_t cancel_request = rcl_action_get_zero_initialized_cancel_request();
|
||||
* ret = rcl_action_cancel_request_init(&cancel_request, &action_server);
|
||||
* // ... error handling, and when done processing request, finalize
|
||||
* ret = rcl_action_cancel_request_fini(&cancel_request, &action_server);
|
||||
* rcl_action_goal_status_array_t goal_status_array =
|
||||
* rcl_action_get_zero_initialized_goal_status_array();
|
||||
* size_t num_status = 42;
|
||||
* ret = rcl_action_goal_status_array_init(
|
||||
* &goal_status_array,
|
||||
* num_status,
|
||||
* rcl_get_default_allocator());
|
||||
* // ... error handling, and when done with message, finalize
|
||||
* ret = rcl_action_goal_status_array_fini(&goal_status_array, rcl_get_default_allocator());
|
||||
* // ... error handling
|
||||
* ```
|
||||
*
|
||||
|
@ -221,26 +160,27 @@ rcl_action_get_zero_initialized_cancel_request(void);
|
|||
* Uses Atomics | No
|
||||
* Lock-Free | Yes
|
||||
*
|
||||
* \param[out] cancel_request a preallocated, zero-initialized, cancel request message
|
||||
* \param[out] status_array a preallocated, zero-initialized, goal status array message
|
||||
* to be initialized.
|
||||
* \param[in] action_server a valid action server handle
|
||||
* \return `RCL_RET_OK` if cancel request was initialized successfully, or
|
||||
* \param[in] num_status the number of status messages to allocate space for
|
||||
* \param[in] allocator a valid allocator
|
||||
* \return `RCL_RET_OK` if cancel response was initialized successfully, or
|
||||
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||
* \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or
|
||||
* \return `RCL_RET_ALREADY_INIT` if the status array has already been initialized, 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_cancel_request_init(
|
||||
rcl_action_cancel_request_t * cancel_request,
|
||||
const rcl_action_server_t * action_server);
|
||||
rcl_action_goal_status_array_init(
|
||||
rcl_action_goal_status_array_t * status_array,
|
||||
const size_t num_status,
|
||||
const rcl_allocator_t allocator);
|
||||
|
||||
/// Finalize a rcl_action_cancel_request_t.
|
||||
/// Finalize a rcl_action_goal_status_array_t.
|
||||
/**
|
||||
* After calling, the cancel request message will no longer be valid.
|
||||
* However, the given action server handle is still valid.
|
||||
* After calling, the goal status array message will no longer be valid.
|
||||
*
|
||||
* <hr>
|
||||
* Attribute | Adherence
|
||||
|
@ -250,29 +190,18 @@ rcl_action_cancel_request_init(
|
|||
* Uses Atomics | No
|
||||
* Lock-Free | Yes
|
||||
*
|
||||
* \param[inout] cancel_request the cancel request message to be deinitialized
|
||||
* \param[in] action_server handle to the action sever used to create the cancel request
|
||||
* \return `RCL_RET_OK` if the cancel request was deinitialized successfully, or
|
||||
* \param[inout] status_array the goal status array message to be deinitialized
|
||||
* \param[in] allocator handle to the allocator used to create the goal status array
|
||||
* \return `RCL_RET_OK` if the goal status array 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 is invalid, or
|
||||
* \return `RCL_RET_ERROR` if an unspecified error occurs.
|
||||
*/
|
||||
RCL_ACTION_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_action_cancel_request_fini(
|
||||
rcl_action_cancel_request_t * cancel_request,
|
||||
const rcl_action_server_t * action_server);
|
||||
|
||||
/// Return a rcl_action_cancel_response_t with members set to `NULL`.
|
||||
/**
|
||||
* Should be called to get a null rcl_action_cancel_response_t before passing to
|
||||
* rcl_action_cancel_response_init().
|
||||
*/
|
||||
RCL_ACTION_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_action_cancel_response_t
|
||||
rcl_action_get_zero_initialized_cancel_response(void);
|
||||
rcl_action_goal_status_array_fini(
|
||||
rcl_action_goal_status_array_t * status_array,
|
||||
const rcl_allocator_t allocator);
|
||||
|
||||
/// Initialize a rcl_action_cancel_response_t.
|
||||
/**
|
||||
|
@ -280,22 +209,21 @@ rcl_action_get_zero_initialized_cancel_response(void);
|
|||
* and used to process cancel requests with an action server using
|
||||
* rcl_action_process_cancel_request().
|
||||
*
|
||||
* The given rcl_action_server_t must be valid and the resulting
|
||||
* rcl_action_cancel_response_t is only valid as long as the given rcl_action_server_t
|
||||
* remains valid.
|
||||
*
|
||||
* Expected usage (for C action servers):
|
||||
* Example usage:
|
||||
*
|
||||
* ```c
|
||||
* #include <rcl/rcl.h>
|
||||
* #include <rcl_action/rcl_action.h>
|
||||
*
|
||||
* // ... init action server
|
||||
* rcl_action_cancel_response_t cancel_response =
|
||||
* rcl_action_get_zero_initialized_cancel_response();
|
||||
* ret = rcl_action_cancel_response_init(&cancel_response, &action_server);
|
||||
* size_t num_goals = 10;
|
||||
* ret = rcl_action_cancel_response_init(
|
||||
* &cancel_response,
|
||||
* num_goals,
|
||||
* rcl_get_default_allocator());
|
||||
* // ... error handling, and when done processing response, finalize
|
||||
* ret = rcl_action_cancel_response_fini(&cancel_response, &action_server);
|
||||
* ret = rcl_action_cancel_response_fini(&cancel_response, rcl_get_default_allocator());
|
||||
* // ... error handling
|
||||
* ```
|
||||
*
|
||||
|
@ -309,10 +237,11 @@ rcl_action_get_zero_initialized_cancel_response(void);
|
|||
*
|
||||
* \param[out] cancel_response a preallocated, zero-initialized, cancel response message
|
||||
* to be initialized.
|
||||
* \param[in] action_server a valid action server handle
|
||||
* \param[in] num_goals the number of goals that are canceling to add to the response
|
||||
* \param[in] allocator a valid allocator
|
||||
* \return `RCL_RET_OK` if cancel response was initialized successfully, or
|
||||
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||
* \return `RCL_RET_ACTION_SERVER_INVALID` if the action server is invalid, or
|
||||
* \return `RCL_RET_ALREADY_INIT` if the cancel response has already been initialized, or
|
||||
* \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
|
||||
* \return `RCL_RET_ERROR` if an unspecified error occurs.
|
||||
*/
|
||||
|
@ -321,12 +250,12 @@ RCL_WARN_UNUSED
|
|||
rcl_ret_t
|
||||
rcl_action_cancel_response_init(
|
||||
rcl_action_cancel_response_t * cancel_response,
|
||||
const rcl_action_server_t * action_server);
|
||||
const size_t num_goals,
|
||||
const rcl_allocator_t allocator);
|
||||
|
||||
/// Finalize a rcl_action_cancel_response_t.
|
||||
/**
|
||||
* After calling, the cancel response message will no longer be valid.
|
||||
* However, the given action server handle is still valid.
|
||||
*
|
||||
* <hr>
|
||||
* Attribute | Adherence
|
||||
|
@ -337,10 +266,9 @@ rcl_action_cancel_response_init(
|
|||
* Lock-Free | Yes
|
||||
*
|
||||
* \param[inout] cancel_response the cancel response message to be deinitialized
|
||||
* \param[in] action_server handle to the action sever used to create the cancel response
|
||||
* \param[in] allocator handle to the allocator used to create the cancel response
|
||||
* \return `RCL_RET_OK` if the cancel response 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 is invalid, or
|
||||
* \return `RCL_RET_ERROR` if an unspecified error occurs.
|
||||
*/
|
||||
RCL_ACTION_PUBLIC
|
||||
|
@ -348,7 +276,7 @@ RCL_WARN_UNUSED
|
|||
rcl_ret_t
|
||||
rcl_action_cancel_response_fini(
|
||||
rcl_action_cancel_response_t * cancel_response,
|
||||
rcl_action_server_t * action_server);
|
||||
const rcl_allocator_t allocator);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue