Add sequence numbers to message info structure (#74)

* Fill reception_sequence_number/publication_sequence_number in all rmw_take_*_with_info() functions Signed-off-by: Ivan Santiago Paunovic <[email protected]> * Add rmw_feature_supported() Signed-off-by: Ivan Santiago Paunovic <[email protected]>
ros2 · Mar 21, 2022 · fd02f99 · fd02f99
1 parent 35a37aa
commit fd02f99
Show file tree

Hide file tree

Showing 6 changed files with 71 additions and 0 deletions.
diff --git a/rmw_connextdds/src/rmw_api_impl_ndds.cpp b/rmw_connextdds/src/rmw_api_impl_ndds.cpp
@@ -937,3 +937,12 @@ rmw_subscription_get_network_flow_endpoints(
     allocator,
     network_flow_endpoint_array);
 }
+
+/******************************************************************************
+ * Feature support functions
+ ******************************************************************************/
+bool
+rmw_feature_supported(rmw_feature_t feature)
+{
+  return rmw_api_connextdds_feature_supported(feature);
+}
diff --git a/rmw_connextdds_common/CMakeLists.txt b/rmw_connextdds_common/CMakeLists.txt
@@ -129,6 +129,7 @@ set(RMW_CONNEXT_COMMON_SOURCE_CPP
     src/common/rmw_network_flow_endpoints.cpp
     src/common/rmw_graph.cpp
     src/common/rmw_event.cpp
+    src/common/rmw_features.cpp
     src/common/rmw_impl.cpp
     src/common/rmw_impl_waitset_std.cpp
     src/common/rmw_info.cpp

diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp
@@ -17,6 +17,8 @@
 
 #include "rmw_connextdds/context.hpp"
 
+#include "rmw/features.h"
+
 /*****************************************************************************
  * Context API
  *****************************************************************************/
@@ -637,4 +639,11 @@ rmw_api_connextdds_subscription_get_network_flow_endpoints(
   rcutils_allocator_t * allocator,
   rmw_network_flow_endpoint_array_t * network_flow_endpoint_array);
 
+/******************************************************************************
+ * Feature support functions
+ ******************************************************************************/
+RMW_CONNEXTDDS_PUBLIC
+bool
+rmw_api_connextdds_feature_supported(rmw_feature_t feature);
+
 #endif  // RMW_CONNEXTDDS__RMW_API_IMPL_HPP_
diff --git a/rmw_connextdds_common/src/common/rmw_features.cpp b/rmw_connextdds_common/src/common/rmw_features.cpp
@@ -0,0 +1,33 @@
+// Copyright 2022 Real-Time Innovations, Inc. (RTI)
+//
+// 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_connextdds/rmw_impl.hpp"
+
+#include "rmw/features.h"
+
+/******************************************************************************
+ * Feature supported functions
+ ******************************************************************************/
+
+bool
+rmw_api_connextdds_feature_supported(rmw_feature_t feature)
+{
+  if (
+    feature == RMW_FEATURE_MESSAGE_INFO_PUBLICATION_SEQUENCE_NUMBER ||
+    feature == RMW_FEATURE_MESSAGE_INFO_RECEPTION_SEQUENCE_NUMBER)
+  {
+    return true;
+  }
+  return false;
+}
diff --git a/rmw_connextdds_common/src/common/rmw_impl.cpp b/rmw_connextdds_common/src/common/rmw_impl.cpp
@@ -1885,6 +1885,16 @@ rmw_connextdds_message_info_from_dds(
   to->source_timestamp = 0;
   to->received_timestamp = 0;
 #endif /* !RTI_WIN32 */
+  // Currently we cannot use `rmw_connextdds_sn_dds_to_ros`, as that was used to convert to
+  // `rmw_request_id_t.sequence_number`, which is an int64_t and not an uint64_t.
+  // When rmw is updated and all sequence numbers are a `uint64_t`,
+  // rmw_connextdds_sn_dds_to_ros() should be updated and used everywhere.
+  to->publication_sequence_number =
+    static_cast<uint64_t>((from->publication_sequence_number).high) << 32 |
+    static_cast<uint64_t>((from->publication_sequence_number).low);
+  to->reception_sequence_number =
+    static_cast<uint64_t>((from->reception_sequence_number).high) << 32 |
+    static_cast<uint64_t>((from->reception_sequence_number).low);
 }
 
 /******************************************************************************

diff --git a/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp b/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp
@@ -936,3 +936,12 @@ rmw_subscription_get_network_flow_endpoints(
     allocator,
     network_flow_endpoint_array);
 }
+
+/******************************************************************************
+ * Feature support functions
+ ******************************************************************************/
+bool
+rmw_feature_supported(rmw_feature_t feature)
+{
+  return rmw_api_connextdds_feature_supported(feature);
+}