From 666a4b51c9cf0605d7335c2baf64970464e5f4a5 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Fri, 1 Aug 2014 16:59:55 -0700 Subject: [PATCH] add ros_dds_connext_static --- rclcpp/CMakeLists.txt | 26 ++- ros_dds_connext_dynamic/CMakeLists.txt | 43 ++++- ros_dds_connext_dynamic/src/functions.cpp | 22 +-- ros_dds_connext_static/CMakeLists.txt | 57 +++++++ .../bin/ros_dds_connext_static | 45 ++++++ ...s_connext_static_generate_interfaces.cmake | 61 +++++++ .../MessageTypeSupport.h | 22 +++ ros_dds_connext_static/package.xml | 25 +++ .../resource/msg_TypeSupport.h.template | 123 ++++++++++++++ .../ros_dds_connext_static-extras.cmake | 12 ++ .../ros_dds_connext_static/__init__.py | 95 +++++++++++ ros_dds_connext_static/src/functions.cpp | 153 ++++++++++++++++++ userland/CMakeLists.txt | 1 + 13 files changed, 668 insertions(+), 17 deletions(-) create mode 100644 ros_dds_connext_static/CMakeLists.txt create mode 100755 ros_dds_connext_static/bin/ros_dds_connext_static create mode 100644 ros_dds_connext_static/cmake/ros_dds_connext_static_generate_interfaces.cmake create mode 100644 ros_dds_connext_static/include/ros_dds_connext_static/MessageTypeSupport.h create mode 100644 ros_dds_connext_static/package.xml create mode 100644 ros_dds_connext_static/resource/msg_TypeSupport.h.template create mode 100644 ros_dds_connext_static/ros_dds_connext_static-extras.cmake create mode 100644 ros_dds_connext_static/ros_dds_connext_static/__init__.py create mode 100644 ros_dds_connext_static/src/functions.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 9f3aac4..9353ffc 100644 --- a/rclcpp/CMakeLists.txt +++ b/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() diff --git a/ros_dds_connext_dynamic/CMakeLists.txt b/ros_dds_connext_dynamic/CMakeLists.txt index 3adf6e0..968d5e1 100644 --- a/ros_dds_connext_dynamic/CMakeLists.txt +++ b/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_LIBRARIES ${ndds_cpp_LIBRARIES}) +#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_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}) diff --git a/ros_dds_connext_dynamic/src/functions.cpp b/ros_dds_connext_dynamic/src/functions.cpp index 1387caf..b64e15f 100644 --- a/ros_dds_connext_dynamic/src/functions.cpp +++ b/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(); - type_code_and_data_writer->dynamic_writer_ = dynamic_writer; - type_code_and_data_writer->type_code_ = type_code; - type_code_and_data_writer->members_ = members; + CustomPublisherInfo* custom_publisher_info = new CustomPublisherInfo(); + custom_publisher_info->dynamic_writer_ = dynamic_writer; + custom_publisher_info->type_code_ = type_code; + 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; - DDSDynamicDataWriter * dynamic_writer = type_code_and_data_writer->dynamic_writer_; - DDS_TypeCode * type_code = type_code_and_data_writer->type_code_; - const ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members = type_code_and_data_writer->members_; + CustomPublisherInfo * custom_publisher_info = (CustomPublisherInfo*)publisher_handle._data; + DDSDynamicDataWriter * dynamic_writer = custom_publisher_info->dynamic_writer_; + DDS_TypeCode * type_code = custom_publisher_info->type_code_; + 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); diff --git a/ros_dds_connext_static/CMakeLists.txt b/ros_dds_connext_static/CMakeLists.txt new file mode 100644 index 0000000..fc1c4b6 --- /dev/null +++ b/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 +) diff --git a/ros_dds_connext_static/bin/ros_dds_connext_static b/ros_dds_connext_static/bin/ros_dds_connext_static new file mode 100755 index 0000000..4623b7f --- /dev/null +++ b/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 ':')") + 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()) diff --git a/ros_dds_connext_static/cmake/ros_dds_connext_static_generate_interfaces.cmake b/ros_dds_connext_static/cmake/ros_dds_connext_static_generate_interfaces.cmake new file mode 100644 index 0000000..cd3d102 --- /dev/null +++ b/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) diff --git a/ros_dds_connext_static/include/ros_dds_connext_static/MessageTypeSupport.h b/ros_dds_connext_static/include/ros_dds_connext_static/MessageTypeSupport.h new file mode 100644 index 0000000..37b5d38 --- /dev/null +++ b/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__ diff --git a/ros_dds_connext_static/package.xml b/ros_dds_connext_static/package.xml new file mode 100644 index 0000000..62f3597 --- /dev/null +++ b/ros_dds_connext_static/package.xml @@ -0,0 +1,25 @@ + + + ros_dds_connext_static + 0.0.0 + Implement the ROS middleware interface using RTI Connext static code generation in C++. + Dirk Thomas + Apache License 2.0 + + ament_cmake + ament_cmake_python + rosidl_cmake + + ament_cmake + rosidl_cmake + + libndds51-dev + ros_middleware_interface + rosidl_generator_cpp + rosidl_generator_dds_connext_cpp + rosidl_generator_dds_idl + + libndds51 + rosidl_generator_cpp + rosidl_generator_dds_connext_cpp + diff --git a/ros_dds_connext_static/resource/msg_TypeSupport.h.template b/ros_dds_connext_static/resource/msg_TypeSupport.h.template new file mode 100644 index 0000000..2b34ff9 --- /dev/null +++ b/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 _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 + +#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)", + ®ister_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__ diff --git a/ros_dds_connext_static/ros_dds_connext_static-extras.cmake b/ros_dds_connext_static/ros_dds_connext_static-extras.cmake new file mode 100644 index 0000000..f5938e5 --- /dev/null +++ b/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") diff --git a/ros_dds_connext_static/ros_dds_connext_static/__init__.py b/ros_dds_connext_static/ros_dds_connext_static/__init__.py new file mode 100644 index 0000000..102f9fc --- /dev/null +++ b/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, ' + + 'typename ContainerAllocator::template rebind::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_ + + @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_ ' % \ + (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 diff --git a/ros_dds_connext_static/src/functions.cpp b/ros_dds_connext_static/src/functions.cpp new file mode 100644 index 0000000..867110c --- /dev/null +++ b/ros_dds_connext_static/src/functions.cpp @@ -0,0 +1,153 @@ +#include +#include + +#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); +} + +} diff --git a/userland/CMakeLists.txt b/userland/CMakeLists.txt index 51d9523..d92c0e1 100644 --- a/userland/CMakeLists.txt +++ b/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})