implement rcl_wait and rcl_take for rcl_event_t

Signed-off-by: Miaofei <miaofei@amazon.com>

rcl/CMakeLists.txt

@@ -35,6 +35,7 @@ set(${PROJECT_NAME}_sources
   src/rcl/client.c
   src/rcl/common.c
   src/rcl/context.c
+  src/rcl/event.c
   src/rcl/expand_topic_name.c
   src/rcl/graph.c
   src/rcl/guard_condition.c

rcl/include/rcl/event.h

@@ -0,0 +1,171 @@
+// Copyright 2019 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.
+
+#ifndef RCL__EVENT_H_
+#define RCL__EVENT_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "rcl/client.h"
+#include "rcl/macros.h"
+#include "rcl/publisher.h"
+#include "rcl/service.h"
+#include "rcl/subscription.h"
+#include "rcl/visibility_control.h"
+
+
+typedef enum rcl_publisher_event_type_t
+{
+  RCL_PUBLISHER_OFFERED_DEADLINE_MISSED,
+  RCL_PUBLISHER_LIVELINESS_LOST
+} rcl_publisher_event_type_t;
+
+typedef enum rcl_subscription_event_type_t
+{
+  RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED,
+  RCL_SUBSCRIPTION_LIVELINESS_CHANGED
+} rcl_subscription_event_type_t;
+
+/// rmw struct.
+typedef struct rmw_event_t rmw_event_t;
+
+/// Internal rcl implementation struct.
+struct rcl_event_impl_t;
+
+/// Structure which encapsulates a ROS QoS event handle.
+typedef struct rcl_event_t
+{
+  struct rcl_event_impl_t * impl;
+} rcl_event_t;
+
+
+/// Return a rcl_event_t struct with members set to `NULL`.
+/**
+ * Should be called to get a null rcl_event_t before passing to
+ * rcl_event_init().
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_event_t
+rcl_get_zero_initialized_event(void);
+
+/// Initialize an rcl_event_t with a publisher.
+/**
+ * Fill the rcl_event_t with the publisher and desired event_type.
+ *
+ * \param[in,out] event pointer to fill
+ * \param[in] publisher to get events from
+ * \param[in] event_type to listen for
+ * \return `RCL_RET_OK` if the rcl_event_t is filled, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_publisher_event_init(
+  rcl_event_t * event,
+  const rcl_publisher_t * publisher,
+  const rcl_publisher_event_type_t event_type);
+
+/// Initialize an rcl_event_t with a subscription.
+/**
+ * Fill the rcl_event_t with the subscription and desired event_type.
+ *
+ * \param[in,out] event pointer to fill
+ * \param[in] subscription to get events from
+ * \param[in] event_type to listen for
+ * \return `RCL_RET_OK` if the rcl_event_t is filled, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_subscription_event_init(
+  rcl_event_t * event,
+  const rcl_subscription_t * subscription,
+  const rcl_subscription_event_type_t event_type);
+
+// Take event using the event handle.
+/**
+ * Take an event from the event handle.
+ *
+ * \param[in] event_handle event object to take from
+ * \param[in, out] event_info event info object to write taken data into
+ * \param[in, out] taken boolean flag indicating if an event was taken or not
+ * \return `RCL_RET_OK` if successful, or
+ * \return `RCL_RET_BAD_ALLOC` if memory allocation failed, or
+ * \return `RCL_RET_ERROR` if an unexpected error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_take_event(
+  const rcl_event_t * event,
+  void * event_info);
+
+// Finalize an event.
+/**
+ * Finalize an event.
+ *
+ * \param[in] event to finalize
+ * \return `RCL_RET_OK` if successful, or
+ * \return `RCL_RET_EVENT_INVALID` if event is null, or
+ * \return `RCL_RET_ERROR` if an unexpected error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_event_fini(rcl_event_t * event);
+
+/// Return the rmw event handle.
+/**
+ * The handle returned is a pointer to the internally held rmw handle.
+ * This function can fail, and therefore return `NULL`, if the:
+ *   - event is `NULL`
+ *   - event is invalid (never called init, called fini, or invalid node)
+ *
+ * The returned handle is made invalid if the event is finalized or if
+ * rcl_shutdown() is called.
+ * The returned handle is not guaranteed to be valid for the life time of the
+ * event as it may be finalized and recreated itself.
+ * Therefore it is recommended to get the handle from the event using
+ * this function each time it is needed and avoid use of the handle
+ * concurrently with functions that might change it.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Yes
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] event pointer to the rcl event
+ * \return rmw event handle if successful, otherwise `NULL`
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rmw_event_t *
+rcl_event_get_rmw_handle(const rcl_event_t * event);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RCL__EVENT_H_

rcl/include/rcl/node.h

@@ -361,6 +361,32 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id);
 
+/// Manually assert that this node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE)
+/**
+ * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the creator of
+ * this node may manually call `assert_liveliness` at some point in time to signal to the rest
+ * of the system that this Node is still alive.
+ * This function must be called at least as often as the qos_profile's liveliness_lease_duration
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] node handle to the node that needs liveliness to be asserted
+ * \return `RCL_RET_OK` if the liveliness assertion was completed successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_NODE_INVALID` if the node is invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_node_assert_liveliness(const rcl_node_t * node);
+
 /// Return the rmw node handle.
 /**
  * The handle returned is a pointer to the internally held rmw handle.

rcl/include/rcl/publisher.h

@@ -290,6 +290,32 @@ rcl_ret_t
 rcl_publish_serialized_message(
   const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message);
 
+/// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC)
+/**
+ * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator of
+ * this publisher may manually call `assert_liveliness` at some point in time to signal to the rest
+ * of the system that this Node is still alive.
+ * This function must be called at least as often as the qos_profile's liveliness_lease_duration
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] publisher handle to the publisher that needs liveliness to be asserted
+ * \return `RCL_RET_OK` if the liveliness assertion was completed successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_PUBLISHER_INVALID` if the publisher is invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_publisher_assert_liveliness(const rcl_publisher_t * publisher);
+
 /// Get the topic name for the publisher.
 /**
  * This function returns the publisher's internal topic name string.

rcl/include/rcl/types.h

@@ -99,6 +99,13 @@ typedef rmw_ret_t rcl_ret_t;
 /// Argument is not a valid log level rule
 #define RCL_RET_INVALID_LOG_LEVEL_RULE 1020
 
+// rcl event specific ret codes in 20XX
+/// Invalid rcl_event_t given return code.
+#define RCL_RET_EVENT_INVALID 2000
+/// Failed to take an event from the event handle
+#define RCL_RET_EVENT_TAKE_FAILED 2001
+
+
 /// typedef for rmw_serialized_message_t;
 typedef rmw_serialized_message_t rcl_serialized_message_t;
 

rcl/include/rcl/wait.h

@@ -29,6 +29,7 @@ extern "C"
 #include "rcl/service.h"
 #include "rcl/subscription.h"
 #include "rcl/timer.h"
+#include "rcl/event.h"
 #include "rcl/types.h"
 #include "rcl/visibility_control.h"
 
@@ -52,6 +53,9 @@ typedef struct rcl_wait_set_t
   /// Storage for service pointers.
   const rcl_service_t ** services;
   size_t size_of_services;
+  /// Storage for event pointers.
+  const rcl_event_t ** events;
+  size_t size_of_events;
   /// Implementation specific storage.
   struct rcl_wait_set_impl_t * impl;
 } rcl_wait_set_t;
@@ -124,6 +128,7 @@ rcl_wait_set_init(
   size_t number_of_timers,
   size_t number_of_clients,
   size_t number_of_services,
+  size_t number_of_events,
   rcl_context_t * context,
   rcl_allocator_t allocator);
 
@@ -289,7 +294,8 @@ rcl_wait_set_resize(
   size_t guard_conditions_size,
   size_t timers_size,
   size_t clients_size,
-  size_t services_size);
+  size_t services_size,
+  size_t events_size);
 
 /// Store a pointer to the guard condition in the next empty spot in the set.
 /**
@@ -343,6 +349,19 @@ rcl_wait_set_add_service(
   const rcl_service_t * service,
   size_t * index);
 
+/// Store a pointer to the event in the next empty spot in the set.
+/**
+ * This function behaves exactly the same as for subscriptions.
+ * \see rcl_wait_set_add_subscription
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_wait_set_add_event(
+  rcl_wait_set_t * wait_set,
+  const rcl_event_t * event,
+  size_t * index);
+
 /// Block until the wait set is ready or until the timeout has been exceeded.
 /**
  * This function will collect the items in the rcl_wait_set_t and pass them