add ros_dds_cpp_dynamic_typesupport

rclcpp/include/rclcpp/Node.h

@@ -4,6 +4,7 @@
 #include "Publisher.h"
 
 #include "ros_middleware_interface/functions.h"
+#include "ros_middleware_interface/handles.h"
 
 
 namespace rclcpp
@@ -20,13 +21,13 @@ public:
   template<typename ROSMessage>
   Publisher<ROSMessage>* create_publisher(const char * topic_name)
   {
-    const rosidl_generator_cpp::MessageTypeSupportMembers & members = rosidl_generator_cpp::MessageTypeSupport<ROSMessage>::get_members();
+    const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = rosidl_generator_cpp::MessageTypeSupport<ROSMessage>::get_type_support_handle();
-    void * publisher_handle = ::ros_middleware_interface::create_publisher(node_handle_, members, topic_name);
+    ros_middleware_interface::PublisherHandle publisher_handle = ::ros_middleware_interface::create_publisher(node_handle_, type_support_handle, topic_name);
     return new Publisher<ROSMessage>(publisher_handle);
   }
 
 private:
-  void * node_handle_;
+  ros_middleware_interface::NodeHandle node_handle_;
 };
 
 }

rclcpp/include/rclcpp/Publisher.h

@@ -2,6 +2,7 @@
 #define __rclcpp__Publisher__h__
 
 #include "ros_middleware_interface/functions.h"
+#include "ros_middleware_interface/handles.h"
 
 
 namespace rclcpp
@@ -11,7 +12,7 @@ template<typename ROSMessage>
 class Publisher
 {
 public:
-  Publisher(void * publisher_handle)
+  Publisher(const ros_middleware_interface::PublisherHandle& publisher_handle)
     : publisher_handle_(publisher_handle)
   {}
 
@@ -21,7 +22,7 @@ public:
   }
 
 private:
-  void * publisher_handle_;
+  ros_middleware_interface::PublisherHandle publisher_handle_;
 };
 
 }

ros_dds_connext_dynamic/CMakeLists.txt

@@ -5,17 +5,19 @@ project(ros_dds_connext_dynamic)
 find_package(ament_cmake REQUIRED)
 find_package(ros_middleware_interface REQUIRED)
 find_package(rosidl_generator_cpp REQUIRED)
+find_package(ros_dds_cpp_dynamic_typesupport REQUIRED)
 
 find_package(ndds_cpp REQUIRED)
 set(CONNEXT_INCLUDE_DIRS ${ndds_cpp_INCLUDE_DIRS})
 set(CONNEXT_LIBRARIES ${ndds_cpp_LIBRARIES})
 set(CONNEXT_DEFINITIONS ${ndds_cpp_DEFINITIONS})
 
+ament_export_dependencies(ros_middleware_interface rosidl_generator_cpp ros_dds_cpp_dynamic_typesupport)
 ament_export_libraries(ros_dds_connext_dynamic dl pthread)
 
 ament_package()
 
-include_directories(${ros_middleware_interface_INCLUDE_DIRS} ${CONNEXT_INCLUDE_DIRS})
+include_directories(${ros_middleware_interface_INCLUDE_DIRS} ${rosidl_generator_cpp} ${ros_dds_cpp_dynamic_typesupport} ${CONNEXT_INCLUDE_DIRS})
 add_definitions(${CONNEXT_DEFINITIONS})
 add_library(ros_dds_connext_dynamic SHARED src/functions.cpp)
 target_link_libraries(ros_dds_connext_dynamic ${std_msgs_LIBRARIES} ${CONNEXT_LIBRARIES})

ros_dds_connext_dynamic/package.xml

@@ -11,7 +11,9 @@
   <build_depend>libndds51-dev</build_depend>
   <build_depend>ros_middleware_interface</build_depend>
   <build_depend>rosidl_generator_cpp</build_depend>
+  <build_depend>ros_dds_cpp_dynamic_typesupport</build_depend>
 
   <build_export_depend>libndds51</build_export_depend>
   <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>ros_dds_cpp_dynamic_typesupport</build_export_depend>
 </package>

ros_dds_connext_dynamic/src/functions.cpp

@@ -1,55 +1,87 @@
 #include <iostream>
+#include <stdexcept>
 
 #include "ndds/ndds_cpp.h"
 
 #include "rosidl_generator_cpp/MessageTypeSupport.h"
+#include "ros_middleware_interface/handles.h"
+#include "ros_dds_cpp_dynamic_typesupport/MessageTypeSupport.h"
 
 namespace ros_middleware_interface
 {
 
-void * create_node()
+const char * _rti_connext_identifier = "connext";
+
+ros_middleware_interface::NodeHandle create_node()
 {
     std::cout << "create_node()" << std::endl;
 
     std::cout << "  create_node() get_instance" << std::endl;
     DDSDomainParticipantFactory* dpf_ = DDSDomainParticipantFactory::get_instance();
+    if (!dpf_) {
+        printf("  create_node() could not get participant factory\n");
+        throw std::runtime_error("could not get participant factory");
+    };
+
     DDS_DomainId_t domain = 23;
 
     std::cout << "  create_node() create_participant" << std::endl;
     DDSDomainParticipant* participant = dpf_->create_participant(
         domain, DDS_PARTICIPANT_QOS_DEFAULT, NULL,
         DDS_STATUS_MASK_NONE);
+    if (!participant) {
+        printf("  create_node() could not create participant\n");
+        throw std::runtime_error("could not create participant");
+    };
 
     std::cout << "  create_node() pass opaque node handle" << std::endl;
 
-    return participant;
+    ros_middleware_interface::NodeHandle node_handle = {
+        _rti_connext_identifier,
+        participant
+    };
+    return node_handle;
 }
 
 struct TypeCodeAndDataWriter {
   DDSDynamicDataWriter * dynamic_writer_;
   DDS_TypeCode * type_code_;
-  rosidl_generator_cpp::MessageTypeSupportMembers members_;
+  ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members_;
 };
 
-void * create_publisher(void * node, const rosidl_generator_cpp::MessageTypeSupportMembers & members, const char * topic_name)
+ros_middleware_interface::PublisherHandle create_publisher(const ros_middleware_interface::NodeHandle& node_handle, const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle, const char * topic_name)
 {
     std::cout << "create_publisher()" << std::endl;
 
-    std::cout << "  create_publisher() extract participant from opaque node handle" << std::endl;
+    if (node_handle._implementation_identifier != _rti_connext_identifier)
-    DDSDomainParticipant* participant = (DDSDomainParticipant*)node;
+    {
+        printf("node handle not from this implementation\n");
+        printf("but from: %s\n", node_handle._implementation_identifier);
+        throw std::runtime_error("node handle not from this implementation");
+    }
 
-    std::cout << "  create_publisher() create type code" << std::endl;
+    std::cout << "  create_publisher() extract participant from opaque node handle" << std::endl;
+    DDSDomainParticipant* participant = (DDSDomainParticipant*)node_handle._data;
+
+    ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members = (ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers*)type_support_handle._data;
+    std::string type_name = std::string(members->_package_name) + "/" + members->_message_name;
+
+    std::cout << "  create_publisher() create type code for " << type_name.c_str() << std::endl;
     DDS_TypeCode * type_code;
     {
         DDS_TypeCodeFactory * factory = NULL;
         DDS_ExceptionCode_t ex = DDS_NO_EXCEPTION_CODE;
         DDS_StructMemberSeq struct_members;
         factory = DDS_TypeCodeFactory::get_instance();
+        if (!factory) {
+            printf("  create_publisher() could not get typecode factory\n");
+            throw std::runtime_error("could not get typecode factory");
+        };
 
-        type_code = factory->create_struct_tc("test_type", struct_members, ex);
+        type_code = factory->create_struct_tc(type_name.c_str(), struct_members, ex);
-        for(unsigned long i = 0; i < members._size; ++i)
+        for(unsigned long i = 0; i < members->_size; ++i)
         {
-            const rosidl_generator_cpp::MessageTypeSupportMember * member = members._members + i * sizeof(rosidl_generator_cpp::MessageTypeSupportMember);
+            const ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMember * member = members->_members + i * sizeof(ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMember);
             std::cout << "  create_publisher() create type code - add member " << i << ": " << member->_name << "" << std::endl;
             const DDS_TypeCode * member_type_code;
             if (strcmp(member->_type, "int32") == 0)
@@ -59,7 +91,7 @@ void * create_publisher(void * node, const rosidl_generator_cpp::MessageTypeSupp
             else
             {
                 printf("unknown type %s\n", member->_type);
-                return 0;
+                throw std::runtime_error("unknown type");
             }
             type_code->add_member(member->_name, DDS_TYPECODE_MEMBER_ID_INVALID, member_type_code,
                         DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
@@ -71,43 +103,52 @@ void * create_publisher(void * node, const rosidl_generator_cpp::MessageTypeSupp
     std::cout << "  create_publisher() register type code" << std::endl;
     DDSDynamicDataTypeSupport* ddts = new DDSDynamicDataTypeSupport(
         type_code, DDS_DYNAMIC_DATA_TYPE_PROPERTY_DEFAULT);
-    DDS_ReturnCode_t status = ddts->register_type(participant, "my_type");
+    DDS_ReturnCode_t status = ddts->register_type(participant, type_name.c_str());
     if (status != DDS_RETCODE_OK) {
         printf("register_type() failed. Status = %d\n", status);
-        return 0;
+        throw std::runtime_error("register type failed");
     };
 
+
     DDS_PublisherQos publisher_qos;
     status = participant->get_default_publisher_qos(publisher_qos);
     if (status != DDS_RETCODE_OK) {
-        printf("register_type() failed. Status = %d\n", status);
+        printf("get_default_publisher_qos() failed. Status = %d\n", status);
-        return 0;
+        throw std::runtime_error("get default publisher qos failed");
     };
 
     std::cout << "  create_publisher() create dds publisher" << std::endl;
     DDSPublisher* dds_publisher = participant->create_publisher(
         publisher_qos, NULL, DDS_STATUS_MASK_NONE);
+    if (!dds_publisher) {
+        printf("  create_publisher() could not create publisher\n");
+        throw std::runtime_error("could not create publisher");
+    };
 
 
     DDS_TopicQos default_topic_qos;
     status = participant->get_default_topic_qos(default_topic_qos);
     if (status != DDS_RETCODE_OK) {
-        printf("register_type() failed. Status = %d\n", status);
+        printf("get_default_topic_qos() failed. Status = %d\n", status);
-        return 0;
+        throw std::runtime_error("get default topic qos failed");
     };
 
     std::cout << "  create_publisher() create topic" << std::endl;
     DDSTopic* topic = participant->create_topic(
-        topic_name, "my_type", default_topic_qos, NULL,
+        topic_name, type_name.c_str(), default_topic_qos, NULL,
         DDS_STATUS_MASK_NONE
     );
+    if (!topic) {
+        printf("  create_topic() could not create topic\n");
+        throw std::runtime_error("could not create topic");
+    };
 
 
     DDS_DataWriterQos default_datawriter_qos;
     status = participant->get_default_datawriter_qos(default_datawriter_qos);
     if (status != DDS_RETCODE_OK) {
-        printf("register_type() failed. Status = %d\n", status);
+        printf("get_default_datawriter_qos() failed. Status = %d\n", status);
-        return 0;
+        throw std::runtime_error("get default datawriter qos failed");
     };
 
     std::cout << "  create_publisher() create data writer" << std::endl;
@@ -118,7 +159,7 @@ void * create_publisher(void * node, const rosidl_generator_cpp::MessageTypeSupp
     DDSDynamicDataWriter* dynamic_writer = DDSDynamicDataWriter::narrow(topic_writer);
     if (!dynamic_writer) {
         printf("narrow() failed.\n");
-        return 0;
+        throw std::runtime_error("narrow datawriter failed");
     };
 
 
@@ -128,39 +169,52 @@ void * create_publisher(void * node, const rosidl_generator_cpp::MessageTypeSupp
     type_code_and_data_writer->type_code_ = type_code;
     type_code_and_data_writer->members_ = members;
 
-    return type_code_and_data_writer;
+    ros_middleware_interface::PublisherHandle publisher_handle = {
+        _rti_connext_identifier,
+        type_code_and_data_writer
+    };
+    return publisher_handle;
 }
 
-void publish(void * publisher, const void * ros_message)
+void publish(const ros_middleware_interface::PublisherHandle& publisher_handle, const void * ros_message)
 {
     std::cout << "publish()" << std::endl;
 
+
+    if (publisher_handle._implementation_identifier != _rti_connext_identifier)
+    {
+        printf("publisher handle not from this implementation\n");
+        printf("but from: %s\n", publisher_handle._implementation_identifier);
+        throw std::runtime_error("publisher handle not from this implementation");
+    }
+
     std::cout << "  publish() extract data writer and type code from opaque publisher handle" << std::endl;
-    TypeCodeAndDataWriter * type_code_and_data_writer = (TypeCodeAndDataWriter*)publisher;
+    TypeCodeAndDataWriter * type_code_and_data_writer = (TypeCodeAndDataWriter*)publisher_handle._data;
     DDSDynamicDataWriter * dynamic_writer = type_code_and_data_writer->dynamic_writer_;
     DDS_TypeCode * type_code = type_code_and_data_writer->type_code_;
-    const rosidl_generator_cpp::MessageTypeSupportMembers & members = type_code_and_data_writer->members_;
+    const ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members = type_code_and_data_writer->members_;
 
-    std::cout << "  publish() create and populate dynamic data" << std::endl;
+    std::cout << "  publish() create " << members->_package_name << "/" << members->_message_name << " and populate dynamic data" << std::endl;
     DDS_DynamicData dynamic_data(type_code, DDS_DYNAMIC_DATA_PROPERTY_DEFAULT);
-    for(unsigned long i = 0; i < members._size; ++i)
+    for(unsigned long i = 0; i < members->_size; ++i)
     {
-        const rosidl_generator_cpp::MessageTypeSupportMember * member = members._members + i * sizeof(rosidl_generator_cpp::MessageTypeSupportMember);
+        const ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMember * member = members->_members + i * sizeof(ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMember);
-        std::cout << "  create_publisher() set member " << i << ": " << member->_name << "" << std::endl;
+        std::cout << "  publish() set member " << i << ": " << member->_name << "" << std::endl;
         DDS_TypeCode * member_type_code;
         if (strcmp(member->_type, "int32") == 0)
         {
             long value = *((long*)((char*)ros_message + member->_offset));
-            std::cout << "  create_publisher() set member " << i << ": " << member->_name << " = " << value << std::endl;
+            std::cout << "  publish() set member " << i << ": " << member->_name << " = " << value << std::endl;
             DDS_ReturnCode_t status = dynamic_data.set_long(member->_name, DDS_DYNAMIC_DATA_MEMBER_ID_UNSPECIFIED, value);
             if (status != DDS_RETCODE_OK) {
                 printf("set_long() failed. Status = %d\n", status);
+                throw std::runtime_error("set member failed");
             };
         }
         else
         {
             printf("unknown type %s\n", member->_type);
-            return;
+            throw std::runtime_error("unknown type");
         }
     }
 
@@ -168,6 +222,7 @@ void publish(void * publisher, const void * ros_message)
     DDS_ReturnCode_t status = dynamic_writer->write(dynamic_data, DDS_HANDLE_NIL);
     if (status != DDS_RETCODE_OK) {
         printf("write() failed. Status = %d\n", status);
+        throw std::runtime_error("write failed");
     };
 }
 

ros_dds_cpp_dynamic_typesupport/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(ros_dds_cpp_dynamic_typesupport)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
+ament_export_dependencies(rosidl_cmake)
+ament_export_include_directories(include)
+
+ament_python_install_package(${PROJECT_NAME})
+
+ament_package(
+  CONFIG_EXTRAS "ros_dds_cpp_dynamic_typesupport-extras.cmake"
+)
+
+install(
+  PROGRAMS bin/ros_dds_cpp_dynamic_typesupport
+  DESTINATION lib/ros_dds_cpp_dynamic_typesupport
+)
+install(
+  DIRECTORY cmake resource
+  DESTINATION share/${PROJECT_NAME}
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)

ros_dds_cpp_dynamic_typesupport/bin/ros_dds_cpp_dynamic_typesupport

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+
+import argparse
+import sys
+
+from ros_dds_cpp_dynamic_typesupport import generate_cpp
+
+
+def main(argv=sys.argv[1:]):
+    parser = argparse.ArgumentParser(
+        description='Generate the C++ type support to dynamically handle ROS messages.',
+        formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument(
+        '--pkg-name',
+        required=True,
+        help='The package name to generate interfaces for')
+    parser.add_argument(
+        '--interface-files',
+        nargs='*',
+        help='The ROS interface files')
+    parser.add_argument(
+        '--deps',
+        nargs='*',
+        help="The dependencies (each as '<pkgname>:<abs_interface_file>')")
+    parser.add_argument(
+        '--output-dir',
+        required=True,
+        help='The location of the generated C++ interfaces')
+    parser.add_argument(
+        '--template-dir',
+        required=True,
+        help='The location of the template files')
+    args = parser.parse_args(argv)
+
+    return generate_cpp(
+        args.pkg_name,
+        args.interface_files,
+        args.deps,
+        args.output_dir,
+        args.template_dir,
+    )
+
+
+if __name__ == '__main__':
+    sys.exit(main())

ros_dds_cpp_dynamic_typesupport/cmake/ros_dds_cpp_dynamic_typesupport_generate_interfaces.cmake

@@ -0,0 +1,61 @@
+message(" - ros_dds_cpp_dynamic_typesupport_generate_interfaces.cmake")
+message("   - target: ${rosidl_generate_interfaces_TARGET}")
+message("   - interface files: ${rosidl_generate_interfaces_IDL_FILES}")
+message("   - dependency package names: ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES}")
+
+set(_output_path "${CMAKE_CURRENT_BINARY_DIR}/ros_dds_cpp_dynamic_typesupport/${PROJECT_NAME}")
+set(_generated_files "")
+foreach(_idl_file ${rosidl_generate_interfaces_IDL_FILES})
+  get_filename_component(name "${_idl_file}" NAME_WE)
+  list(APPEND _generated_files
+    "${_output_path}/${name}_TypeSupport.h"
+  )
+endforeach()
+
+set(_dependency_files "")
+set(_dependencies "")
+foreach(_pkg_name ${rosidl_generate_interfaces_DEPENDENCY_PACKAGE_NAMES})
+  foreach(_idl_file ${${_pkg_name}_INTERFACE_FILES})
+    set(_abs_idl_file "${${_pkg_name}_DIR}/../${_idl_file}")
+    list(APPEND _dependency_files "${_abs_idl_file}")
+    list(APPEND _dependencies "${_pkg_name}:${_abs_idl_file}")
+  endforeach()
+endforeach()
+
+message("   - generated files: ${_generated_files}")
+message("   - dependencies: ${_dependencies}")
+
+add_custom_command(
+  OUTPUT ${_generated_files}
+  COMMAND ${PYTHON_EXECUTABLE} ${ros_dds_cpp_dynamic_typesupport_BIN}
+  --pkg-name ${PROJECT_NAME}
+  --interface-files ${rosidl_generate_interfaces_IDL_FILES}
+  --deps ${_dependencies}
+  --output-dir ${_output_path}
+  --template-dir ${ros_dds_cpp_dynamic_typesupport_TEMPLATE_DIR}
+  DEPENDS
+  ${ros_dds_cpp_dynamic_typesupport_BIN}
+  ${ros_dds_cpp_dynamic_typesupport_DIR}/../../../${PYTHON_INSTALL_DIR}/ros_dds_cpp_dynamic_typesupport/__init__.py
+  ${ros_dds_cpp_dynamic_typesupport_TEMPLATE_DIR}/msg_TypeSupport.h.template
+  ${rosidl_generate_interfaces_IDL_FILES}
+  ${_dependency_files}
+  COMMENT "Generating C++ code for interfaces"
+  VERBATIM
+)
+
+add_custom_target(
+  ${rosidl_generate_interfaces_TARGET}_cpp_dynamic_typesupport
+  DEPENDS
+  ${_generated_files}
+)
+add_dependencies(
+  ${rosidl_generate_interfaces_TARGET}
+  ${rosidl_generate_interfaces_TARGET}_cpp_dynamic_typesupport
+)
+
+install(
+  FILES ${_generated_files}
+  DESTINATION "include/${PROJECT_NAME}"
+)
+
+ament_export_include_directories(include)

ros_dds_cpp_dynamic_typesupport/include/ros_dds_cpp_dynamic_typesupport/MessageTypeSupport.h

@@ -0,0 +1,25 @@
+
+#ifndef __ros_dds_cpp_dynamic_typesupport__MessageTypeSupport__h__
+#define __ros_dds_cpp_dynamic_typesupport__MessageTypeSupport__h__
+
+namespace ros_dds_cpp_dynamic_typesupport
+{
+
+const char * _dynamic_identifier = "dynamic";
+
+typedef struct MessageTypeSupportMember {
+  const char * _name;
+  const char * _type;
+  unsigned long _offset;
+} MessageTypeSupportMember;
+
+typedef struct MessageTypeSupportMembers {
+  const char * _package_name;
+  const char * _message_name;
+  unsigned long _size;
+  const MessageTypeSupportMember * _members;
+} MessageTypeSupportMembers;
+
+}  // namespace ros_dds_cpp_dynamic_typesupport
+
+#endif  // __ros_dds_cpp_dynamic_typesupport__MessageTypeSupport__h__

ros_dds_cpp_dynamic_typesupport/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_dds_cpp_dynamic_typesupport</name>
+  <version>0.0.0</version>
+  <description>Generate the message type support for dynamic message construction in C++.</description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>rosidl_cmake</buildtool_depend>
+
+  <buildtool_export_depend>ament_cmake</buildtool_export_depend>
+  <buildtool_export_depend>rosidl_cmake</buildtool_export_depend>
+</package>

ros_dds_cpp_dynamic_typesupport/resource/msg_TypeSupport.h.template

@@ -0,0 +1,75 @@
+@###############################################
+@#
+@# ROS message type support code generation for C++
+@#
+@# EmPy template for generating <msg>_TypeSupport.h files
+@#
+@###############################################
+@# Start of Template
+@#
+@# Context:
+@#  - spec (rosidl_parser.MessageSpecification)
+@#    Parsed specification of the .msg file
+@###############################################
+
+#ifndef __@(spec.base_type.pkg_name)__@(spec.base_type.type)_TypeSupport__h__
+#define __@(spec.base_type.pkg_name)__@(spec.base_type.type)_TypeSupport__h__
+
+#include "@(spec.base_type.pkg_name)/@(spec.base_type.type)_Struct.h"
+#include "rosidl_generator_cpp/MessageTypeSupport.h"
+#include "ros_dds_cpp_dynamic_typesupport/MessageTypeSupport.h"
+
+namespace @(spec.base_type.pkg_name)
+{
+
+namespace type_support
+{
+
+static ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMember member_array[@(len(spec.fields))] = {
+@{
+for index, field in enumerate(spec.fields):
+    print('    {')
+
+    print('        "%s",' % field.name)
+    print('        "%s",' % field.type.type)  # TODO use type id
+    print('        __builtin_offsetof(%s::%s, %s)' % (spec.base_type.pkg_name, spec.base_type.type, field.name))
+
+    if index < len(spec.fields) - 1:
+        print('    },')
+    else:
+        print('    }')
+}@
+};
+
+static ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers members = {
+    "@(spec.base_type.pkg_name)",
+    "@(spec.base_type.type)",
+    @(len(spec.fields)),
+    member_array
+};
+
+static rosidl_generator_cpp::MessageTypeSupportHandle handle = {
+    ros_dds_cpp_dynamic_typesupport::_dynamic_identifier,
+    &members
+};
+
+}  // namespace type_support
+
+}  // namespace @(spec.base_type.pkg_name)
+
+
+namespace rosidl_generator_cpp
+{
+
+template<>
+struct MessageTypeSupport<@(spec.base_type.pkg_name)::@(spec.base_type.type)>
+{
+    static const rosidl_generator_cpp::MessageTypeSupportHandle& get_type_support_handle()
+    {
+        return @(spec.base_type.pkg_name)::type_support::handle;
+    }
+};
+
+}  // namespace rosidl_generator_cpp
+
+#endif  // __@(spec.base_type.pkg_name)__@(spec.base_type.type)_TypeSupport__h__

ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport-extras.cmake

@@ -0,0 +1,12 @@
+# copied from ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport-extras.cmake
+
+find_package(ament_cmake_core REQUIRED)
+# TODO
+# instead of being an extension for "rosidl_generate_interfaces"
+# this should be an extension of "rosidl_generator_cpp"
+# which can then ensure that there is only one
+ament_register_extension("rosidl_generate_interfaces" "ros_dds_cpp_dynamic_typesupport"
+  "ros_dds_cpp_dynamic_typesupport_generate_interfaces.cmake")
+
+set(ros_dds_cpp_dynamic_typesupport_BIN "${ros_dds_cpp_dynamic_typesupport_DIR}/../../../lib/ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport")
+set(ros_dds_cpp_dynamic_typesupport_TEMPLATE_DIR "${ros_dds_cpp_dynamic_typesupport_DIR}/../resource")

ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport/__init__.py

@@ -0,0 +1,95 @@
+import em
+import os
+
+from rosidl_parser import parse_message_file
+
+
+def generate_cpp(pkg_name, interface_files, deps, output_dir, template_dir):
+    mapping = {
+        os.path.join(template_dir, 'msg_TypeSupport.h.template'): '%s_TypeSupport.h',
+    }
+    for template_file in mapping.keys():
+        assert(os.path.exists(template_file))
+
+    try:
+        os.makedirs(output_dir)
+    except FileExistsError:
+        pass
+
+    for idl_file in interface_files:
+        spec = parse_message_file(pkg_name, idl_file)
+        for template_file, generated_filename in mapping.items():
+            generated_file = os.path.join(output_dir, generated_filename % spec.base_type.type)
+            print('Generating: %s' % generated_file)
+
+            try:
+                # TODO only touch generated file if its content actually changes
+                ofile = open(generated_file, 'w')
+                # TODO reuse interpreter
+                interpreter = em.Interpreter(
+                    output=ofile,
+                    options={
+                        em.RAW_OPT: True,
+                        em.BUFFERED_OPT: True,
+                    },
+                    globals={'spec': spec},
+                )
+                interpreter.file(open(template_file))
+                interpreter.shutdown()
+            except Exception:
+                os.remove(generated_file)
+                raise
+
+    return 0
+
+
+MSG_TYPE_TO_CPP = {
+    'bool': 'uint8_t',
+    'float32': 'float',
+    'float64': 'double',
+    'uint8': 'uint8_t',
+    'int8': 'int8_t',
+    'uint16': 'uint16_t',
+    'int16': 'int16_t',
+    'uint32': 'uint32_t',
+    'int32': 'int32_t',
+    'uint64': 'uint64_t',
+    'int64': 'int64_t',
+    'string': 'std::basic_string<char, std::char_traits<char>, ' +
+              'typename ContainerAllocator::template rebind<char>::other>',
+    'byte': 'int8_t',
+    'char': 'uint8_t',
+}
+
+
+def msg_type_to_cpp(type_):
+    """
+    Convert a message type into the C++ declaration.
+
+    Example input: uint32, std_msgs/String
+    Example output: uint32_t, std_msgs::String_<ContainerAllocator>
+
+    @param type: The message type
+    @type type: rosidl_parser.Type
+    """
+    cpp_type = None
+    if type_.is_primitive_type():
+        cpp_type = MSG_TYPE_TO_CPP[type_.type]
+    else:
+        cpp_type = '::%s::%s_<ContainerAllocator> ' % \
+            (type_.pkg_name, type_.type)
+
+    if type_.is_array:
+        if type_.array_size is None:
+            return 'std::vector<%s, typename ContainerAllocator::template ' + \
+                'rebind<%s>::other > ' % (cpp_type, cpp_type)
+        else:
+            return 'boost::array<%s, %s> ' % (cpp_type, type_.array_size)
+    else:
+        return cpp_type
+
+
+def escape_string(s):
+    s = s.replace('\\', '\\\\')
+    s = s.replace('"', '\\"')
+    return s

ros_middleware_interface/include/ros_middleware_interface/functions.h

@@ -3,14 +3,16 @@
 
 #include "rosidl_generator_cpp/MessageTypeSupport.h"
 
+#include "handles.h"
+
 namespace ros_middleware_interface
 {
 
-void * create_node();
+NodeHandle create_node();
 
-void * create_publisher(void * node, const rosidl_generator_cpp::MessageTypeSupportMembers & members, const char * topic_name);
+PublisherHandle create_publisher(const NodeHandle& node_handle, const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle, const char * topic_name);
 
-void publish(void * publisher, const void * ros_message);
+void publish(const PublisherHandle& publisher_handle, const void * ros_message);
 
 }
 

ros_middleware_interface/include/ros_middleware_interface/handles.h

@@ -0,0 +1,19 @@
+#ifndef __ros_middleware_interface__handles__h__
+#define __ros_middleware_interface__handles__h__
+
+namespace ros_middleware_interface
+{
+
+typedef struct NodeHandle {
+  const char * _implementation_identifier;
+  void * _data;
+} NodeHandle;
+
+typedef struct PublisherHandle {
+  const char * _implementation_identifier;
+  void * _data;
+} PublisherHandle;
+
+}
+
+#endif  // __ros_middleware_interface__handles__h__