niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

add ros_dds_cpp_dynamic_typesupport

Browse source
This commit is contained in:
Dirk Thomas 2014-08-01 13:30:19 -07:00
parent bcecf00928
commit 6b07c8f928
15 changed files with 480 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

7
rclcpp/include/rclcpp/Node.h
View file

@ -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();
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<ROSMessage>::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<ROSMessage>(publisher_handle);
}
private:
void * node_handle_;
ros_middleware_interface::NodeHandle node_handle_;
};
}

5
rclcpp/include/rclcpp/Publisher.h
View file

@ -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_;
};
}

4
ros_dds_connext_dynamic/CMakeLists.txt
View file

@ -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})

2
ros_dds_connext_dynamic/package.xml
View file

@ -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>

117
ros_dds_connext_dynamic/src/functions.cpp
View file

@ -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;
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");
};
}

28
ros_dds_cpp_dynamic_typesupport/CMakeLists.txt Normal file
View file

@ -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
)

45
ros_dds_cpp_dynamic_typesupport/bin/ros_dds_cpp_dynamic_typesupport Executable file
View file

@ -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())

61
ros_dds_cpp_dynamic_typesupport/cmake/ros_dds_cpp_dynamic_typesupport_generate_interfaces.cmake Normal file
View file

@ -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)

25
ros_dds_cpp_dynamic_typesupport/include/ros_dds_cpp_dynamic_typesupport/MessageTypeSupport.h Normal file
View file

@ -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__

15
ros_dds_cpp_dynamic_typesupport/package.xml Normal file
View file

@ -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>

75
ros_dds_cpp_dynamic_typesupport/resource/msg_TypeSupport.h.template Normal file
View file

@ -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__

12
ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport-extras.cmake Normal file
View file

@ -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")

95
ros_dds_cpp_dynamic_typesupport/ros_dds_cpp_dynamic_typesupport/__init__.py Normal file
View file

@ -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

8
ros_middleware_interface/include/ros_middleware_interface/functions.h
View file

@ -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);
}

19
ros_middleware_interface/include/ros_middleware_interface/handles.h Normal file
View file

@ -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__