Add rmw_publisher_wait_for_all_acked support (#20)

Signed-off-by: Barry Xu <barry.xu@sony.com>

rmw_connextdds/src/rmw_api_impl_ndds.cpp

@@ -413,6 +413,17 @@ rmw_publisher_assert_liveliness(
 }
 
 
+rmw_ret_t
+rmw_publisher_wait_for_all_acked(
+  const rmw_publisher_t * publisher,
+  rmw_time_t wait_timeout
+)
+{
+  return rmw_api_connextdds_publisher_wait_for_all_acked(
+    publisher, wait_timeout);
+}
+
+
 rmw_ret_t
 rmw_publisher_get_actual_qos(
   const rmw_publisher_t * publisher,

rmw_connextdds_common/include/rmw_connextdds/dds_api_rtime.hpp

@@ -70,4 +70,7 @@ struct DDS_LifespanQosPolicy;
 #define DDS_InstanceHandle_compare_prefix(ih_a_, ih_b_) \
   memcmp((ih_a_)->octet, (ih_b_)->octet, 12)
 
+// Not available function
+#define DDS_DataWriter_wait_for_acknowledgments(writer_, timeout_)    DDS_RETCODE_UNSUPPORTED
+
 #endif  // RMW_CONNEXTDDS__DDS_API_RTIME_HPP_

rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp

@@ -284,6 +284,12 @@ rmw_ret_t
 rmw_api_connextdds_publisher_assert_liveliness(
   const rmw_publisher_t * publisher);
 
+RMW_CONNEXTDDS_PUBLIC
+rmw_ret_t
+rmw_api_connextdds_publisher_wait_for_all_acked(
+  const rmw_publisher_t * publisher,
+  rmw_time_t wait_timeout);
+
 RMW_CONNEXTDDS_PUBLIC
 rmw_ret_t
 rmw_api_connextdds_publisher_get_actual_qos(

rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp

@@ -64,6 +64,9 @@ bool rmw_connextdds_find_string_in_list(
   const DDS_StringSeq * const values,
   const char * const value);
 
+DDS_Duration_t rmw_connextdds_duration_from_ros_time(
+  const rmw_time_t * const ros_time);
+
 /******************************************************************************
  * WaitSet wrapper
  ******************************************************************************/
@@ -197,6 +200,9 @@ class RMW_Connext_Publisher
   rmw_ret_t
   assert_liveliness();
 
+  rmw_ret_t
+  wait_for_all_acked(rmw_time_t wait_timeout);
+
   rmw_ret_t
   qos(rmw_qos_profile_t * const qos);
 

rmw_connextdds_common/src/common/rmw_impl.cpp

@@ -221,6 +221,22 @@ rmw_connextdds_find_string_in_list(
   return false;
 }
 
+DDS_Duration_t
+rmw_connextdds_duration_from_ros_time(
+  const rmw_time_t * const ros_time)
+{
+  if (rmw_time_equal(*ros_time, RMW_DURATION_INFINITE)) {
+    return DDS_DURATION_INFINITE;
+  }
+
+  rmw_time_t in_time = rmw_dds_common::clamp_rmw_time_to_dds_time(*ros_time);
+
+  DDS_Duration_t duration;
+  duration.sec = static_cast<DDS_Long>(in_time.sec);
+  duration.nanosec = static_cast<DDS_UnsignedLong>(in_time.nsec);
+  return duration;
+}
+
 /******************************************************************************
  * Qos Helpers
  ******************************************************************************/
@@ -927,6 +943,28 @@ RMW_Connext_Publisher::assert_liveliness()
   return RMW_RET_OK;
 }
 
+rmw_ret_t
+RMW_Connext_Publisher::wait_for_all_acked(rmw_time_t wait_timeout)
+{
+  DDS_Duration_t timeout = rmw_connextdds_duration_from_ros_time(&wait_timeout);
+
+  const DDS_ReturnCode_t dds_rc =
+    DDS_DataWriter_wait_for_acknowledgments(this->dds_writer, &timeout);
+
+  switch (dds_rc) {
+    case DDS_RETCODE_OK:
+      return RMW_RET_OK;
+    case DDS_RETCODE_TIMEOUT:
+      return RMW_RET_TIMEOUT;
+    default:
+      {
+        RMW_CONNEXT_LOG_ERROR_A_SET(
+          "failed to wait for reader acknowledgements: dds_rc=%d", dds_rc)
+        return RMW_RET_ERROR;
+      }
+  }
+}
+
 rmw_ret_t
 RMW_Connext_Publisher::qos(rmw_qos_profile_t * const qos)
 {

rmw_connextdds_common/src/common/rmw_publication.cpp

@@ -283,6 +283,26 @@ rmw_api_connextdds_publisher_assert_liveliness(
 }
 
 
+rmw_ret_t
+rmw_api_connextdds_publisher_wait_for_all_acked(
+  const rmw_publisher_t * publisher,
+  rmw_time_t wait_timeout)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    publisher,
+    publisher->implementation_identifier,
+    RMW_CONNEXTDDS_ID,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+
+  RMW_Connext_Publisher * const pub_impl =
+    reinterpret_cast<RMW_Connext_Publisher *>(publisher->data);
+  RMW_CHECK_ARGUMENT_FOR_NULL(pub_impl, RMW_RET_INVALID_ARGUMENT);
+
+  return pub_impl->wait_for_all_acked(wait_timeout);
+}
+
+
 rmw_ret_t
 rmw_api_connextdds_publisher_get_actual_qos(
   const rmw_publisher_t * publisher,

rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp

@@ -413,6 +413,17 @@ rmw_publisher_assert_liveliness(
 }
 
 
+rmw_ret_t
+rmw_publisher_wait_for_all_acked(
+  const rmw_publisher_t * publisher,
+  rmw_time_t wait_timeout
+)
+{
+  return rmw_api_connextdds_publisher_wait_for_all_acked(
+    publisher, wait_timeout);
+}
+
+
 rmw_ret_t
 rmw_publisher_get_actual_qos(
   const rmw_publisher_t * publisher,