diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt new file mode 100644 index 0000000..9f3aac4 --- /dev/null +++ b/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 +) diff --git a/rclcpp/include/rclcpp/Node.h b/rclcpp/include/rclcpp/Node.h new file mode 100644 index 0000000..7ff0c0f --- /dev/null +++ b/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 + 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); + return new Publisher(publisher_handle); + } + +private: + void * node_handle_; +}; + +} + +rclcpp::Node* create_node() +{ + return new rclcpp::Node(); +} + + +#endif // __rclcpp__Node__h__ diff --git a/rclcpp/include/rclcpp/Publisher.h b/rclcpp/include/rclcpp/Publisher.h new file mode 100644 index 0000000..4228fe2 --- /dev/null +++ b/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 +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__ diff --git a/rclcpp/package.xml b/rclcpp/package.xml new file mode 100644 index 0000000..25fa3f6 --- /dev/null +++ b/rclcpp/package.xml @@ -0,0 +1,15 @@ + + + rclcpp + 0.0.0 + The ROS client library in C++. + Dirk Thomas + Apache License 2.0 + + ament_cmake + + ros_middleware_interface + rosidl_default_generators + + ros_dds_connext_dynamic + diff --git a/ros_dds_connext_dynamic/CMakeLists.txt b/ros_dds_connext_dynamic/CMakeLists.txt new file mode 100644 index 0000000..77b2537 --- /dev/null +++ b/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 +) diff --git a/ros_dds_connext_dynamic/package.xml b/ros_dds_connext_dynamic/package.xml new file mode 100644 index 0000000..897b4ac --- /dev/null +++ b/ros_dds_connext_dynamic/package.xml @@ -0,0 +1,17 @@ + + + ros_dds_connext_dynamic + 0.0.0 + Implement the ROS middleware interface using RTI Connext DynammicData interface in C++. + Dirk Thomas + Apache License 2.0 + + ament_cmake + + libndds51-dev + ros_middleware_interface + rosidl_generator_cpp + + libndds51 + rosidl_generator_cpp + diff --git a/ros_dds_connext_dynamic/src/functions.cpp b/ros_dds_connext_dynamic/src/functions.cpp new file mode 100644 index 0000000..1ae71c3 --- /dev/null +++ b/ros_dds_connext_dynamic/src/functions.cpp @@ -0,0 +1,174 @@ +#include + +#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); + }; +} + +} diff --git a/ros_middleware_interface/CMakeLists.txt b/ros_middleware_interface/CMakeLists.txt new file mode 100644 index 0000000..883b4a0 --- /dev/null +++ b/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 +) diff --git a/ros_middleware_interface/include/ros_middleware_interface/functions.h b/ros_middleware_interface/include/ros_middleware_interface/functions.h new file mode 100644 index 0000000..c46fea8 --- /dev/null +++ b/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__ diff --git a/ros_middleware_interface/package.xml b/ros_middleware_interface/package.xml new file mode 100644 index 0000000..2d5e264 --- /dev/null +++ b/ros_middleware_interface/package.xml @@ -0,0 +1,12 @@ + + + ros_middleware_interface + 0.0.0 + The ros_middleware_interface package + Dirk Thomas + Apache License 2.0 + + ament_cmake + + rosidl_generator_cpp + diff --git a/userland/CMakeLists.txt b/userland/CMakeLists.txt new file mode 100644 index 0000000..51d9523 --- /dev/null +++ b/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 +) diff --git a/userland/package.xml b/userland/package.xml new file mode 100644 index 0000000..e3abed4 --- /dev/null +++ b/userland/package.xml @@ -0,0 +1,16 @@ + + + userland + 0.0.0 + The userland package + Dirk Thomas + Apache License 2.0 + + ament_cmake + + rclcpp + std_msgs + + rclcpp + std_msgs + diff --git a/userland/src/main.cpp b/userland/src/main.cpp new file mode 100644 index 0000000..6b0db4f --- /dev/null +++ b/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* p = n->create_publisher("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; +}