Implement QoS: liveliness, deadline, lifespan

rmw_connext_cpp/CMakeLists.txt

@@ -125,6 +125,8 @@ add_library(
   rmw_connext_cpp
   SHARED
   ${patched_files}
+  src/connext_static_publisher_info.cpp
+  src/connext_static_subscriber_info.cpp
   src/get_client.cpp
   src/get_participant.cpp
   src/get_publisher.cpp
@@ -135,6 +137,7 @@ add_library(
   src/rmw_client.cpp
   src/rmw_compare_gid_equals.cpp
   src/rmw_count.cpp
+  src/rmw_event.cpp
   src/rmw_get_gid_for_publisher.cpp
   src/rmw_get_implementation_identifier.cpp
   src/rmw_get_serialization_format.cpp

rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp

@@ -17,21 +17,43 @@
 
 #include <atomic>
 
+#include "rmw_connext_shared_cpp/ndds_include.hpp"
 #include "rmw_connext_shared_cpp/types.hpp"
+#include "rmw_connext_shared_cpp/connext_static_event_info.hpp"
+
+#include "ndds/ndds_cpp.h"
+#include "ndds/ndds_namespace_cpp.h"
 
 #include "rosidl_typesupport_connext_cpp/message_type_support.h"
 
+#include "rmw/types.h"
+#include "rmw/ret_types.h"
+
+
 class ConnextPublisherListener;
 
 extern "C"
 {
-struct ConnextStaticPublisherInfo
+struct ConnextStaticPublisherInfo : ConnextCustomEventInfo
 {
   DDS::Publisher * dds_publisher_;
   ConnextPublisherListener * listener_;
   DDS::DataWriter * topic_writer_;
   const message_type_support_callbacks_t * callbacks_;
   rmw_gid_t publisher_gid;
+  /**
+   * Remap the specific RTI Connext DDS DataWriter Status to a generic RMW status type.
+   *
+   * @param mask input status mask
+   * @param event
+   */
+  rmw_ret_t get_status(DDS::StatusMask mask, void * event) override;
+  /// Return the topic writer entity for this publisher
+  /**
+   *
+   * @return the topic writer associated with this publisher
+   */
+  DDS::Entity * get_entity() override;
 };
 }  // extern "C"
 

rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp

@@ -18,21 +18,40 @@
 #include <atomic>
 
 #include "rmw_connext_shared_cpp/ndds_include.hpp"
+#include "rmw_connext_shared_cpp/connext_static_event_info.hpp"
+#include "rmw_connext_shared_cpp/types.hpp"
+
+#include "ndds/ndds_cpp.h"
+#include "ndds/ndds_namespace_cpp.h"
 
 #include "rosidl_typesupport_connext_cpp/message_type_support.h"
+#include "rmw/types.h"
+#include "rmw/ret_types.h"
+
 
 class ConnextSubscriberListener;
 
 extern "C"
 {
-struct ConnextStaticSubscriberInfo
+struct ConnextStaticSubscriberInfo : ConnextCustomEventInfo
 {
   DDS::Subscriber * dds_subscriber_;
   ConnextSubscriberListener * listener_;
   DDS::DataReader * topic_reader_;
   DDS::ReadCondition * read_condition_;
   bool ignore_local_publications;
   const message_type_support_callbacks_t * callbacks_;
+  /// Remap the specific RTI Connext DDS DataReader Status to a generic RMW status type.
+  /**
+   * @param mask input status mask
+   * @param event
+   */
+  rmw_ret_t get_status(DDS::StatusMask mask, void * event) override;
+  /// Return the topic reader entity for this subsciber
+  /**
+   * @return the topic reader associated with this subscriber
+   */
+  DDS::Entity * get_entity() override;
 };
 }  // extern "C"
 

rmw_connext_cpp/src/connext_static_publisher_info.cpp

@@ -0,0 +1,70 @@
+// Copyright 2015-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.
+
+
+#include "rmw_connext_cpp/connext_static_publisher_info.hpp"
+#include "rmw_connext_shared_cpp/event_converter.hpp"
+
+rmw_ret_t ConnextStaticPublisherInfo::get_status(
+  DDS::StatusMask mask,
+  void * event)
+{
+  switch (mask) {
+    case DDS::StatusKind::DDS_LIVELINESS_LOST_STATUS:
+      {
+        DDS::LivelinessLostStatus liveliness_lost;
+        DDS::ReturnCode_t dds_return_code =
+          topic_writer_->get_liveliness_lost_status(liveliness_lost);
+
+        rmw_ret_t from_dds = check_dds_ret_code(dds_return_code);
+        if (from_dds != RMW_RET_OK) {
+          return from_dds;
+        }
+
+        rmw_liveliness_lost_status_t * rmw_liveliness_lost =
+          static_cast<rmw_liveliness_lost_status_t *>(event);
+        rmw_liveliness_lost->total_count = liveliness_lost.total_count;
+        rmw_liveliness_lost->total_count_change = liveliness_lost.total_count_change;
+
+        break;
+      }
+    case DDS::StatusKind::DDS_OFFERED_DEADLINE_MISSED_STATUS:
+      {
+        DDS::OfferedDeadlineMissedStatus offered_deadline_missed;
+        DDS::ReturnCode_t dds_return_code = topic_writer_
+          ->get_offered_deadline_missed_status(offered_deadline_missed);
+
+        rmw_ret_t from_dds = check_dds_ret_code(dds_return_code);
+        if (from_dds != RMW_RET_OK) {
+          return from_dds;
+        }
+
+        rmw_offered_deadline_missed_status_t * rmw_offered_deadline_missed =
+          static_cast<rmw_offered_deadline_missed_status_t *>(event);
+        rmw_offered_deadline_missed->total_count = offered_deadline_missed.total_count;
+        rmw_offered_deadline_missed->total_count_change =
+          offered_deadline_missed.total_count_change;
+
+        break;
+      }
+    default:
+      return RMW_RET_UNSUPPORTED;
+  }
+  return RMW_RET_OK;
+}
+
+DDS::Entity * ConnextStaticPublisherInfo::get_entity()
+{
+  return topic_writer_;
+}

rmw_connext_cpp/src/connext_static_subscriber_info.cpp

@@ -0,0 +1,72 @@
+// Copyright 2014-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.
+
+#include "rmw_connext_cpp/connext_static_subscriber_info.hpp"
+#include "rmw_connext_shared_cpp/event_converter.hpp"
+
+rmw_ret_t ConnextStaticSubscriberInfo::get_status(
+  DDS::StatusMask mask,
+  void * event)
+{
+  switch (mask) {
+    case DDS::StatusKind::DDS_LIVELINESS_CHANGED_STATUS:
+      {
+        DDS::LivelinessChangedStatus liveliness_changed;
+        DDS::ReturnCode_t dds_return_code = topic_reader_
+          ->get_liveliness_changed_status(liveliness_changed);
+
+        rmw_ret_t from_dds = check_dds_ret_code(dds_return_code);
+        if (from_dds != RMW_RET_OK) {
+          return from_dds;
+        }
+
+        rmw_liveliness_changed_status_t * rmw_liveliness_changed_status =
+          static_cast<rmw_liveliness_changed_status_t *>(event);
+        rmw_liveliness_changed_status->alive_count = liveliness_changed.alive_count;
+        rmw_liveliness_changed_status->not_alive_count = liveliness_changed.not_alive_count;
+        rmw_liveliness_changed_status->alive_count_change = liveliness_changed.alive_count_change;
+        rmw_liveliness_changed_status->not_alive_count_change =
+          liveliness_changed.not_alive_count_change;
+
+        break;
+      }
+    case DDS::StatusKind::DDS_REQUESTED_DEADLINE_MISSED_STATUS:
+      {
+        DDS::RequestedDeadlineMissedStatus requested_deadline_missed;
+        DDS::ReturnCode_t dds_return_code = topic_reader_
+          ->get_requested_deadline_missed_status(requested_deadline_missed);
+
+        rmw_ret_t from_dds = check_dds_ret_code(dds_return_code);
+        if (from_dds != RMW_RET_OK) {
+          return from_dds;
+        }
+
+        rmw_requested_deadline_missed_status_t * rmw_requested_deadline_missed_status =
+          static_cast<rmw_requested_deadline_missed_status_t *>(event);
+        rmw_requested_deadline_missed_status->total_count = requested_deadline_missed.total_count;
+        rmw_requested_deadline_missed_status->total_count_change =
+          requested_deadline_missed.total_count_change;
+
+        break;
+      }
+    default:
+      return RMW_RET_UNSUPPORTED;
+  }
+  return RMW_RET_OK;
+}
+
+DDS::Entity * ConnextStaticSubscriberInfo::get_entity()
+{
+  return topic_reader_;
+}

rmw_connext_cpp/src/rmw_event.cpp

@@ -0,0 +1,46 @@
+// Copyright 2014-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.
+
+#include "rmw/rmw.h"
+
+#include "rmw_connext_shared_cpp/event.hpp"
+
+#include "rmw_connext_cpp/identifier.hpp"
+
+
+extern "C"
+{
+/*
+ * Take an event from the event handle.
+ *
+ * \param event_handle event object to take from
+ * \param event_info event info object to write taken data into
+ * \param taken boolean flag indicating if an event was taken or not
+ * \return `RMW_RET_OK` if successful, or
+ * \return `RMW_RET_BAD_ALLOC` if memory allocation failed, or
+ * \return `RMW_RET_ERROR` if an unexpected error occurs.
+ */
+rmw_ret_t
+rmw_take_event(
+  const rmw_event_t * event_handle,
+  void * event_info,
+  bool * taken)
+{
+  return __rmw_take_event(
+    rti_connext_identifier,
+    event_handle,
+    event_info,
+    taken);
+}
+}  // extern "C"

rmw_connext_cpp/src/rmw_node.cpp

@@ -40,6 +40,12 @@ rmw_destroy_node(rmw_node_t * node)
   return destroy_node(rti_connext_identifier, node);
 }
 
+rmw_ret_t
+rmw_node_assert_liveliness(const rmw_node_t * node)
+{
+  return assert_liveliness(rti_connext_identifier, node);
+}
+
 const rmw_guard_condition_t *
 rmw_node_get_graph_guard_condition(const rmw_node_t * node)
 {

rmw_connext_cpp/src/rmw_publisher.cpp

@@ -405,6 +405,29 @@ rmw_publisher_get_actual_qos(
   return RMW_RET_OK;
 }
 
+rmw_ret_t
+rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT);
+
+  auto info = static_cast<ConnextStaticPublisherInfo *>(publisher->data);
+  if (nullptr == info) {
+    RMW_SET_ERROR_MSG("publisher internal data is invalid");
+    return RMW_RET_ERROR;
+  }
+  if (nullptr == info->topic_writer_) {
+    RMW_SET_ERROR_MSG("publisher internal datawriter is invalid");
+    return RMW_RET_ERROR;
+  }
+
+  if (info->topic_writer_->assert_liveliness() != DDS::RETCODE_OK) {
+    RMW_SET_ERROR_MSG("failed to assert liveliness of datawriter");
+    return RMW_RET_ERROR;
+  }
+
+  return RMW_RET_OK;
+}
+
 rmw_ret_t
 rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher)
 {

rmw_connext_cpp/src/rmw_wait.cpp

@@ -29,11 +29,12 @@ rmw_wait(
   rmw_guard_conditions_t * guard_conditions,
   rmw_services_t * services,
   rmw_clients_t * clients,
+  rmw_events_t * events,
   rmw_wait_set_t * wait_set,
   const rmw_time_t * wait_timeout)
 {
   return wait<ConnextStaticSubscriberInfo, ConnextStaticServiceInfo, ConnextStaticClientInfo>(
-    rti_connext_identifier, subscriptions, guard_conditions, services, clients, wait_set,
+    rti_connext_identifier, subscriptions, guard_conditions, services, clients, events, wait_set,
     wait_timeout);
 }
 }  // extern "C"

rmw_connext_shared_cpp/CMakeLists.txt

@@ -39,6 +39,8 @@ add_library(
   src/condition_error.cpp
   src/count.cpp
   src/demangle.cpp
+  src/event.cpp
+  src/event_converter.cpp
   src/guard_condition.cpp
   src/init.cpp
   src/namespace_prefix.cpp