fix up publisher

rcl/include/rcl/publisher.h

@@ -114,7 +114,12 @@ rcl_get_zero_initialized_publisher();
  * \param[in] type_support type support object for the topic's type
  * \param[in] topic_name the name of the topic to publish on
  * \param[in] options publisher options, including quality of service settings
- * \return RMW_RET_OK if publisher was initialized successfully, otherwise RMW_RET_ERROR
+ * \return RCL_RET_OK if the publisher was initialized successfully, or
+ *         RCL_RET_NODE_INVALID if the node is invalid, or
+ *         RCL_RET_ALREADY_INIT if the publisher is already initialized, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_BAD_ALLOC if any arugments are invalid, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
  */
 rcl_ret_t
 rcl_publisher_init(
@@ -135,7 +140,9 @@ rcl_publisher_init(
  *
  * \param[inout] publisher handle to the publisher to be deinitialized
  * \param[in] node handle to the node used to create the publisher
- * \return RMW_RET_OK if publisher was deinitialized successfully, otherwise RMW_RET_ERROR
+ * \return RCL_RET_OK if publisher was finalized successfully, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
  */
 rcl_ret_t
 rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node);
@@ -144,13 +151,6 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node);
 rcl_publisher_options_t
 rcl_publisher_get_default_options();
 
-// TODO(wjwwood): I have an idea on how to do a type-checking macro version of
-//                publish using C11's _Generic "generic selection", but it
-//                needs support from the generated code (look into it later)
-//                It might also use static assert, see:
-//                  http://www.robertgamble.net/2012/01/c11-static-assertions.html
-// #define RCL_PUBLISH(publisher, ros_message) ...
-
 /// Publish a ROS message on a topic using a publisher.
 /* It is the job of the caller to ensure that the type of the ros_message
  * parameter and the type associate with the publisher (via the type support)
@@ -190,7 +190,10 @@ rcl_publisher_get_default_options();
  *
  * \param[in] publisher handle to the publisher which will do the publishing
  * \param[in] ros_message type-erased pointer to the ROS message
- * \return RMW_RET_OK if the message was published, otherwise RMW_RET_ERROR
+ * \return RCL_RET_OK if the message was published successfully, or
+ *         RCL_RET_INVALID_ARGUMENT if any arugments are invalid, or
+ *         RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or
+ *         RCL_RET_ERROR if an unspecified error occurs.
  */
 rcl_ret_t
 rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
@@ -231,7 +234,7 @@ rcl_publisher_get_topic_name(const rcl_publisher_t * publisher);
  * \return options struct if successful, otherwise NULL
  */
 const rcl_publisher_options_t *
-rcl_publisher_get_options(rcl_publisher_t * publisher);
+rcl_publisher_get_options(const rcl_publisher_t * publisher);
 
 /// Return the rmw publisher handle.
 /* The handle returned is a pointer to the internally held rmw handle.
@@ -254,7 +257,7 @@ rcl_publisher_get_options(rcl_publisher_t * publisher);
  * \return rmw publisher handle if successful, otherwise NULL
  */
 rmw_publisher_t *
-rcl_publisher_get_rmw_publisher_handle(rcl_publisher_t * publisher);
+rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher);
 
 #if __cplusplus
 }

rcl/include/rcl/types.h

@@ -29,6 +29,7 @@ typedef rmw_ret_t rcl_ret_t;
 // rcl node specific ret codes in 2XX
 #define RCL_RET_NODE_INVALID 200
 // rcl publisher specific ret codes in 3XX
+#define RCL_RET_PUBLISHER_INVALID 300
 // rcl subscription specific ret codes in 4XX
 // rcl service client specific ret codes in 5XX
 // rcl service server specific ret codes in 6XX

rcl/src/rcl/node.c

@@ -108,8 +108,8 @@ rcl_node_fini(rcl_node_t * node)
     return RCL_RET_OK;
   }
   rcl_ret_t result = RCL_RET_OK;
-  rmw_ret_t node_ret = rmw_destroy_node(node->impl->rmw_node_handle);
-  if (node_ret != RMW_RET_OK) {
+  rmw_ret_t ret = rmw_destroy_node(node->impl->rmw_node_handle);
+  if (ret != RMW_RET_OK) {
     RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
     result = RCL_RET_ERROR;
   }

rcl/src/rcl/publisher.c

@@ -86,6 +86,78 @@ fail:
   return fail_ret;
 }
 
+rcl_ret_t
+rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
+{
+  rcl_ret_t result = RCL_RET_OK;
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT);
+  if (publisher->impl) {
+    rmw_ret_t ret =
+      rmw_destroy_publisher(rcl_node_get_rmw_node_handle(node), publisher->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 = publisher->impl->options.allocator;
+    allocator.deallocate(publisher->impl, allocator.state);
+  }
+  return result;
+}
+
+rcl_publisher_options_t
+rcl_publisher_get_default_options()
+{
+  static rcl_publisher_options_t default_options = {
+    .qos = rmw_qos_profile_default,
+  };
+  default_options.allocator = rcl_get_default_allocator();
+  return default_options;
+}
+
+rcl_ret_t
+rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "publisher is invalid", return RCL_RET_PUBLISHER_INVALID);
+  if (rmw_publish(publisher->impl->rmw_handle, ros_message) != RMW_RET_OK) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string_safe());
+    return RCL_RET_ERROR;
+  }
+  return RCL_RET_OK;
+}
+
+const char *
+rcl_publisher_get_topic_name(const rcl_publisher_t * publisher)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "publisher is invalid", return NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl->rmw_handle, "publisher is invalid", return NULL);
+  return publisher->impl->rmw_handle->topic_name;
+}
+
+const rcl_publisher_options_t *
+rcl_publisher_get_options(const rcl_publisher_t * publisher)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "publisher is invalid", return NULL);
+  return &publisher->impl->options;
+}
+
+rmw_publisher_t *
+rcl_publisher_get_rmw_publisher_handle(const rcl_publisher_t * publisher)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(publisher, NULL);
+  RCL_CHECK_FOR_NULL_WITH_MSG(
+    publisher->impl, "publisher is invalid", return NULL);
+  return publisher->impl->rmw_handle;
+}
+
 #if __cplusplus
 }
 #endif