implement subscriptions

rcl/include/rcl/subscription.h

@@ -156,7 +156,7 @@ rcl_subscription_get_default_options();
 
 /// Take a ROS message from a topic using a rcl subscription.
 /* It is the job of the caller to ensure that the type of the ros_message
- * parameter and the type associate with the subscription, via the type
+ * argument and the type associate with the subscription, via the type
  * support, match.
  * Passing a different type to rcl_take produces undefined behavior and cannot
  * be checked by this function and therefore no deliberate error will occur.
@@ -164,6 +164,7 @@ rcl_subscription_get_default_options();
  * TODO(wjwwood) blocking of take?
  * TODO(wjwwood) pre-, during-, and post-conditions for message ownership?
  * TODO(wjwwood) is rcl_take thread-safe?
+ * TODO(wjwwood) Should there be an rcl_message_info_t?
  *
  * The ros_message pointer should point to an already allocated ROS message
  * struct of the correct type, into which the taken ROS message will be copied
@@ -181,9 +182,9 @@ rcl_subscription_get_default_options();
  * message instance, like what the GUID of the publisher which published it
  * originally or whether or not the message received from within the same
  * process.
- * The message_info parameter should be an already allocated rmw_message_info_t
+ * The message_info argument should be an already allocated rmw_message_info_t
  * structure.
- * Passing NULL for message_info will be ignored.
+ * Passing NULL for message_info will result in the argument being ignored.
  *
  * \param[in] subscription the handle to the subscription from which to take
  * \param[inout] ros_message type-erased ptr to a allocated ROS message
@@ -191,6 +192,7 @@ rcl_subscription_get_default_options();
  * \param[out] message_info rmw struct which contains meta-data for the message
  * \return RCL_RET_OK if the message was published, or
  *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or
  *         RCL_RET_BAD_ALLOC if allocating memory failed, or
  *         RCL_RET_ERROR if an unspecified error occurs.
  */
@@ -205,7 +207,7 @@ rcl_take(
 /* This function returns the subscription's internal topic name string.
  * This function can fail, and therefore return NULL, if the:
  *   - subscription is NULL
- *   - subscription is invalid (never called init, called fini, or invalid node)
+ *   - subscription is invalid (never called init, called fini, or invalid)
  *
  * The returned string is only valid as long as the subscription is valid.
  * The value of the string may change if the topic name changes, and therefore
@@ -224,7 +226,7 @@ rcl_subscription_get_topic_name(const rcl_subscription_t * subscription);
 /* This function returns the subscription's internal options struct.
  * This function can fail, and therefore return NULL, if the:
  *   - subscription is NULL
- *   - subscription is invalid (never called init, called fini, or invalid node)
+ *   - subscription is invalid (never called init, called fini, or invalid)
  *
  * The returned struct is only valid as long as the subscription is valid.
  * The values in the struct may change if the subscription's options change,
@@ -243,7 +245,7 @@ rcl_subscription_get_options(rcl_subscription_t * subscription);
 /* The handle returned is a pointer to the internally held rmw handle.
  * This function can fail, and therefore return NULL, if the:
  *   - subscription is NULL
- *   - subscription is invalid (never called init, called fini, or invalid node)
+ *   - subscription is invalid (never called init, called fini, or invalid)
  *
  * The returned handle is only valid as long as the rcl subscription is valid.
  *

rcl/include/rcl/types.h

@@ -31,6 +31,7 @@ typedef rmw_ret_t rcl_ret_t;
 // rcl publisher specific ret codes in 3XX
 #define RCL_RET_PUBLISHER_INVALID 300
 // rcl subscription specific ret codes in 4XX
+#define RCL_RET_SUBSCRIPTION_INVALID 400
 // rcl service client specific ret codes in 5XX
 // rcl service server specific ret codes in 6XX
 // rcl guard condition specific ret codes in 7XX

rcl/src/rcl/common.h

@@ -23,8 +23,8 @@ extern "C"
 #include "rcl/error_handling.h"
 #include "rcl/types.h"
 
-#define RCL_CHECK_ARGUMENT_FOR_NULL(parameter, error_return_type) \
-  RCL_CHECK_FOR_NULL_WITH_MSG(parameter, #parameter " argument is null", return error_return_type)
+#define RCL_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type) \
+  RCL_CHECK_FOR_NULL_WITH_MSG(argument, #argument " argument is null", return error_return_type)
 
 #define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) if (!value) { \
   RCL_SET_ERROR_MSG(msg); \

rcl/src/rcl/publisher.c

@@ -108,9 +108,9 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
 rcl_publisher_options_t
 rcl_publisher_get_default_options()
 {
-  static rcl_publisher_options_t default_options = {
-    .qos = rmw_qos_profile_default,
-  };
+  static rcl_publisher_options_t default_options;
+  // Must set the allocator and qos after because they are not a compile time constant.
+  default_options.qos = rmw_qos_profile_default;
   default_options.allocator = rcl_get_default_allocator();
   return default_options;
 }

rcl/src/rcl/subscription.c

@@ -19,7 +19,160 @@ extern "C"
 
 #include "rcl/subscription.h"
 
+#include "rmw/rmw.h"
+#include "./common.h"
 
+typedef struct rcl_subscription_impl_t {
+  rcl_subscription_options_t options;
+  rmw_subscription_t * rmw_handle;
+} rcl_subscription_impl_t;
+
+rcl_subscription_t
+rcl_get_zero_initialized_subscription()
+{
+  static rcl_subscription_t null_subscription = {0};
+  return null_subscription;
+}
+
+rcl_ret_t
+rcl_subscription_init(
+  rcl_subscription_t * subscription,
+  const rcl_node_t * node,
+  const rosidl_message_type_support_t * type_support,
+  const char * topic_name,
+  const rcl_subscription_options_t * options)
+{
+  rcl_ret_t fail_ret = RCL_RET_ERROR;
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT);
+  if (subscription->impl) {
+    RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninialized");
+    return RCL_RET_ALREADY_INIT;
+  }
+  const rcl_allocator_t * allocator = &options->allocator;
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator->allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    allocator->deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT);
+  // Allocate memory for the implementation struct.
+  subscription->impl = (rcl_subscription_impl_t *)allocator->allocate(
+    sizeof(rcl_subscription_impl_t), allocator->state);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    subscription->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC);
+  // Fill out the implemenation struct.
+  // rmw_handle
+  // TODO(wjwwood): pass allocator once supported in rmw api.
+  subscription->impl->rmw_handle = rmw_create_subscription(
+    rcl_node_get_rmw_node_handle(node),
+    type_support,
+    topic_name,
+    &rmw_qos_profile_default,
+    options->ignore_local_publications);
+  if (!subscription->impl->rmw_handle) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    goto fail;
+  }
+  // options
+  subscription->impl->options = *options;
+  return RCL_RET_OK;
+fail:
+  if (subscription->impl) {
+    allocator->deallocate(subscription->impl, allocator->state);
+  }
+  return fail_ret;
+}
+
+rcl_ret_t
+rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node)
+{
+  rcl_ret_t result = RCL_RET_OK;
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  if (subscription->impl) {
+    rmw_ret_t ret =
+      rmw_destroy_subscription(rcl_node_get_rmw_node_handle(node), subscription->impl->rmw_handle);
+    if (ret != RMW_RET_OK) {
+      RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+      result = RCL_RET_ERROR;
+    }
+    rcl_allocator_t allocator = subscription->impl->options.allocator;
+    allocator.deallocate(subscription->impl, allocator.state);
+  }
+  return result;
+}
+
+rcl_subscription_options_t
+rcl_subscription_get_default_options()
+{
+  static rcl_subscription_options_t default_options = {
+    .ignore_local_publications = false,
+  };
+  // Must set the allocator and qos after because they are not a compile time constant.
+  default_options.qos = rmw_qos_profile_default;
+  default_options.allocator = rcl_get_default_allocator();
+  return default_options;
+}
+
+rcl_ret_t
+rcl_take(
+  const rcl_subscription_t * subscription,
+  void * ros_message,
+  bool * taken,
+  rmw_message_info_t * message_info)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(taken, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    subscription->impl, "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    subscription->impl->rmw_handle,
+    "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID);
+  // If message_info is NULL, use a place holder which can be discarded.
+  rmw_message_info_t dummy_message_info;
+  rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
+  // Call rmw_take_with_info.
+  rmw_ret_t ret =
+    rmw_take_with_info(subscription->impl->rmw_handle, ros_message, taken, message_info);
+  if (ret != RMW_RET_OK) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    return RCL_RET_ERROR;
+  }
+  return RCL_RET_OK;
+}
+
+const char *
+rcl_subscription_get_topic_name(const rcl_subscription_t * subscription)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    subscription->impl, "subscription is invalid", return NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    subscription->impl->rmw_handle,
+    "subscription is invalid", return NULL);
+  return subscription->impl->rmw_handle->topic_name;
+}
+
+const rcl_subscription_options_t *
+rcl_subscription_get_options(rcl_subscription_t * subscription)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    subscription->impl, "subscription is invalid", return NULL);
+  return &subscription->impl->options;
+}
+
+rmw_subscription_t *
+rcl_subscription_get_rmw_subscription_handle(rcl_subscription_t * subscription)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    subscription->impl, "subscription is invalid", return NULL);
+  return subscription->impl->rmw_handle;
+}
 
 #if __cplusplus
 }