Add sequence numbers to message info structure (#587)

* Fill publication_sequence_number in all rmw_take_*_with_info() functions Signed-off-by: Ivan Santiago Paunovic <[email protected]> * Implement rmw_feature_supported() Signed-off-by: Ivan Santiago Paunovic <[email protected]> * Set message_info->reception_sequence_number to RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED Signed-off-by: Ivan Santiago Paunovic <[email protected]>
ros2 · Mar 21, 2022 · a4c07a1 · a4c07a1
1 parent eb189fb
commit a4c07a1
Show file tree

Hide file tree

Showing 8 changed files with 92 additions and 0 deletions.
diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt
@@ -62,6 +62,7 @@ add_library(rmw_fastrtps_cpp
   src/rmw_compare_gids_equal.cpp
   src/rmw_count.cpp
   src/rmw_event.cpp
+  src/rmw_features.cpp
   src/rmw_get_gid_for_publisher.cpp
   src/rmw_get_implementation_identifier.cpp
   src/rmw_get_serialization_format.cpp

diff --git a/rmw_fastrtps_cpp/src/rmw_features.cpp b/rmw_fastrtps_cpp/src/rmw_features.cpp
@@ -0,0 +1,26 @@
+// Copyright 2022 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/features.h"
+
+#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
+
+extern "C"
+{
+bool
+rmw_feature_supported(rmw_feature_t feature)
+{
+  return rmw_fastrtps_shared_cpp::__rmw_feature_supported(feature);
+}
+}  // extern "C"
diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt
@@ -64,6 +64,7 @@ add_library(rmw_fastrtps_dynamic_cpp
   src/rmw_compare_gids_equal.cpp
   src/rmw_count.cpp
   src/rmw_event.cpp
+  src/rmw_features.cpp
   src/rmw_get_gid_for_publisher.cpp
   src/rmw_get_implementation_identifier.cpp
   src/rmw_get_serialization_format.cpp

diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_features.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_features.cpp
@@ -0,0 +1,27 @@
+// Copyright 2022 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/features.h"
+
+#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
+
+extern "C"
+{
+
+bool
+rmw_feature_supported(rmw_feature_t feature)
+{
+  return rmw_fastrtps_shared_cpp::__rmw_feature_supported(feature);
+}
+}  // extern "C"
diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt
@@ -61,6 +61,7 @@ add_library(rmw_fastrtps_shared_cpp
   src/rmw_compare_gids_equal.cpp
   src/rmw_count.cpp
   src/rmw_event.cpp
+  src/rmw_features.cpp
   src/rmw_get_endpoint_network_flow.cpp
   src/rmw_get_gid_for_publisher.cpp
   src/rmw_get_topic_endpoint_info.cpp

diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp
@@ -19,6 +19,7 @@
 
 #include "rmw/error_handling.h"
 #include "rmw/event.h"
+#include "rmw/features.h"
 #include "rmw/rmw.h"
 #include "rmw/topic_endpoint_info_array.h"
 #include "rmw/types.h"
@@ -514,6 +515,10 @@ __rmw_event_set_callback(
   rmw_event_callback_t callback,
   const void * user_data);
 
+RMW_FASTRTPS_SHARED_CPP_PUBLIC
+bool
+__rmw_feature_supported(rmw_feature_t feature);
+
 }  // namespace rmw_fastrtps_shared_cpp
 
 #endif  // RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_features.cpp b/rmw_fastrtps_shared_cpp/src/rmw_features.cpp
@@ -0,0 +1,26 @@
+// Copyright 2022 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/features.h"
+
+#include "rmw_fastrtps_shared_cpp/rmw_common.hpp"
+
+bool
+rmw_fastrtps_shared_cpp::__rmw_feature_supported(rmw_feature_t feature)
+{
+  if (feature == RMW_FEATURE_MESSAGE_INFO_PUBLICATION_SEQUENCE_NUMBER) {
+    return true;
+  }
+  return false;
+}
diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp
@@ -47,6 +47,11 @@ _assign_message_info(
 {
   message_info->source_timestamp = sinfo->source_timestamp.to_ns();
   message_info->received_timestamp = sinfo->reception_timestamp.to_ns();
+  auto fastdds_sn = sinfo->sample_identity.sequence_number();
+  message_info->publication_sequence_number =
+    (static_cast<uint64_t>(fastdds_sn.high) << 32) |
+    static_cast<uint64_t>(fastdds_sn.low);
+  message_info->reception_sequence_number = RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED;
   rmw_gid_t * sender_gid = &message_info->publisher_gid;
   sender_gid->implementation_identifier = identifier;
   memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE);