diff --git a/rclcpp/include/rclcpp/Node.h b/rclcpp/include/rclcpp/Node.h index 7ff0c0f..7043bb8 100644 --- a/rclcpp/include/rclcpp/Node.h +++ b/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 Publisher* create_publisher(const char * topic_name) { - const rosidl_generator_cpp::MessageTypeSupportMembers & members = rosidl_generator_cpp::MessageTypeSupport::get_members(); - void * publisher_handle = ::ros_middleware_interface::create_publisher(node_handle_, members, topic_name); + const rosidl_generator_cpp::MessageTypeSupportHandle & type_support_handle = rosidl_generator_cpp::MessageTypeSupport::get_type_support_handle(); + ros_middleware_interface::PublisherHandle publisher_handle = ::ros_middleware_interface::create_publisher(node_handle_, type_support_handle, topic_name); return new Publisher(publisher_handle); } private: - void * node_handle_; + ros_middleware_interface::NodeHandle node_handle_; }; } diff --git a/rclcpp/include/rclcpp/Publisher.h b/rclcpp/include/rclcpp/Publisher.h index 4228fe2..bea0573 100644 --- a/rclcpp/include/rclcpp/Publisher.h +++ b/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 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_; }; } diff --git a/ros_dds_connext_dynamic/CMakeLists.txt b/ros_dds_connext_dynamic/CMakeLists.txt index 77b2537..3adf6e0 100644 --- a/ros_dds_connext_dynamic/CMakeLists.txt +++ b/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}) diff --git a/ros_dds_connext_dynamic/package.xml b/ros_dds_connext_dynamic/package.xml index 897b4ac..1fc587f 100644 --- a/ros_dds_connext_dynamic/package.xml +++ b/ros_dds_connext_dynamic/package.xml @@ -11,7 +11,9 @@ libndds51-dev ros_middleware_interface rosidl_generator_cpp + ros_dds_cpp_dynamic_typesupport libndds51 rosidl_generator_cpp + ros_dds_cpp_dynamic_typesupport diff --git a/ros_dds_connext_dynamic/src/functions.cpp b/ros_dds_connext_dynamic/src/functions.cpp index 1ae71c3..1387caf 100644 --- a/ros_dds_connext_dynamic/src/functions.cpp +++ b/ros_dds_connext_dynamic/src/functions.cpp @@ -1,55 +1,87 @@ #include +#include #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_; + DDSDynamicDataWriter * dynamic_writer_; + DDS_TypeCode * type_code_; + 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; - DDSDomainParticipant* participant = (DDSDomainParticipant*)node; + 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() 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); - for(unsigned long i = 0; i < members._size; ++i) + type_code = factory->create_struct_tc(type_name.c_str(), struct_members, ex); + 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); - return 0; + 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("register_type() failed. Status = %d\n", status); - return 0; + 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, "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); - return 0; + 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; @@ -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); - std::cout << " create_publisher() set member " << i << ": " << member->_name << "" << std::endl; + const ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMember * member = members->_members + i * sizeof(ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMember); + 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"); }; } diff --git a/ros_dds_cpp_dynamic_typesupport/CMakeLists.txt b/ros_dds_cpp_dynamic_typesupport/CMakeLists.txt new file mode 100644 index 0000000..8d20a3d --- /dev/null +++ b/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 +) diff --git a/ros_dds_cpp_dynamic_typesupport/bin/ros_dds_cpp_dynamic_typesupport b/ros_dds_cpp_dynamic_typesupport/bin/ros_dds_cpp_dynamic_typesupport new file mode 100755 index 0000000..389b706 --- /dev/null +++ b/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 ':')") + 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_cpp_dynamic_typesupport/cmake/ros_dds_cpp_dynamic_typesupport_generate_interfaces.cmake b/ros_dds_cpp_dynamic_typesupport/cmake/ros_dds_cpp_dynamic_typesupport_generate_interfaces.cmake new file mode 100644 index 0000000..c909b52 --- /dev/null +++ b/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) diff --git a/ros_dds_cpp_dynamic_typesupport/include/ros_dds_cpp_dynamic_typesupport/MessageTypeSupport.h b/ros_dds_cpp_dynamic_typesupport/include/ros_dds_cpp_dynamic_typesupport/MessageTypeSupport.h new file mode 100644 index 0000000..b1b40f2 --- /dev/null +++ b/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__ diff --git a/ros_dds_cpp_dynamic_typesupport/package.xml b/ros_dds_cpp_dynamic_typesupport/package.xml new file mode 100644 index 0000000..1c4a85d --- /dev/null +++ b/ros_dds_cpp_dynamic_typesupport/package.xml @@ -0,0 +1,15 @@ + + + ros_dds_cpp_dynamic_typesupport + 0.0.0 + Generate the message type support for dynamic message construction in C++. + Dirk Thomas + Apache License 2.0 + + ament_cmake + ament_cmake_python + rosidl_cmake + + ament_cmake + rosidl_cmake + diff --git a/ros_dds_cpp_dynamic_typesupport/resource/msg_TypeSupport.h.template b/ros_dds_cpp_dynamic_typesupport/resource/msg_TypeSupport.h.template new file mode 100644 index 0000000..ef0dcd5 --- /dev/null +++ b/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 _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__ diff --git a/ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport-extras.cmake b/ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport-extras.cmake new file mode 100644 index 0000000..399dbca --- /dev/null +++ b/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") diff --git a/ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport/__init__.py b/ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport/__init__.py new file mode 100644 index 0000000..102f9fc --- /dev/null +++ b/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, ' + + '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_middleware_interface/include/ros_middleware_interface/functions.h b/ros_middleware_interface/include/ros_middleware_interface/functions.h index c46fea8..173b1f3 100644 --- a/ros_middleware_interface/include/ros_middleware_interface/functions.h +++ b/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); } diff --git a/ros_middleware_interface/include/ros_middleware_interface/handles.h b/ros_middleware_interface/include/ros_middleware_interface/handles.h new file mode 100644 index 0000000..0dc6a9f --- /dev/null +++ b/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__