Implement QoS: liveliness, deadline, lifespan

rmw_opensplice_cpp/CMakeLists.txt

@@ -45,13 +45,15 @@ ament_export_dependencies(
 link_directories(${OpenSplice_LIBRARY_DIRS})
 add_library(rmw_opensplice_cpp SHARED
   src/demangle.cpp
+  src/event_converter.cpp
   src/identifier.cpp
   src/namespace_prefix.cpp
   src/qos.cpp
   src/rmw_logging.cpp
   src/rmw_client.cpp
   src/rmw_compare_gids_equal.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_opensplice_cpp/src/event_converter.cpp

@@ -0,0 +1,52 @@
+// Copyright 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 "event_converter.hpp"
+
+#include <dcps/C++/SACPP/ccpp.h>
+#include <rmw/event.h>
+
+#include <unordered_map>
+
+/// mapping of RMW_EVENT to the corresponding DDS::StatusKind
+static const std::unordered_map<rmw_event_type_t, DDS::StatusKind> mask_map{
+  {RMW_EVENT_LIVELINESS_CHANGED, DDS::LIVELINESS_CHANGED_STATUS},
+  {RMW_EVENT_REQUESTED_DEADLINE_MISSED, DDS::REQUESTED_DEADLINE_MISSED_STATUS},
+  {RMW_EVENT_LIVELINESS_LOST, DDS::LIVELINESS_LOST_STATUS},
+  {RMW_EVENT_OFFERED_DEADLINE_MISSED, DDS::OFFERED_DEADLINE_MISSED_STATUS},
+};
+
+DDS::StatusKind get_status_kind_from_rmw(const rmw_event_type_t event_t)
+{
+  return mask_map.at(event_t);
+}
+
+bool is_event_supported(const rmw_event_type_t event_t)
+{
+  return mask_map.count(event_t) > 0;
+}
+
+rmw_ret_t check_dds_ret_code(const DDS::ReturnCode_t dds_return_code)
+{
+  switch (dds_return_code) {
+    case DDS::RETCODE_OK:
+      return RMW_RET_OK;
+    case DDS::RETCODE_ERROR:
+      return RMW_RET_ERROR;
+    case DDS::RETCODE_TIMEOUT:
+      return RMW_RET_TIMEOUT;
+    default:
+      return RMW_RET_ERROR;
+  }
+}

rmw_opensplice_cpp/src/event_converter.hpp

@@ -0,0 +1,43 @@
+// Copyright 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.
+
+#ifndef EVENT_CONVERTER_HPP_
+#define EVENT_CONVERTER_HPP_
+
+#include <dcps/C++/SACPP/ccpp.h>
+#include <rmw/event.h>
+
+/// Return the corresponding DDS_StatusKind to the input RMW_EVENT.
+/**
+ * \param event_t to translate to status kind
+ * \return the StatusKind corresponding to the rmw_event_type_t
+ */
+DDS::StatusKind get_status_kind_from_rmw(const rmw_event_type_t event_t);
+
+/// Return true if the input RMW event has a corresponding DDS_StatusKind.
+/**
+ * \param event_t input rmw event to check
+ * \return true if there is an RMW to DDS_StatusKind mapping, false otherwise
+ */
+bool is_event_supported(const rmw_event_type_t event_t);
+
+/// Assign the input DDS return code to its corresponding RMW return code.
+/**
+  * \param dds_return_code input DDS return code
+  * \return to_return the corresponding rmw_ret_t that maps to the input DDS_ReturnCode_t.
+  *   By default RMW_RET_ERROR is returned if no corresponding rmw_ret_t is not defined.
+  */
+rmw_ret_t check_dds_ret_code(const DDS::ReturnCode_t dds_return_code);
+
+#endif  // EVENT_CONVERTER_HPP_

rmw_opensplice_cpp/src/opensplice_static_event_info.hpp

@@ -0,0 +1,40 @@
+// Copyright 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.
+
+#ifndef OPENSPLICE_STATIC_EVENT_INFO_HPP_
+#define OPENSPLICE_STATIC_EVENT_INFO_HPP_
+
+#include <dcps/C++/SACPP/ccpp.h>
+#include <rmw/ret_types.h>
+
+typedef struct OpenSpliceStaticEventInfo
+{
+  /// Get the status for a particular status mask.
+  /**
+   * Return the corresponding RMW status given the input DDS_StatusMask and its corresponding event.
+   * \param mask input DDS_StatusMask
+   * \param event to fill
+   * \return `RMW_RET_OK` if the event was filled or
+   * \return `RMW_RET_UNSUPPORTED` if the status mask is unsupported
+   */
+  virtual rmw_ret_t get_status(const DDS::StatusMask mask, void * event) = 0;
+
+  /// Get dds entity for events.
+  /**
+   * \return the entity for which events are retrieved from.
+   */
+  virtual DDS::Entity * get_entity() = 0;
+} OpenSpliceStaticEventInfo;
+
+#endif  // OPENSPLICE_STATIC_EVENT_INFO_HPP_

rmw_opensplice_cpp/src/qos.cpp

@@ -23,7 +23,7 @@ using rosidl_typesupport_opensplice_cpp::impl::check_get_default_datareader_qos;
 using rosidl_typesupport_opensplice_cpp::impl::check_get_default_datawriter_qos;
 
 template<typename DDSEntityQos>
-bool set_entity_qos_from_profile(
+bool set_entity_qos_from_profile_generic(
   const rmw_qos_profile_t & qos_profile,
   DDSEntityQos & entity_qos)
 {
@@ -74,6 +74,35 @@ bool set_entity_qos_from_profile(
       static_cast<DDS::Long>(qos_profile.depth);
   }
 
+  // DDS_DeadlineQosPolicy has default value of DDS_DURATION_INFINITE, don't touch it for 0
+  if (qos_profile.deadline.sec != 0 || qos_profile.deadline.nsec != 0) {
+    entity_qos.deadline.period.sec = qos_profile.deadline.sec;
+    entity_qos.deadline.period.nanosec = qos_profile.deadline.nsec;
+  }
+
+  switch (qos_profile.liveliness) {
+    case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC:
+      entity_qos.liveliness.kind = DDS::AUTOMATIC_LIVELINESS_QOS;
+      break;
+    case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE:
+      entity_qos.liveliness.kind = DDS::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS;
+      break;
+    case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC:
+      entity_qos.liveliness.kind = DDS::MANUAL_BY_TOPIC_LIVELINESS_QOS;
+      break;
+    case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT:
+      break;
+    default:
+      RMW_SET_ERROR_MSG("Unknown QoS liveliness policy");
+      return false;
+  }
+  if (qos_profile.liveliness_lease_duration.sec != 0 ||
+    qos_profile.liveliness_lease_duration.nsec != 0)
+  {
+    entity_qos.liveliness.lease_duration.sec = qos_profile.liveliness_lease_duration.sec;
+    entity_qos.liveliness.lease_duration.nanosec = qos_profile.liveliness_lease_duration.nsec;
+  }
+
   // ensure the history depth is at least the requested queue size
   assert(entity_qos.history.depth >= 0);
   if (
@@ -91,6 +120,28 @@ bool set_entity_qos_from_profile(
   return true;
 }
 
+bool
+set_entity_qos_from_profile(
+  const rmw_qos_profile_t & qos_profile,
+  DDS::DataReaderQos & entity_qos)
+{
+  // Set any QoS settings that are specific to DataReader, then call the shared version
+  return set_entity_qos_from_profile_generic(qos_profile, entity_qos);
+}
+
+bool
+set_entity_qos_from_profile(
+  const rmw_qos_profile_t & qos_profile,
+  DDS::DataWriterQos & entity_qos)
+{
+  // Set any QoS settings that are specific to DataWriter, then call the shared version
+  if (qos_profile.lifespan.sec != 0 || qos_profile.lifespan.nsec != 0) {
+    entity_qos.lifespan.duration.sec = qos_profile.lifespan.sec;
+    entity_qos.lifespan.duration.nanosec = qos_profile.lifespan.nsec;
+  }
+  return set_entity_qos_from_profile_generic(qos_profile, entity_qos);
+}
+
 bool get_datareader_qos(
   DDS::Subscriber * subscriber,
   const rmw_qos_profile_t & qos_profile,

rmw_opensplice_cpp/src/rmw_event.cpp

@@ -0,0 +1,78 @@
+// Copyright 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 <dds_dcps.h>
+
+#include "rmw/allocators.h"
+#include "rmw/error_handling.h"
+#include "rmw/impl/cpp/macros.hpp"
+#include "rmw/event.h"
+#include "rmw/rmw.h"
+#include "rmw/types.h"
+
+#include "event_converter.hpp"
+#include "identifier.hpp"
+#include "types.hpp"
+
+extern "C"
+{
+/// Take an event from the event handle and store it in event_info.
+/*
+ * 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)
+{
+  // pointer error checking here
+  RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    event handle,
+    event_handle->implementation_identifier, opensplice_cpp_identifier,
+    return RMW_RET_ERROR);
+  RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT);
+
+  rmw_ret_t ret_code = RMW_RET_UNSUPPORTED;
+
+  // check if we support the input event type
+  if (is_event_supported(event_handle->event_type)) {
+    // lookup status mask from rmw_event_type
+    DDS::StatusKind status_kind = get_status_kind_from_rmw(event_handle->event_type);
+
+    // cast the event_handle to the appropriate type to get the appropriate
+    // status from the handle
+    // CustomConnextPublisher and CustomConnextSubscriber should implement this interface
+    OpenSpliceStaticEventInfo * custom_event_info =
+      static_cast<OpenSpliceStaticEventInfo *>(event_handle->data);
+
+    // call get status with the appropriate mask
+    // get_status should fill the event with the appropriate status information
+    ret_code = custom_event_info->get_status(status_kind, event_info);
+  }
+
+  // if ret_code is not okay, return error and set taken to false.
+  *taken = (ret_code == RMW_RET_OK);
+  return ret_code;
+}
+}  // extern "C"

rmw_opensplice_cpp/src/rmw_node.cpp

@@ -35,6 +35,7 @@
 #include "rmw/rmw.h"
 #include "rmw/types.h"
 
+#include "event_converter.hpp"
 #include "identifier.hpp"
 #include "types.hpp"
 
@@ -418,6 +419,28 @@ rmw_destroy_node(rmw_node_t * node)
   return result;
 }
 
+rmw_ret_t
+rmw_node_assert_liveliness(const rmw_node_t * node)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
+
+  auto node_info = static_cast<OpenSpliceStaticNodeInfo *>(node->data);
+  if (nullptr == node_info) {
+    RMW_SET_ERROR_MSG("node info handle is null");
+    return RMW_RET_ERROR;
+  }
+  if (nullptr == node_info->participant) {
+    RMW_SET_ERROR_MSG("node internal participant is invalid");
+    return RMW_RET_ERROR;
+  }
+
+  rmw_ret_t result = check_dds_ret_code(node_info->participant->assert_liveliness());
+  if (RMW_RET_OK != result) {
+    RMW_SET_ERROR_MSG("failed to assert liveliness of participant");
+  }
+  return result;
+}
+
 const rmw_guard_condition_t *
 rmw_node_get_graph_guard_condition(const rmw_node_t * node)
 {