*_raw function (#170)

* publish_raw function

* rcl_take_raw function

* comply to master

* add documentation

* use rcl_subscription_is_valid

* documentation fixups

* address review comments

* raw->serialized

rcl/include/rcl/publisher.h

@@ -252,6 +252,42 @@ RCL_WARN_UNUSED
 rcl_ret_t
 rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
 
+/// Publish a serialized message on a topic using a publisher.
+/**
+ * It is the job of the caller to ensure that the type of the serialized message
+ * parameter and the type associate with the publisher (via the type support)
+ * match.
+ * Even though this call to publish takes an already serialized serialized message,
+ * the publisher has to register its type as a ROS known message type.
+ * Passing a serialized message from a different type leads to undefined behavior on the subscriber side.
+ * The publish call might be able to send any abitrary serialized message, it is however
+ * not garantueed that the subscriber side successfully deserializes this byte stream.
+ *
+ * Apart from this, the `publish_serialized` function has the same behavior as `rcl_publish`
+ * expect that no serialization step is done.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | Yes [1]
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ * <i>[1] for unique pairs of publishers and messages, see above for more</i>
+ *
+ * \param[in] publisher handle to the publisher which will do the publishing
+ * \param[in] serialized_message  pointer to the already serialized message in raw form
+ * \return `RCL_RET_OK` if the message was published 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_publish_serialized_message(
+  const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message);
+
 /// Get the topic name for the publisher.
 /**
  * This function returns the publisher's internal topic name string.

rcl/include/rcl/subscription.h

@@ -256,6 +256,49 @@ rcl_take(
   void * ros_message,
   rmw_message_info_t * message_info);
 
+/// Take a serialized raw message from a topic using a rcl subscription.
+/**
+ * In contrast to `rcl_take`, this function stores the taken message in
+ * its raw binary representation.
+ * It is the job of the caller to ensure that the type associate with the subscription
+ * matches, and can optionally be deserialized into its ROS message via, the correct
+ * type support.
+ * If the `serialized_message` parameter contains enough preallocated memory, the incoming
+ * message can be taken without any additional memory allocation.
+ * If not, the function will dynamically allocate enough memory for the message.
+ * Passing a different type to rcl_take produces undefined behavior and cannot
+ * be checked by this function and therefore no deliberate error will occur.
+ *
+ * Apart from the differences above, this function behaves like `rcl_take`.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | Maybe [1]
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ * <i>[1] only if storage in the serialized_message is insufficient</i>
+ *
+ * \param[in] subscription the handle to the subscription from which to take
+ * \param[inout] serialized_message pointer to a (pre-allocated) serialized message.
+ * \param[out] message_info rmw struct which contains meta-data for the message
+ * \return `RCL_RET_OK` if the message was published, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_SUBSCRIPTION_TAKE_FAILED` if take failed but no error
+ *         occurred in the middleware, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_take_serialized_message(
+  const rcl_subscription_t * subscription,
+  rcl_serialized_message_t * serialized_message,
+  rmw_message_info_t * message_info);
+
 /// Get the topic name for the subscription.
 /**
  * This function returns the subscription's internal topic name string.

rcl/include/rcl/types.h

@@ -97,4 +97,7 @@ typedef rmw_ret_t rcl_ret_t;
 /// Argument is not a valid log level rule
 #define RCL_RET_INVALID_LOG_LEVEL_RULE 1020
 
+/// typedef for rmw_serialized_message_t;
+typedef rmw_serialized_message_t rcl_serialized_message_t;
+
 #endif  // RCL__TYPES_H_

rcl/src/rcl/publisher.c

@@ -244,6 +244,24 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+rcl_publish_serialized_message(
+  const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message)
+{
+  if (!rcl_publisher_is_valid(publisher, NULL)) {
+    return RCL_RET_PUBLISHER_INVALID;
+  }
+  rmw_ret_t ret = rmw_publish_serialized_message(publisher->impl->rmw_handle, serialized_message);
+  if (ret != RMW_RET_OK) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), rcl_get_default_allocator());
+    if (ret == RMW_RET_BAD_ALLOC) {
+      return RCL_RET_BAD_ALLOC;
+    }
+    return RMW_RET_ERROR;
+  }
+  return RCL_RET_OK;
+}
+
 const char *
 rcl_publisher_get_topic_name(const rcl_publisher_t * publisher)
 {

rcl/src/rcl/subscription.c

@@ -241,18 +241,12 @@ rcl_take(
   rmw_message_info_t * message_info)
 {
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking message")
-  RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
-  const rcl_subscription_options_t * options = rcl_subscription_get_options(subscription);
-  if (!options) {
-    return RCL_RET_SUBSCRIPTION_INVALID;
+  rcl_allocator_t error_allocator = rcl_get_default_allocator();
+  if (!rcl_subscription_is_valid(subscription, &error_allocator)) {
+    return RCL_RET_SUBSCRIPTION_INVALID;  // error message already set
   }
-  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT, options->allocator);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    subscription->impl, "subscription is invalid",
-    return RCL_RET_SUBSCRIPTION_INVALID, options->allocator);
-  RCL_CHECK_FOR_NULL_WITH_MSG(
-    subscription->impl->rmw_handle,
-    "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID, options->allocator);
+  RCL_CHECK_ARGUMENT_FOR_NULL(
+    ros_message, RCL_RET_INVALID_ARGUMENT, error_allocator);
   // 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;
@@ -261,7 +255,7 @@ rcl_take(
   rmw_ret_t ret =
     rmw_take_with_info(subscription->impl->rmw_handle, ros_message, &taken, message_info_local);
   if (ret != RMW_RET_OK) {
-    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), error_allocator);
     return RCL_RET_ERROR;
   }
   RCUTILS_LOG_DEBUG_NAMED(
@@ -272,6 +266,41 @@ rcl_take(
   return RCL_RET_OK;
 }
 
+rcl_ret_t
+rcl_take_serialized_message(
+  const rcl_subscription_t * subscription,
+  rcl_serialized_message_t * serialized_message,
+  rmw_message_info_t * message_info)
+{
+  RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking serialized message")
+  rcl_allocator_t error_allocator = rcl_get_default_allocator();
+  if (!rcl_subscription_is_valid(subscription, &error_allocator)) {
+    return RCL_RET_SUBSCRIPTION_INVALID;  // error message already set
+  }
+  RCL_CHECK_ARGUMENT_FOR_NULL(
+    serialized_message, RCL_RET_INVALID_ARGUMENT, error_allocator);
+  // 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.
+  bool taken = false;
+  rmw_ret_t ret = rmw_take_serialized_message_with_info(
+    subscription->impl->rmw_handle, serialized_message, &taken, message_info_local);
+  if (ret != RMW_RET_OK) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), error_allocator);
+    if (ret == RMW_RET_BAD_ALLOC) {
+      return RCL_RET_BAD_ALLOC;
+    }
+    return RCL_RET_ERROR;
+  }
+  RCUTILS_LOG_DEBUG_NAMED(
+    ROS_PACKAGE_NAME, "Subscription serialized take succeeded: %s", taken ? "true" : "false")
+  if (!taken) {
+    return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
+  }
+  return RCL_RET_OK;
+}
+
 const char *
 rcl_subscription_get_topic_name(const rcl_subscription_t * subscription)
 {