first prototype of templated rcl, using non-templated middleware interface, implemented by connext using dynamic data

rclcpp/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+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_include_directories(include)
+
+ament_package()
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)

rclcpp/include/rclcpp/Node.h

@@ -0,0 +1,40 @@
+#ifndef __rclcpp__Node__h__
+#define __rclcpp__Node__h__
+
+#include "Publisher.h"
+
+#include "ros_middleware_interface/functions.h"
+
+
+namespace rclcpp
+{
+
+class Node
+{
+public:
+  Node()
+  {
+    node_handle_ = ::ros_middleware_interface::create_node();
+  }
+
+  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);
+    return new Publisher<ROSMessage>(publisher_handle);
+  }
+
+private:
+  void * node_handle_;
+};
+
+}
+
+rclcpp::Node* create_node()
+{
+  return new rclcpp::Node();
+}
+
+
+#endif  // __rclcpp__Node__h__

rclcpp/include/rclcpp/Publisher.h

@@ -0,0 +1,29 @@
+#ifndef __rclcpp__Publisher__h__
+#define __rclcpp__Publisher__h__
+
+#include "ros_middleware_interface/functions.h"
+
+
+namespace rclcpp
+{
+
+template<typename ROSMessage>
+class Publisher
+{
+public:
+  Publisher(void * publisher_handle)
+    : publisher_handle_(publisher_handle)
+  {}
+
+  void publish(const ROSMessage& ros_message)
+  {
+    ::ros_middleware_interface::publish(publisher_handle_, &ros_message);
+  }
+
+private:
+  void * publisher_handle_;
+};
+
+}
+
+#endif  // __rclcpp__Publisher__h__

rclcpp/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rclcpp</name>
+  <version>0.0.0</version>
+  <description>The ROS client library in C++.</description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>ros_middleware_interface</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <build_depend>ros_dds_connext_dynamic</build_depend>
+</package>

ros_dds_connext_dynamic/CMakeLists.txt

@@ -0,0 +1,27 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(ros_dds_connext_dynamic)
+
+find_package(ament_cmake REQUIRED)
+find_package(ros_middleware_interface REQUIRED)
+find_package(rosidl_generator_cpp 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_libraries(ros_dds_connext_dynamic dl pthread)
+
+ament_package()
+
+include_directories(${ros_middleware_interface_INCLUDE_DIRS} ${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})
+
+install(
+  TARGETS ros_dds_connext_dynamic
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+)

ros_dds_connext_dynamic/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_dds_connext_dynamic</name>
+  <version>0.0.0</version>
+  <description>Implement the ROS middleware interface using RTI Connext DynammicData interface in C++.</description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>libndds51-dev</build_depend>
+  <build_depend>ros_middleware_interface</build_depend>
+  <build_depend>rosidl_generator_cpp</build_depend>
+
+  <build_export_depend>libndds51</build_export_depend>
+  <build_export_depend>rosidl_generator_cpp</build_export_depend>
+</package>

ros_dds_connext_dynamic/src/functions.cpp

@@ -0,0 +1,174 @@
+#include <iostream>
+
+#include "ndds/ndds_cpp.h"
+
+#include "rosidl_generator_cpp/MessageTypeSupport.h"
+
+namespace ros_middleware_interface
+{
+
+void * create_node()
+{
+    std::cout << "create_node()" << std::endl;
+
+    std::cout << "  create_node() get_instance" << std::endl;
+    DDSDomainParticipantFactory* dpf_ = DDSDomainParticipantFactory::get_instance();
+    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);
+
+    std::cout << "  create_node() pass opaque node handle" << std::endl;
+
+    return participant;
+}
+
+struct TypeCodeAndDataWriter {
+  DDSDynamicDataWriter* dynamic_writer_;
+  DDS_TypeCode* type_code_;
+  rosidl_generator_cpp::MessageTypeSupportMembers members_;
+};
+
+void * create_publisher(void * node, const rosidl_generator_cpp::MessageTypeSupportMembers & members, 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;
+
+    std::cout << "  create_publisher() create type code" << 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();
+
+        type_code = factory->create_struct_tc("test_type", 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);
+            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)
+            {
+                member_type_code = factory->get_primitive_tc(DDS_TK_LONG);
+            }
+            else
+            {
+                printf("unknown type %s\n", member->_type);
+                return 0;
+            }
+            type_code->add_member(member->_name, DDS_TYPECODE_MEMBER_ID_INVALID, member_type_code,
+                        DDS_TYPECODE_NONKEY_REQUIRED_MEMBER, ex);
+        }
+        DDS_StructMemberSeq_finalize(&struct_members);
+    }
+
+
+    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");
+    if (status != DDS_RETCODE_OK) {
+        printf("register_type() failed. Status = %d\n", status);
+        return 0;
+    };
+
+    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;
+    };
+
+    std::cout << "  create_publisher() create dds publisher" << std::endl;
+    DDSPublisher* dds_publisher = participant->create_publisher(
+        publisher_qos, NULL, DDS_STATUS_MASK_NONE);
+
+
+    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;
+    };
+
+    std::cout << "  create_publisher() create topic" << std::endl;
+    DDSTopic* topic = participant->create_topic(
+        topic_name, "my_type", default_topic_qos, NULL,
+        DDS_STATUS_MASK_NONE
+    );
+
+
+    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;
+    };
+
+    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);
+
+    DDSDynamicDataWriter* dynamic_writer = DDSDynamicDataWriter::narrow(topic_writer);
+    if (!dynamic_writer) {
+        printf("narrow() failed.\n");
+        return 0;
+    };
+
+
+    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;
+
+    return type_code_and_data_writer;
+}
+
+void publish(void * publisher, const void * ros_message)
+{
+    std::cout << "publish()" << std::endl;
+
+    std::cout << "  publish() extract data writer and type code from opaque publisher handle" << std::endl;
+    TypeCodeAndDataWriter * type_code_and_data_writer = (TypeCodeAndDataWriter*)publisher;
+    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_;
+
+    std::cout << "  publish() create 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)
+    {
+        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;
+        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;
+            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);
+            };
+        }
+        else
+        {
+            printf("unknown type %s\n", member->_type);
+            return;
+        }
+    }
+
+    std::cout << "  publish() write dynamic data" << std::endl;
+    DDS_ReturnCode_t status = dynamic_writer->write(dynamic_data, DDS_HANDLE_NIL);
+    if (status != DDS_RETCODE_OK) {
+        printf("write() failed. Status = %d\n", status);
+    };
+}
+
+}

ros_middleware_interface/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(ros_middleware_interface)
+
+find_package(ament_cmake REQUIRED)
+
+ament_export_include_directories(include)
+
+ament_package()
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)

ros_middleware_interface/include/ros_middleware_interface/functions.h

@@ -0,0 +1,17 @@
+#ifndef __ros_middleware_interface__functions__h__
+#define __ros_middleware_interface__functions__h__
+
+#include "rosidl_generator_cpp/MessageTypeSupport.h"
+
+namespace ros_middleware_interface
+{
+
+void * create_node();
+
+void * create_publisher(void * node, const rosidl_generator_cpp::MessageTypeSupportMembers & members, const char * topic_name);
+
+void publish(void * publisher, const void * ros_message);
+
+}
+
+#endif  // __ros_middleware_interface__functions__h__

ros_middleware_interface/package.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros_middleware_interface</name>
+  <version>0.0.0</version>
+  <description>The ros_middleware_interface package</description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_export_depend>rosidl_generator_cpp</build_export_depend>
+</package>

userland/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(userland)
+
+set(CMAKE_CXX_FLAGS "-std=c++0x")
+
+find_package(ros_middleware_interface REQUIRED)
+find_package(ros_dds_connext_dynamic REQUIRED)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+ament_package()
+
+include_directories(${rclcpp_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS})
+add_executable(userland src/main.cpp)
+target_link_libraries(userland ${rclcpp_LIBRARIES} ${std_msgs_LIBRARIES})
+
+install(
+  TARGETS userland
+  DESTINATION bin
+)

userland/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>userland</name>
+  <version>0.0.0</version>
+  <description>The userland package</description>
+  <maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+</package>

userland/src/main.cpp

@@ -0,0 +1,16 @@
+#include "rclcpp/Node.h"
+#include "rclcpp/Publisher.h"
+#include "std_msgs/Int32.h"
+
+
+main(int argc, char** argv)
+{
+  rclcpp::Node* n = create_node();
+  rclcpp::Publisher<std_msgs::Int32>* p = n->create_publisher<std_msgs::Int32>("topic_name");
+  std_msgs::Int32 ros_msg;
+  ros_msg.data = 23;
+  p->publish(ros_msg);
+  ros_msg.data = 42;
+  p->publish(ros_msg);
+  return 0;
+}