Expose raw CDR stream for publish and subscribe

rmw_connext_cpp/CMakeLists.txt

@@ -66,9 +66,62 @@ register_rmw_implementation(
   "cpp:rosidl_typesupport_cpp:rosidl_typesupport_connext_cpp"
 )
 
+# generate code for raw data
+set(_idl_pp "${Connext_DDSGEN}")
+if(NOT "${Connext_DDSGEN_SERVER}" STREQUAL "")
+  # use the code generator in server mode when available
+  # because it speeds up the code generation step significantly
+  set(_idl_pp "${Connext_DDSGEN_SERVER}")
+endif()
+set(generated_directory "${CMAKE_CURRENT_BINARY_DIR}/resources/generated")
+file(MAKE_DIRECTORY ${generated_directory})
+set(generated_files
+  ${generated_directory}/connext_static_serialized_data.cxx
+  ${generated_directory}/connext_static_serialized_data.h
+  ${generated_directory}/connext_static_serialized_dataPlugin.cxx
+  ${generated_directory}/connext_static_serialized_dataPlugin.h
+  ${generated_directory}/connext_static_serialized_dataSupport.cxx
+  ${generated_directory}/connext_static_serialized_dataSupport.h
+)
+add_custom_command(
+  OUTPUT ${generated_files}
+  COMMAND "${_idl_pp}" -language C++ -unboundedSupport "connext_static_serialized_data.idl" -d ${generated_directory}
+  WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/resources"
+  COMMENT "Generating serialized type support for RTI Connext (using '${_idl_pp}')"
+  VERBATIM
+)
+
+# patch the generate code for raw data
+set(patch_files
+  ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_data.cxx.patch
+  ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_data.h.patch
+  ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_dataPlugin.cxx.patch
+  ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_dataPlugin.h.patch
+  ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_dataSupport.cxx.patch
+  ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_dataSupport.h.patch
+)
+set(patched_directory "${CMAKE_CURRENT_BINARY_DIR}/resources/patched")
+file(MAKE_DIRECTORY ${patched_directory})
+set(patched_files
+  ${patched_directory}/connext_static_serialized_data.cxx
+  ${patched_directory}/connext_static_serialized_data.h
+  ${patched_directory}/connext_static_serialized_dataPlugin.cxx
+  ${patched_directory}/connext_static_serialized_dataPlugin.h
+  ${patched_directory}/connext_static_serialized_dataSupport.cxx
+  ${patched_directory}/connext_static_serialized_dataSupport.h
+)
+add_custom_command(
+  OUTPUT ${patched_files}
+  COMMAND ${PYTHON_EXECUTABLE} "${CMAKE_SOURCE_DIR}/bin/apply-patch.py" --input ${generated_files} --patch ${patch_files} --out ${patched_files}
+  DEPENDS ${generated_files}
+  COMMENT "Patching serialized type support for RTI Connext"
+  VERBATIM
+)
+
 add_library(
   rmw_connext_cpp
   SHARED
+  ${patched_files}
   src/get_client.cpp
   src/get_participant.cpp
   src/get_publisher.cpp
@@ -90,6 +143,7 @@ add_library(
   src/rmw_publisher.cpp
   src/rmw_request.cpp
   src/rmw_response.cpp
+  src/rmw_serialize.cpp
   src/rmw_service.cpp
   src/rmw_service_names_and_types.cpp
   src/rmw_service_server_is_available.cpp
@@ -108,6 +162,7 @@ ament_target_dependencies(rmw_connext_cpp
   "rosidl_typesupport_connext_c"
   "rosidl_typesupport_connext_cpp"
   "Connext")
+target_include_directories(rmw_connext_cpp PUBLIC ${patched_directory})
 ament_export_libraries(rmw_connext_cpp)
 
 # Causes the visibility macros to use dllexport rather than dllimport

rmw_connext_cpp/bin/apply-patch.py

@@ -0,0 +1,80 @@
+# Copyright 2016 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.
+
+# Based on `unifieddiff.py` by @noporpoise which is licensed as `Public domain (CC0)`
+# see: https://gist.github.com/noporpoise/16e731849eb1231e86d78f9dfeca3abc
+# TODO(Karsten1987): Rewrite this function for clarity and tests
+
+import argparse
+import re
+
+# regular expression / pattern for patch header
+_hdr_pat = re.compile('^@@ -(\d+),?(\d+)? \+(\d+),?(\d+)? @@.*$')
+
+
+def apply_patch(s, patch):
+    """
+    Apply unified diff patch to string s to recover newer string.
+
+    If revert is True, treat s as the newer string, recover older string.
+    """
+    s = s.splitlines(True)
+    p = patch.splitlines(True)
+    t = ''
+    i = sl = 0
+    (midx, sign) = (1, '+')
+    while i < len(p) and p[i].startswith(('---', '+++')):
+        i += 1  # skip header lines
+    while i < len(p):
+        # find patch header
+        m = _hdr_pat.match(p[i])
+        if not m:
+            raise Exception('Cannot process diff in line ' + str(i))
+
+        i += 1
+        ll = int(m.group(midx)) - 1 + (m.group(midx + 1) == '0')
+        t += ''.join(s[sl:ll])
+        sl = ll
+        while i < len(p) and p[i][0] != '@':
+            if i + 1 < len(p) and p[i + 1][0] == '\\':
+                line = p[i][:-1]
+                i += 2
+            else:
+                line = p[i]
+                i += 1
+            if len(line) > 0:
+                if line[0] == sign or line[0] == ' ':
+                    t += line[1:]
+                sl += (line[0] != sign)
+    t += ''.join(s[sl:])
+    return t
+
+
+parser = argparse.ArgumentParser()
+parser.add_argument('--input', nargs='+')
+parser.add_argument('--patch', nargs='+')
+parser.add_argument('--out', nargs='+')
+args = parser.parse_args()
+for i, p, o in zip(args.input, args.patch, args.out):
+    with open(i, 'r') as h:
+        content_in = h.read()
+    with open(p, 'r') as h:
+        content_patch = h.read()
+    try:
+        content_out = apply_patch(content_in, content_patch)
+    except Exception:
+        print(i, p, o)
+        raise
+    with open(o, 'w') as h:
+        h.write(content_out)

rmw_connext_cpp/resources/connext_static_serialized_data.idl

@@ -0,0 +1,7 @@
+const long KEY_HASH_LENGTH_16 = 16;
+
+struct ConnextStaticSerializedData {
+	@key octet       key_hash[KEY_HASH_LENGTH_16];
+	sequence<octet>  serialized_key;
+	sequence<octet>  serialized_data;
+};

rmw_connext_cpp/resources/patch_files/connext_static_serialized_data.cxx.patch

@@ -0,0 +1,11 @@
+--- a/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_data.cxx
++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_data.cxx
+@@ -11,7 +11,7 @@ or consult the RTI Connext manual.
+
+ #ifndef NDDS_STANDALONE_TYPE
+ #ifndef ndds_cpp_h
+-#include "ndds/ndds_cpp.h"
++#include "rmw_connext_shared_cpp/ndds_include.hpp"
+ #endif
+ #ifndef dds_c_log_impl_h              
+ #include "dds_c/dds_c_log_impl.h"                                

rmw_connext_cpp/resources/patch_files/connext_static_serialized_data.h.patch

@@ -0,0 +1,11 @@
+--- a/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_data.h
++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_data.h
+@@ -14,7 +14,7 @@ or consult the RTI Connext manual.
+
+ #ifndef NDDS_STANDALONE_TYPE
+ #ifndef ndds_cpp_h
+-#include "ndds/ndds_cpp.h"
++#include "rmw_connext_shared_cpp/ndds_include.hpp"
+ #endif
+ #else
+ #include "ndds_standalone_type.h"