add ros_dds_connext_static

rclcpp/CMakeLists.txt

@@ -5,7 +5,31 @@ project(rclcpp)
 find_package(ament_cmake REQUIRED)
 find_package(ros_middleware_interface REQUIRED)
 
-ament_export_dependencies(ros_dds_connext_dynamic ros_middleware_interface)
+ament_export_dependencies(ros_middleware_interface)
+
+set(_dds_vendor "$ENV{ROS_DDS_IMPLEMENTATION}")
+set(_dds_type "$ENV{ROS_DDS_TYPE}")
+
+if(NOT "${_dds_type}" STREQUAL "dynamic" AND NOT "${_dds_type}" STREQUAL "static")
+  message(FATAL_ERROR "Set the environment variable 'ROS_DDS_TYPE' to either 'dynamic' or 'static'.")
+endif()
+
+if("${_dds_vendor}" STREQUAL "connext")
+  if("${_dds_type}" STREQUAL "dynamic")
+    ament_export_dependencies(ros_dds_connext_dynamic)
+  elseif("${_dds_type}" STREQUAL "static")
+    ament_export_dependencies(rosidl_generator_dds_connext_cpp ros_dds_connext_static)
+  endif()
+elseif("${_dds_vendor}" STREQUAL "opensplice")
+  if("${_dds_type}" STREQUAL "dynamic")
+    message(FATAL_ERROR "The DDS implementation '${_dds_vendor}' does not support the type '${_dds_type}'.")
+  elseif("${_dds_type}" STREQUAL "static")
+    ament_export_dependencies(rosidl_generator_dds_opensplice_cpp)
+  endif()
+else()
+  message(FATAL_ERROR "Set the environment variable 'ROS_DDS_IMPLEMENTATION' to either 'connext' or 'opensplice'.")
+endif()
+
 ament_export_include_directories(include)
 
 ament_package()

ros_dds_connext_dynamic/CMakeLists.txt

@@ -5,19 +5,52 @@ 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_INCLUDE_DIRS ${ndds_cpp_INCLUDE_DIRS})
-set(CONNEXT_LIBRARIES ${ndds_cpp_LIBRARIES})
+set(CONNEXT_INCLUDE_DIRS
+  "/home/dthomas/RTI/ndds.5.1.0/include/ndds"
+  "/home/dthomas/RTI/ndds.5.1.0/include"
+)
+#set(CONNEXT_LIBRARIES ${ndds_cpp_LIBRARIES})
+set(CONNEXT_LIBRARIES
+  "/home/dthomas/RTI/ndds.5.1.0/lib/x64Linux2.6gcc4.4.5/libnddscppz.a"
+  "/home/dthomas/RTI/ndds.5.1.0/lib/x64Linux2.6gcc4.4.5/libnddscz.a"
+  "/home/dthomas/RTI/ndds.5.1.0/lib/x64Linux2.6gcc4.4.5/libnddscorez.a"
+)
 set(CONNEXT_DEFINITIONS ${ndds_cpp_DEFINITIONS})
 
-ament_export_dependencies(ros_middleware_interface rosidl_generator_cpp ros_dds_cpp_dynamic_typesupport)
+
+set(_dds_vendor "$ENV{ROS_DDS_IMPLEMENTATION}")
+set(_dds_type "$ENV{ROS_DDS_TYPE}")
+if("${_dds_type}" STREQUAL "dynamic")
+  find_package(ros_dds_cpp_dynamic_typesupport REQUIRED)
+  ament_export_dependencies(ros_dds_cpp_dynamic_typesupport)
+  include_directories(${ros_dds_cpp_dynamic_typesupport_INCLUDE_DIRS})
+elseif("${_dds_type}" STREQUAL "static")
+  if("${_dds_vendor}" STREQUAL "connext")
+    find_package(ros_dds_connext_static REQUIRED)
+    ament_export_definitions(${CONNEXT_DEFINITIONS})
+    ament_export_dependencies(ros_dds_connext_static)
+    include_directories(${ros_dds_connext_static_INCLUDE_DIRS})
+  elseif("${_dds_vendor}" STREQUAL "opensplice")
+    message(FATAL_ERROR "The DDS implementation '${_dds_vendor}' does not yet support the type '${_dds_type}'.")
+  else()
+    message(FATAL_ERROR "Set the environment variable 'ROS_DDS_IMPLEMENTATION' to either 'connext' or 'opensplice'.")
+  endif()
+else()
+  message(FATAL_ERROR "Set the environment variable 'ROS_DDS_TYPE' to either 'dynamic' or 'static'.")
+endif()
+
+
+ament_export_dependencies(ros_middleware_interface rosidl_generator_cpp)
+
 ament_export_libraries(ros_dds_connext_dynamic dl pthread)
 
 ament_package()
 
-include_directories(${ros_middleware_interface_INCLUDE_DIRS} ${rosidl_generator_cpp} ${ros_dds_cpp_dynamic_typesupport} ${CONNEXT_INCLUDE_DIRS})
+include_directories(${ros_middleware_interface_INCLUDE_DIRS} ${rosidl_generator_cpp_INCLUDE_DIRS} ${CONNEXT_INCLUDE_DIRS})
+link_directories("/home/dthomas/RTI/ndds.5.1.0/lib/x64Linux2.6gcc4.4.5")
 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/src/functions.cpp

@@ -10,7 +10,7 @@
 namespace ros_middleware_interface
 {
 
-const char * _rti_connext_identifier = "connext";
+const char * _rti_connext_identifier = "connext_dynamic";
 
 ros_middleware_interface::NodeHandle create_node()
 {
@@ -43,7 +43,7 @@ ros_middleware_interface::NodeHandle create_node()
     return node_handle;
 }
 
-struct TypeCodeAndDataWriter {
+struct CustomPublisherInfo {
   DDSDynamicDataWriter * dynamic_writer_;
   DDS_TypeCode * type_code_;
   ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members_;
@@ -164,14 +164,14 @@ ros_middleware_interface::PublisherHandle create_publisher(const ros_middleware_
 
 
     std::cout << "  create_publisher() build opaque publisher handle" << std::endl;
-    TypeCodeAndDataWriter* type_code_and_data_writer = new TypeCodeAndDataWriter();
+    CustomPublisherInfo* custom_publisher_info = new CustomPublisherInfo();
-    type_code_and_data_writer->dynamic_writer_ = dynamic_writer;
+    custom_publisher_info->dynamic_writer_ = dynamic_writer;
-    type_code_and_data_writer->type_code_ = type_code;
+    custom_publisher_info->type_code_ = type_code;
-    type_code_and_data_writer->members_ = members;
+    custom_publisher_info->members_ = members;
 
     ros_middleware_interface::PublisherHandle publisher_handle = {
         _rti_connext_identifier,
-        type_code_and_data_writer
+        custom_publisher_info
     };
     return publisher_handle;
 }
@@ -189,10 +189,10 @@ void publish(const ros_middleware_interface::PublisherHandle& publisher_handle,
     }
 
     std::cout << "  publish() extract data writer and type code from opaque publisher handle" << std::endl;
-    TypeCodeAndDataWriter * type_code_and_data_writer = (TypeCodeAndDataWriter*)publisher_handle._data;
+    CustomPublisherInfo * custom_publisher_info = (CustomPublisherInfo*)publisher_handle._data;
-    DDSDynamicDataWriter * dynamic_writer = type_code_and_data_writer->dynamic_writer_;
+    DDSDynamicDataWriter * dynamic_writer = custom_publisher_info->dynamic_writer_;
-    DDS_TypeCode * type_code = type_code_and_data_writer->type_code_;
+    DDS_TypeCode * type_code = custom_publisher_info->type_code_;
-    const ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members = type_code_and_data_writer->members_;
+    const ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members = custom_publisher_info->members_;
 
     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);

ros_dds_connext_static/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(ros_dds_connext_static)
+
+find_package(ament_cmake REQUIRED)
+find_package(ros_middleware_interface REQUIRED)
+find_package(rosidl_generator_cpp REQUIRED)
+find_package(rosidl_generator_dds_connext_cpp REQUIRED)
+
+find_package(ndds_cpp REQUIRED)
+#set(CONNEXT_INCLUDE_DIRS ${ndds_cpp_INCLUDE_DIRS})
+set(CONNEXT_INCLUDE_DIRS
+  "/home/dthomas/RTI/ndds.5.1.0/include/ndds"
+  "/home/dthomas/RTI/ndds.5.1.0/include"
+)
+#set(CONNEXT_LIBRARIES ${ndds_cpp_LIBRARIES})
+set(CONNEXT_LIBRARIES
+  "/home/dthomas/RTI/ndds.5.1.0/lib/x64Linux2.6gcc4.4.5/libnddscppz.a"
+  "/home/dthomas/RTI/ndds.5.1.0/lib/x64Linux2.6gcc4.4.5/libnddscz.a"
+  "/home/dthomas/RTI/ndds.5.1.0/lib/x64Linux2.6gcc4.4.5/libnddscorez.a"
+)
+set(CONNEXT_DEFINITIONS ${ndds_cpp_DEFINITIONS})
+
+ament_export_definitions(${CONNEXT_DEFINITIONS})
+ament_export_dependencies(ros_middleware_interface rosidl_generator_cpp rosidl_generator_dds_connext_cpp)
+ament_export_include_directories(include ${CONNEXT_INCLUDE_DIRS})
+ament_export_libraries(ros_dds_connext_static dl pthread)
+
+ament_python_install_package(${PROJECT_NAME})
+
+ament_package(
+  CONFIG_EXTRAS "ros_dds_connext_static-extras.cmake"
+)
+
+include_directories(include ${ros_middleware_interface_INCLUDE_DIRS} ${rosidl_generator_cpp_INCLUDE_DIRS} ${rosidl_generator_dds_connext_cpp_INCLUDE_DIRS} ${CONNEXT_INCLUDE_DIRS})
+link_directories("/home/dthomas/RTI/ndds.5.1.0/lib/x64Linux2.6gcc4.4.5")
+add_definitions(${CONNEXT_DEFINITIONS})
+add_library(ros_dds_connext_static SHARED src/functions.cpp)
+target_link_libraries(ros_dds_connext_static ${std_msgs_LIBRARIES} ${rosidl_generator_dds_connext_cpp_LIBRARIES} ${CONNEXT_LIBRARIES})
+
+install(
+  PROGRAMS bin/ros_dds_connext_static
+  DESTINATION lib/ros_dds_connext_static
+)
+install(
+  DIRECTORY cmake resource
+  DESTINATION share/${PROJECT_NAME}
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+install(
+  TARGETS ros_dds_connext_static
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+)

ros_dds_connext_static/bin/ros_dds_connext_static

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+
+import argparse
+import sys
+
+from ros_dds_connext_static import generate_cpp
+
+
+def main(argv=sys.argv[1:]):
+    parser = argparse.ArgumentParser(
+        description='Generate the C++ type support to statically 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_connext_static/cmake/ros_dds_connext_static_generate_interfaces.cmake

@@ -0,0 +1,61 @@
+message(" - ros_dds_connext_static_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_connext_static/${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_connext_static_BIN}
+  --pkg-name ${PROJECT_NAME}
+  --interface-files ${rosidl_generate_interfaces_IDL_FILES}
+  --deps ${_dependencies}
+  --output-dir ${_output_path}
+  --template-dir ${ros_dds_connext_static_TEMPLATE_DIR}
+  DEPENDS
+  ${ros_dds_connext_static_BIN}
+  ${ros_dds_connext_static_DIR}/../../../${PYTHON_INSTALL_DIR}/ros_dds_connext_static/__init__.py
+  ${ros_dds_connext_static_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_connext_static
+  DEPENDS
+  ${_generated_files}
+)
+add_dependencies(
+  ${rosidl_generate_interfaces_TARGET}
+  ${rosidl_generate_interfaces_TARGET}_cpp_connext_static
+)
+
+install(
+  FILES ${_generated_files}
+  DESTINATION "include/${PROJECT_NAME}"
+)
+
+ament_export_include_directories(include)

ros_dds_connext_static/include/ros_dds_connext_static/MessageTypeSupport.h

@@ -0,0 +1,22 @@
+
+#ifndef __ros_dds_connext_static__MessageTypeSupport__h__
+#define __ros_dds_connext_static__MessageTypeSupport__h__
+
+class DDSDomainParticipant;
+class DDSDataWriter;
+
+namespace ros_dds_connext_static
+{
+
+const char * _connext_static_identifier = "connext_static";
+
+typedef struct MessageTypeSupportCallbacks {
+  const char * _package_name;
+  const char * _message_name;
+  void (*_register_type)(DDSDomainParticipant * participant, const char * type_name);
+  void (*_publish)(DDSDataWriter * topic_writer, const void * ros_message);
+} MessageTypeSupportCallbacks;
+
+}  // namespace ros_dds_connext_static
+
+#endif  // __ros_dds_connext_static__MessageTypeSupport__h__

ros_dds_connext_static/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_dds_connext_static</name>
+  <version>0.0.0</version>
+  <description>Implement the ROS middleware interface using RTI Connext static code generation 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>
+
+  <build_depend>libndds51-dev</build_depend>
+  <build_depend>ros_middleware_interface</build_depend>
+  <build_depend>rosidl_generator_cpp</build_depend>
+  <build_depend>rosidl_generator_dds_connext_cpp</build_depend>
+  <build_depend>rosidl_generator_dds_idl</build_depend>
+
+  <build_export_depend>libndds51</build_export_depend>
+  <build_export_depend>rosidl_generator_cpp</build_export_depend>
+  <build_export_depend>rosidl_generator_dds_connext_cpp</build_export_depend>
+</package>

ros_dds_connext_static/resource/msg_TypeSupport.h.template

@@ -0,0 +1,123 @@
+@###############################################
+@#
+@# 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 <iostream>
+
+#include "@(spec.base_type.pkg_name)/@(spec.base_type.type)_Struct.h"
+#include "@(spec.base_type.pkg_name)/dds_idl/@(spec.base_type.type)_Support.h"
+#include "rosidl_generator_cpp/MessageTypeSupport.h"
+#include "ros_dds_connext_static/MessageTypeSupport.h"
+
+namespace @(spec.base_type.pkg_name)
+{
+
+namespace type_support
+{
+
+void register_type__@(spec.base_type.type)(DDSDomainParticipant * participant, const char * type_name)
+{
+    std::cout << "  @(spec.base_type.pkg_name)::type_support::register_type__@(spec.base_type.type)()" << std::endl;
+    @(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_TypeSupport::register_type(participant, type_name);
+}
+
+void convert_ros_message_to_dds__@(spec.base_type.type)(const void * untyped_ros_message, @(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_& dds_message)
+{
+    std::cout << "  @(spec.base_type.pkg_name)::type_support::convert_ros_message_to_dds__@(spec.base_type.type)()" << std::endl;
+    const @(spec.base_type.pkg_name)::@(spec.base_type.type) & ros_message = *(const @(spec.base_type.pkg_name)::@(spec.base_type.type) *)untyped_ros_message;
+@[for field in spec.fields]@
+    // field.name @(field.name)
+    // field.type @(field.type)
+    // field.type.is_array @(field.type.is_array)
+@[if field.type.array_size]@
+    // field.type.array_size @(field.type.array_size)
+@[else]@
+    // field.type.array_size dynamic
+@[end if]@
+@[if field.type.is_array]@
+    {
+@[if field.type.array_size]@
+        size_t size = @(field.type.array_size);
+@[else]@
+        size_t size = ros_message.@(field.name).size();
+        dds_message.@(field.name)_.length(size);
+@[end if]@
+        for (size_t i = 0; i < size; i++) {
+@[if field.type.is_primitive_type()]@
+            dds_message.@(field.name)_[i] = ros_message.@(field.name)[i];
+@[else]@
+            ::dds_impl::DDSTypeResolver<@(field.type.pkg_name)::@(field.type.type)>::convert_ros_message_to_dds(ros_message.@(field.name)[i], dds_message.@(field.name)_[i]);
+@[end if]@
+        }
+    }
+@[elif field.type.is_primitive_type()]@
+      dds_message.@(field.name)_ = ros_message.@(field.name);
+@[else]@
+      ::dds_impl::DDSTypeResolver<@(field.type.pkg_name)::@(field.type.type)>::convert_ros_message_to_dds(ros_message.@(field.name), dds_message.@(field.name)_);
+@[end if]@
+
+@[end for]@
+}
+
+void publish__@(spec.base_type.type)(DDSDataWriter * topic_writer, const void * ros_message)
+{
+    std::cout << "  @(spec.base_type.pkg_name)::type_support::publish__@(spec.base_type.type)()" << std::endl;
+
+    @(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_ dds_message;
+    convert_ros_message_to_dds__@(spec.base_type.type)(ros_message, dds_message);
+
+    @(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_DataWriter * data_writer = @(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_DataWriter::narrow(topic_writer);
+    std::cout << "  @(spec.base_type.pkg_name)::type_support::publish__@(spec.base_type.type)() write dds message" << std::endl;
+    DDS_ReturnCode_t status = data_writer->write(dds_message, DDS_HANDLE_NIL);
+    if (status != DDS_RETCODE_OK) {
+        printf("write() failed. Status = %d\n", status);
+        throw std::runtime_error("write failed");
+    };
+}
+
+
+static ros_dds_connext_static::MessageTypeSupportCallbacks callbacks = {
+    "@(spec.base_type.pkg_name)",
+    "@(spec.base_type.type)",
+    &register_type__@(spec.base_type.type),
+    &publish__@(spec.base_type.type)
+};
+
+static rosidl_generator_cpp::MessageTypeSupportHandle handle = {
+    ros_dds_connext_static::_connext_static_identifier,
+    &callbacks
+};
+
+}  // 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_connext_static/ros_dds_connext_static-extras.cmake

@@ -0,0 +1,12 @@
+# copied from ros_dds_connext_static/ros_dds_connext_static-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_connext_static"
+  "ros_dds_connext_static_generate_interfaces.cmake")
+
+set(ros_dds_connext_static_BIN "${ros_dds_connext_static_DIR}/../../../lib/ros_dds_connext_static/ros_dds_connext_static")
+set(ros_dds_connext_static_TEMPLATE_DIR "${ros_dds_connext_static_DIR}/../resource")

ros_dds_connext_static/ros_dds_connext_static/__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_dds_connext_static/src/functions.cpp

@@ -0,0 +1,153 @@
+#include <iostream>
+#include <stdexcept>
+
+#include "ndds/ndds_cpp.h"
+
+#include "rosidl_generator_cpp/MessageTypeSupport.h"
+#include "ros_middleware_interface/handles.h"
+#include "ros_dds_connext_static/MessageTypeSupport.h"
+
+namespace ros_middleware_interface
+{
+
+const char * _rti_connext_identifier = "connext_static";
+
+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;
+
+    ros_middleware_interface::NodeHandle node_handle = {
+        _rti_connext_identifier,
+        participant
+    };
+    return node_handle;
+}
+
+struct CustomPublisherInfo {
+  DDSDataWriter * topic_writer_;
+  ros_dds_connext_static::MessageTypeSupportCallbacks * callbacks_;
+};
+
+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;
+
+    if (node_handle._implementation_identifier != _rti_connext_identifier)
+    {
+        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() extract participant from opaque node handle" << std::endl;
+    DDSDomainParticipant* participant = (DDSDomainParticipant*)node_handle._data;
+
+    ros_dds_connext_static::MessageTypeSupportCallbacks * callbacks = (ros_dds_connext_static::MessageTypeSupportCallbacks*)type_support_handle._data;
+    std::string type_name = std::string(callbacks->_package_name) + "/" + callbacks->_message_name;
+
+
+    std::cout << "  create_publisher() invoke register callback" << std::endl;
+    callbacks->_register_type(participant, type_name.c_str());
+
+
+    DDS_PublisherQos publisher_qos;
+    DDS_ReturnCode_t status = participant->get_default_publisher_qos(publisher_qos);
+    if (status != DDS_RETCODE_OK) {
+        printf("get_default_publisher_qos() failed. Status = %d\n", status);
+        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("get_default_topic_qos() failed. Status = %d\n", status);
+        throw std::runtime_error("get default topic qos failed");
+    };
+
+    std::cout << "  create_publisher() create topic" << std::endl;
+    DDSTopic* topic = participant->create_topic(
+        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("get_default_datawriter_qos() failed. Status = %d\n", status);
+        throw std::runtime_error("get default datawriter qos failed");
+    };
+
+    std::cout << "  create_publisher() create data writer" << std::endl;
+    DDSDataWriter* topic_writer = dds_publisher->create_datawriter(
+        topic, default_datawriter_qos,
+        NULL, DDS_STATUS_MASK_NONE);
+
+
+    std::cout << "  create_publisher() build opaque publisher handle" << std::endl;
+    CustomPublisherInfo* custom_publisher_info = new CustomPublisherInfo();
+    custom_publisher_info->topic_writer_ = topic_writer;
+    custom_publisher_info->callbacks_ = callbacks;
+
+    ros_middleware_interface::PublisherHandle publisher_handle = {
+        _rti_connext_identifier,
+        custom_publisher_info
+    };
+    return publisher_handle;
+}
+
+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;
+    CustomPublisherInfo * custom_publisher_info = (CustomPublisherInfo*)publisher_handle._data;
+    DDSDataWriter * topic_writer = custom_publisher_info->topic_writer_;
+    const ros_dds_connext_static::MessageTypeSupportCallbacks * callbacks = custom_publisher_info->callbacks_;
+
+
+    std::cout << "  publish() invoke publish callback" << std::endl;
+    callbacks->_publish(topic_writer, ros_message);
+}
+
+}

userland/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(std_msgs REQUIRED)
 ament_package()
 
 include_directories(${rclcpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})
+add_definitions(${rclcpp_DEFINITIONS})
 add_executable(userland src/main.cpp)
 target_link_libraries(userland ${rclcpp_LIBRARIES} ${std_msgs_LIBRARIES})