[rcl_action] Implement goal handle

rcl_action/CMakeLists.txt

@@ -32,6 +32,7 @@ add_executable(test_compile_headers
 )
 
 set(rcl_action_sources
+  src/${PROJECT_NAME}/goal_handle.c
   src/${PROJECT_NAME}/goal_state_machine.c
   src/${PROJECT_NAME}/names.c
   src/${PROJECT_NAME}/types.c
@@ -71,6 +72,18 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
   ament_find_gtest()
   # Gtests
+  ament_add_gtest(test_goal_handle
+    test/rcl_action/test_goal_handle.cpp
+  )
+  if(TARGET test_goal_handle)
+    target_include_directories(test_goal_handle PUBLIC
+      include
+      ${rcl_INCLUDE_DIRS}
+    )
+    target_link_libraries(test_goal_handle
+      ${PROJECT_NAME}
+    )
+  endif()
   ament_add_gtest(test_goal_state_machine
     test/rcl_action/test_goal_state_machine.cpp
   )

rcl_action/include/rcl_action/goal_handle.h

@@ -23,9 +23,8 @@ extern "C"
 #include "rcl_action/goal_state_machine.h"
 #include "rcl_action/types.h"
 #include "rcl_action/visibility_control.h"
+#include "rcl/allocator.h"
 
-// Forward declare
-typedef struct rcl_action_server_t rcl_action_server_t;
 
 /// Internal rcl action goal implementation struct.
 struct rcl_action_goal_handle_impl_t;
@@ -55,33 +54,10 @@ rcl_action_get_zero_initialized_goal_handle(void);
  * Goal information can be accessed with rcl_action_goal_handle_get_message() and
  * rcl_action_goal_handle_get_info().
  *
- * The given rcl_action_server_t must be valid and the resulting rcl_action_goal_handle_t is
- * only valid as long as the given rcl_action_server_t remains valid.
- *
- * Expected usage:
- *
- * ```c
- * #include <rcl/rcl.h>
- * #include <rcl_action/rcl_action.h>
- * #include <rosidl_generator_c/action_type_support_struct.h>
- * #include <example_interfaces/action/fibonacci.h>
- *
- * // ... initialize node
- * const rosidl_action_type_support_t * ts =
- *   ROSIDL_GET_ACTION_TYPE_SUPPORT(example_interfaces, Fibonacci);
- * rcl_action_server_t action_server = rcl_action_get_zero_initialized_server();
- * rcl_action_server_options_t action_server_ops = rcl_action_server_get_default_options();
- * ret = rcl_action_server_init(&action_server, &node, ts, "fibonacci", &action_server_ops);
- * // ... error handling
- * rcl_action_goal_handle_t goal_handle = rcl_action_get_zero_initialized_goal_handle();
- * ret = rcl_action_goal_handle_init(&goal_handle, &action_server);
- * // ... error handling, and on shutdown do finalization:
- * ret = rcl_action_goal_handle_fini(&goal_handle);
- * // ... error handling for rcl_goal_handle_fini()
- * ret = rcl_action_server_fini(&action_server, &node);
- * // ... error handling for rcl_action_server_fini()
- * // ... finalize and error handling for node
- * ```
+ * Goal handles are typically initialized and finalized by action servers.
+ * I.e. The allocator should be provided by the action server.
+ * Goal handles are created with rcl_action_accept_new_goal() and destroyed with
+ * rcl_action_clear_expired_goals() or rcl_action_server_fini().
  *
  * <hr>
  * Attribute          | Adherence
@@ -93,20 +69,21 @@ rcl_action_get_zero_initialized_goal_handle(void);
  *
  * \param[out] goal_handle preallocated, zero-initialized, goal handle structure
  *   to be initialized
- * \param[in] action_server valid rcl action server
- * \param[in] type_support type support object for the action's type
+ * \param[in] goal_info information about the goal to be copied to the goal handle
+ * \param[in] allocator a valid allocator used to initialized the goal handle
  * \return `RCL_RET_OK` if goal_handle was initialized successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if the allocator is invalid, or
  * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is 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.
+ * \return `RCL_RET_ALREADY_INIT` if the goal handle has already been initialized, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
 rcl_action_goal_handle_init(
   rcl_action_goal_handle_t * goal_handle,
-  const rcl_action_server_t * action_server);
+  rcl_action_goal_info_t * goal_info,
+  const rcl_allocator_t allocator);
 
 /// Finalize a rcl_action_goal_handle_t.
 /**
@@ -128,11 +105,9 @@ rcl_action_goal_handle_init(
  * Uses Atomics       | No
  * Lock-Free          | Yes
  *
- * \param[in] goal_handle struct to be deinitialized
- * \param[in] action_server used to create the goal handle
+ * \param[inout] goal_handle struct to be deinitialized
  * \return `RCL_RET_OK` if the goal handle was deinitialized successfully, or
  * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or
- * \return `RCL_RET_ERROR` if an unspecified error occurs.
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
@@ -156,7 +131,6 @@ rcl_action_goal_handle_fini(rcl_action_goal_handle_t * goal_handle);
  * \return `RCL_RET_OK` if the goal state was updated successfully, or
  * \return `RCL_RET_ACTION_GOAL_EVENT_INVALID` if the goal event is invalid, or
  * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or
- * \return `RCL_RET_ERROR` if an unspecified error occurs.
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
@@ -181,7 +155,7 @@ rcl_action_update_goal_state(
  * \param[out] goal_info a preallocated struct where the goal info is copied
  * \return `RCL_RET_OK` if the goal ID was accessed successfully, or
  * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or
- * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ * \return `RCL_RET_INVALID_ARGUMENT` if the goal_info argument is invalid
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
@@ -206,7 +180,7 @@ rcl_action_goal_handle_get_info(
  * \param[out] status a preallocated struct where the goal status is copied
  * \return `RCL_RET_OK` if the goal ID was accessed successfully, or
  * \return `RCL_RET_ACTION_GOAL_HANDLE_INVALID` if the goal handle is invalid, or
- * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ * \return `RCL_RET_INVALID_ARGUMENT` if the status argument is invalid
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
@@ -219,10 +193,6 @@ rcl_action_goal_handle_get_status(
 /**
  * This is a non-blocking call.
  *
- * The allocator needs to either be a valid allocator or `NULL`, in which case
- * the default allocator will be used.
- * The allocator is used when allocation is needed for an error message.
- *
  * <hr>
  * Attribute          | Adherence
  * ------------------ | -------------
@@ -232,26 +202,19 @@ rcl_action_goal_handle_get_status(
  * Lock-Free          | Yes
  *
  * \param[in] goal_handle struct containing the goal and metadata
- * \param[in] error_msg_allocator a valid allocator or `NULL`
  * \return `true` if a goal is in one of the following states: ACCEPTED, EXECUTING, or CANCELING, or
  * \return `false` otherwise, also
- * \return `false` if the goal handle pointer is invalid or the allocator is invalid
+ * \return `false` if the goal handle pointer is invalid
 */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
 bool
-rcl_action_goal_handle_is_active(
-  const rcl_action_goal_handle_t * goal_handle,
-  rcl_allocator_t * error_msg_allocator);
+rcl_action_goal_handle_is_active(const rcl_action_goal_handle_t * goal_handle);
 
 /// Check if a rcl_action_goal_handle_t is valid.
 /**
  * This is a non-blocking call.
  *
- * The allocator needs to either be a valid allocator or `NULL`, in which case
- * the default allocator will be used.
- * The allocator is used when allocation is needed for an error message.
- *
  * A goal handle is invalid if:
  *   - the implementation is `NULL` (rcl_action_goal_handle_init() not called or failed)
  *   - rcl_shutdown() has been called since the goal handle has been initialized
@@ -266,16 +229,13 @@ rcl_action_goal_handle_is_active(
  * Lock-Free          | Yes
  *
  * \param[in] goal_handle struct to evaluate as valid or not
- * \param[in] error_msg_allocator a valid allocator or `NULL`
  * \return `true` if the goal handle is valid, `false` otherwise, also
- * \return `false` if the allocator is invalid
+ * \return `false` if the goal handle pointer is null
  */
 RCL_ACTION_PUBLIC
 RCL_WARN_UNUSED
 bool
-rcl_action_goal_handle_is_valid(
-  const rcl_action_goal_handle_t * goal_handle,
-  rcl_allocator_t * error_msg_allocator);
+rcl_action_goal_handle_is_valid(const rcl_action_goal_handle_t * goal_handle);
 
 #ifdef __cplusplus
 }

rcl_action/src/rcl_action/goal_handle.c

@@ -0,0 +1,155 @@
+// 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/goal_handle.h"
+
+#include "rcl/rcl.h"
+#include "rcl/error_handling.h"
+
+typedef struct rcl_action_goal_handle_impl_t
+{
+  rcl_action_goal_info_t info;
+  rcl_action_goal_state_t state;
+  rcl_allocator_t allocator;
+} rcl_action_goal_handle_impl_t;
+
+rcl_action_goal_handle_t
+rcl_action_get_zero_initialized_goal_handle(void)
+{
+  static rcl_action_goal_handle_t null_handle = {0};
+  return null_handle;
+}
+
+rcl_ret_t
+rcl_action_goal_handle_init(
+  rcl_action_goal_handle_t * goal_handle,
+  rcl_action_goal_info_t * goal_info,
+  const rcl_allocator_t allocator)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(goal_info, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
+
+  // Ensure the goal handle is zero initialized
+  if (goal_handle->impl) {
+    RCL_SET_ERROR_MSG("goal_handle already initialized, or memory was unintialized");
+    return RCL_RET_ALREADY_INIT;
+  }
+  // Allocate space for the goal handle impl
+  goal_handle->impl = (rcl_action_goal_handle_impl_t *)allocator.allocate(
+    sizeof(rcl_action_goal_handle_impl_t), allocator.state);
+  if (!goal_handle->impl) {
+    RCL_SET_ERROR_MSG("goal_handle memory allocation failed");
+    return RCL_RET_BAD_ALLOC;
+  }
+  // Copy goal info (assuming it is trivially copyable)
+  goal_handle->impl->info = *goal_info;
+  // Initialize state to ACCEPTED
+  goal_handle->impl->state = GOAL_STATE_ACCEPTED;
+  // Copy the allocator
+  goal_handle->impl->allocator = allocator;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_action_goal_handle_fini(rcl_action_goal_handle_t * goal_handle)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT);
+  if (!rcl_action_goal_handle_is_valid(goal_handle)) {
+    return RCL_RET_ACTION_GOAL_HANDLE_INVALID;
+  }
+  goal_handle->impl->allocator.deallocate(goal_handle->impl, goal_handle->impl->allocator.state);
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_action_update_goal_state(
+  rcl_action_goal_handle_t * goal_handle,
+  const rcl_action_goal_event_t goal_event)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT);
+  if (!rcl_action_goal_handle_is_valid(goal_handle)) {
+    return RCL_RET_ACTION_GOAL_HANDLE_INVALID;
+  }
+  rcl_action_goal_state_t new_state = rcl_action_transition_goal_state(
+    goal_handle->impl->state, goal_event);
+  if (GOAL_STATE_UNKNOWN == new_state) {
+    return RCL_RET_ACTION_GOAL_EVENT_INVALID;
+  }
+  goal_handle->impl->state = new_state;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_action_goal_handle_get_info(
+  const rcl_action_goal_handle_t * goal_handle,
+  rcl_action_goal_info_t * goal_info)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT);
+  if (!rcl_action_goal_handle_is_valid(goal_handle)) {
+    return RCL_RET_ACTION_GOAL_HANDLE_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(goal_info, RCL_RET_INVALID_ARGUMENT);
+  // Assumption: goal info is trivially copyable
+  *goal_info = goal_handle->impl->info;
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_action_goal_handle_get_status(
+  const rcl_action_goal_handle_t * goal_handle,
+  rcl_action_goal_state_t * status)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, RCL_RET_INVALID_ARGUMENT);
+  if (!rcl_action_goal_handle_is_valid(goal_handle)) {
+    return RCL_RET_ACTION_GOAL_HANDLE_INVALID;
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(status, RCL_RET_INVALID_ARGUMENT);
+  *status = goal_handle->impl->state;
+  return RCL_RET_OK;
+}
+
+bool
+rcl_action_goal_handle_is_active(const rcl_action_goal_handle_t * goal_handle)
+{
+  if (!rcl_action_goal_handle_is_valid(goal_handle)) {
+    return false;
+  }
+  switch (goal_handle->impl->state) {
+    case GOAL_STATE_ACCEPTED:
+    case GOAL_STATE_EXECUTING:
+    case GOAL_STATE_CANCELING:
+      return true;
+    default:
+      return false;
+  }
+}
+
+bool
+rcl_action_goal_handle_is_valid(const rcl_action_goal_handle_t * goal_handle)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(goal_handle, false);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    goal_handle->impl, "goal handle implementation is invalid", return false);
+  return true;
+}
+
+#ifdef __cplusplus
+}
+#endif

rcl_action/src/rcl_action/goal_state_machine.c

@@ -98,6 +98,13 @@ rcl_action_transition_goal_state(
   const rcl_action_goal_state_t state,
   const rcl_action_goal_event_t event)
 {
+  if (state < 0 ||
+    state >= GOAL_STATE_NUM_STATES ||
+    event < 0 ||
+    event >= GOAL_EVENT_NUM_EVENTS)
+  {
+    return GOAL_STATE_UNKNOWN;
+  }
   rcl_action_goal_event_handler handler = _goal_state_transition_map[state][event];
   if (NULL == handler) {
     return GOAL_STATE_UNKNOWN;