From 277d676aa2ecde36b71147ba0bb1301ff5545865 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Mon, 4 Aug 2014 12:09:47 -0700 Subject: [PATCH] measure time static vs. dynamic --- ros_dds_connext_dynamic/src/functions.cpp | 28 ++++++++++++------- .../resource/msg_TypeSupport.h.template | 15 ++++++---- ros_dds_connext_static/src/functions.cpp | 8 ++++-- userland/src/main.cpp | 26 ++++++++++++++--- 4 files changed, 54 insertions(+), 23 deletions(-) diff --git a/ros_dds_connext_dynamic/src/functions.cpp b/ros_dds_connext_dynamic/src/functions.cpp index b64e15f..8adffd9 100644 --- a/ros_dds_connext_dynamic/src/functions.cpp +++ b/ros_dds_connext_dynamic/src/functions.cpp @@ -44,6 +44,7 @@ ros_middleware_interface::NodeHandle create_node() } struct CustomPublisherInfo { + DDSDynamicDataTypeSupport * dynamic_data_type_support_; DDSDynamicDataWriter * dynamic_writer_; DDS_TypeCode * type_code_; ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members_; @@ -60,6 +61,8 @@ ros_middleware_interface::PublisherHandle create_publisher(const ros_middleware_ throw std::runtime_error("node handle not from this implementation"); } + std::cout << "create_publisher() " << node_handle._implementation_identifier << std::endl; + std::cout << " create_publisher() extract participant from opaque node handle" << std::endl; DDSDomainParticipant* participant = (DDSDomainParticipant*)node_handle._data; @@ -165,6 +168,7 @@ ros_middleware_interface::PublisherHandle create_publisher(const ros_middleware_ std::cout << " create_publisher() build opaque publisher handle" << std::endl; CustomPublisherInfo* custom_publisher_info = new CustomPublisherInfo(); + custom_publisher_info->dynamic_data_type_support_ = ddts; custom_publisher_info->dynamic_writer_ = dynamic_writer; custom_publisher_info->type_code_ = type_code; custom_publisher_info->members_ = members; @@ -178,7 +182,7 @@ ros_middleware_interface::PublisherHandle create_publisher(const ros_middleware_ void publish(const ros_middleware_interface::PublisherHandle& publisher_handle, const void * ros_message) { - std::cout << "publish()" << std::endl; + //std::cout << "publish()" << std::endl; if (publisher_handle._implementation_identifier != _rti_connext_identifier) @@ -188,24 +192,26 @@ void publish(const ros_middleware_interface::PublisherHandle& publisher_handle, 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; + //std::cout << " publish() extract data writer and type code from opaque publisher handle" << std::endl; CustomPublisherInfo * custom_publisher_info = (CustomPublisherInfo*)publisher_handle._data; + DDSDynamicDataTypeSupport* ddts = custom_publisher_info->dynamic_data_type_support_; 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); + //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); + DDS_DynamicData * dynamic_data = ddts->create_data(); for(unsigned long i = 0; i < members->_size; ++i) { 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; + //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 << " 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); + long value = *((int*)((char*)ros_message + member->_offset)); + //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"); @@ -218,12 +224,14 @@ void publish(const ros_middleware_interface::PublisherHandle& publisher_handle, } } - std::cout << " publish() write dynamic data" << std::endl; - DDS_ReturnCode_t status = dynamic_writer->write(dynamic_data, DDS_HANDLE_NIL); + //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); throw std::runtime_error("write failed"); }; + + ddts->delete_data(dynamic_data); } } diff --git a/ros_dds_connext_static/resource/msg_TypeSupport.h.template b/ros_dds_connext_static/resource/msg_TypeSupport.h.template index 2b34ff9..fe623f7 100644 --- a/ros_dds_connext_static/resource/msg_TypeSupport.h.template +++ b/ros_dds_connext_static/resource/msg_TypeSupport.h.template @@ -36,7 +36,7 @@ void register_type__@(spec.base_type.type)(DDSDomainParticipant * participant, c 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; + //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) @@ -74,18 +74,21 @@ void convert_ros_message_to_dds__@(spec.base_type.type)(const void * untyped_ros 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; + //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)_ dds_message; + @(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_TypeSupport::create_data(); + 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); + //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"); }; + + @(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_TypeSupport::delete_data(dds_message); } diff --git a/ros_dds_connext_static/src/functions.cpp b/ros_dds_connext_static/src/functions.cpp index 867110c..2f1c96d 100644 --- a/ros_dds_connext_static/src/functions.cpp +++ b/ros_dds_connext_static/src/functions.cpp @@ -59,6 +59,8 @@ ros_middleware_interface::PublisherHandle create_publisher(const ros_middleware_ throw std::runtime_error("node handle not from this implementation"); } + std::cout << "create_publisher() " << node_handle._implementation_identifier << std::endl; + std::cout << " create_publisher() extract participant from opaque node handle" << std::endl; DDSDomainParticipant* participant = (DDSDomainParticipant*)node_handle._data; @@ -131,7 +133,7 @@ ros_middleware_interface::PublisherHandle create_publisher(const ros_middleware_ void publish(const ros_middleware_interface::PublisherHandle& publisher_handle, const void * ros_message) { - std::cout << "publish()" << std::endl; + //std::cout << "publish()" << std::endl; if (publisher_handle._implementation_identifier != _rti_connext_identifier) { @@ -140,13 +142,13 @@ void publish(const ros_middleware_interface::PublisherHandle& publisher_handle, 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; + //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; + //std::cout << " publish() invoke publish callback" << std::endl; callbacks->_publish(topic_writer, ros_message); } diff --git a/userland/src/main.cpp b/userland/src/main.cpp index 6b0db4f..3b4ef6f 100644 --- a/userland/src/main.cpp +++ b/userland/src/main.cpp @@ -1,3 +1,6 @@ +#include +#include + #include "rclcpp/Node.h" #include "rclcpp/Publisher.h" #include "std_msgs/Int32.h" @@ -8,9 +11,24 @@ 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); + + int number_of_msgs = 1000000; + + timespec start; + clock_gettime(CLOCK_REALTIME, &start); + + for (int i = 1; i < number_of_msgs; ++i) { + ros_msg.data = i; + p->publish(ros_msg); + if (i % 100000 == 0) { + std::cout << "published ros msg #" << i << std::endl; + } + } + + timespec end; + clock_gettime(CLOCK_REALTIME, &end); + + std::cout << (end.tv_sec - start.tv_sec) << "." << (end.tv_nsec - start.tv_nsec) << std::endl; + return 0; }