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

measure time static vs. dynamic

Browse source
This commit is contained in:
Dirk Thomas 2014-08-04 12:09:47 -07:00
parent 97322affd2
commit 277d676aa2
4 changed files with 54 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

28
ros_dds_connext_dynamic/src/functions.cpp
View file

@ -44,6 +44,7 @@ ros_middleware_interface::NodeHandle create_node()
} }
struct CustomPublisherInfo { struct CustomPublisherInfo {
DDSDynamicDataTypeSupport * dynamic_data_type_support_;
DDSDynamicDataWriter * dynamic_writer_; DDSDynamicDataWriter * dynamic_writer_;
DDS_TypeCode * type_code_; DDS_TypeCode * type_code_;
ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members_; 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"); 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; std::cout << " create_publisher() extract participant from opaque node handle" << std::endl;
DDSDomainParticipant* participant = (DDSDomainParticipant*)node_handle._data; 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; std::cout << " create_publisher() build opaque publisher handle" << std::endl;
CustomPublisherInfo* custom_publisher_info = new CustomPublisherInfo(); CustomPublisherInfo* custom_publisher_info = new CustomPublisherInfo();
custom_publisher_info->dynamic_data_type_support_ = ddts;
custom_publisher_info->dynamic_writer_ = dynamic_writer; custom_publisher_info->dynamic_writer_ = dynamic_writer;
custom_publisher_info->type_code_ = type_code; custom_publisher_info->type_code_ = type_code;
custom_publisher_info->members_ = members; 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) 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) 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"); 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; 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_; DDSDynamicDataWriter * dynamic_writer = custom_publisher_info->dynamic_writer_;
DDS_TypeCode * type_code = custom_publisher_info->type_code_; DDS_TypeCode * type_code = custom_publisher_info->type_code_;
const ros_dds_cpp_dynamic_typesupport::MessageTypeSupportMembers * members = custom_publisher_info->members_; 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; //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(type_code, DDS_DYNAMIC_DATA_PROPERTY_DEFAULT);
DDS_DynamicData * dynamic_data = ddts->create_data();
for(unsigned long i = 0; i < members->_size; ++i) 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); 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; DDS_TypeCode * member_type_code;
if (strcmp(member->_type, "int32") == 0) if (strcmp(member->_type, "int32") == 0)
{ {
long value = *((long*)((char*)ros_message + member->_offset)); long value = *((int*)((char*)ros_message + member->_offset));
std::cout << " publish() 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); DDS_ReturnCode_t status = dynamic_data->set_long(member->_name, DDS_DYNAMIC_DATA_MEMBER_ID_UNSPECIFIED, value);
if (status != DDS_RETCODE_OK) { if (status != DDS_RETCODE_OK) {
printf("set_long() failed. Status = %d\n", status); printf("set_long() failed. Status = %d\n", status);
throw std::runtime_error("set member failed"); 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; //std::cout << " publish() write dynamic data" << std::endl;
DDS_ReturnCode_t status = dynamic_writer->write(dynamic_data, DDS_HANDLE_NIL); DDS_ReturnCode_t status = dynamic_writer->write(*dynamic_data, DDS_HANDLE_NIL);
if (status != DDS_RETCODE_OK) { if (status != DDS_RETCODE_OK) {
printf("write() failed. Status = %d\n", status); printf("write() failed. Status = %d\n", status);
throw std::runtime_error("write failed"); throw std::runtime_error("write failed");
}; };
ddts->delete_data(dynamic_data);
} }
} }

15
ros_dds_connext_static/resource/msg_TypeSupport.h.template
View file

@ -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) 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; 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]@ @[for field in spec.fields]@
// field.name @(field.name) // 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) 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; //@(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)_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); @(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; //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); DDS_ReturnCode_t status = data_writer->write(*dds_message, DDS_HANDLE_NIL);
if (status != DDS_RETCODE_OK) { if (status != DDS_RETCODE_OK) {
printf("write() failed. Status = %d\n", status); printf("write() failed. Status = %d\n", status);
throw std::runtime_error("write failed"); throw std::runtime_error("write failed");
}; };
@(spec.base_type.pkg_name)::dds_idl::@(spec.base_type.type)_TypeSupport::delete_data(dds_message);
} }

8
ros_dds_connext_static/src/functions.cpp
View file

@ -59,6 +59,8 @@ ros_middleware_interface::PublisherHandle create_publisher(const ros_middleware_
throw std::runtime_error("node handle not from this implementation"); 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; std::cout << " create_publisher() extract participant from opaque node handle" << std::endl;
DDSDomainParticipant* participant = (DDSDomainParticipant*)node_handle._data; 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) 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) 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"); 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; CustomPublisherInfo * custom_publisher_info = (CustomPublisherInfo*)publisher_handle._data;
DDSDataWriter * topic_writer = custom_publisher_info->topic_writer_; DDSDataWriter * topic_writer = custom_publisher_info->topic_writer_;
const ros_dds_connext_static::MessageTypeSupportCallbacks * callbacks = custom_publisher_info->callbacks_; 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); callbacks->_publish(topic_writer, ros_message);
} }

24
userland/src/main.cpp
View file

@ -1,3 +1,6 @@
#include <iostream>
#include <sys/time.h>
#include "rclcpp/Node.h" #include "rclcpp/Node.h"
#include "rclcpp/Publisher.h" #include "rclcpp/Publisher.h"
#include "std_msgs/Int32.h" #include "std_msgs/Int32.h"
@ -8,9 +11,24 @@ main(int argc, char** argv)
rclcpp::Node* n = create_node(); rclcpp::Node* n = create_node();
rclcpp::Publisher<std_msgs::Int32>* p = n->create_publisher<std_msgs::Int32>("topic_name"); rclcpp::Publisher<std_msgs::Int32>* p = n->create_publisher<std_msgs::Int32>("topic_name");
std_msgs::Int32 ros_msg; std_msgs::Int32 ros_msg;
ros_msg.data = 23;
p->publish(ros_msg); int number_of_msgs = 1000000;
ros_msg.data = 42;
timespec start;
clock_gettime(CLOCK_REALTIME, &start);
for (int i = 1; i < number_of_msgs; ++i) {
ros_msg.data = i;
p->publish(ros_msg); 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; return 0;
} }