From 350fcc4324833ca00cd2998bb738f3f9d42fd3c7 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 23 Oct 2017 11:09:24 -0700 Subject: [PATCH 01/34] publish_raw function with type cast --- rmw_connext_cpp/src/rmw_publish.cpp | 50 +++++++++++++++- .../resource/msg__type_support_c.cpp.em | 4 +- .../connext_static_message_handle.hpp | 29 +++++++++ .../message_type_support.h | 4 +- .../resource/msg__type_support.cpp.em | 59 ++++++++++++------- 5 files changed, 122 insertions(+), 24 deletions(-) create mode 100644 rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index ddc26b29..304991c0 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -19,6 +19,8 @@ #include "rmw_connext_cpp/identifier.hpp" #include "rmw_connext_cpp/connext_static_publisher_info.hpp" +#include "rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp" + extern "C" { rmw_ret_t @@ -54,7 +56,53 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) return RMW_RET_ERROR; } - bool published = callbacks->publish(topic_writer, ros_message); + ConnextStaticMessageHandle message_handle; + message_handle.untyped_ros_message = ros_message; + bool published = callbacks->publish(topic_writer, &message_handle); + if (!published) { + RMW_SET_ERROR_MSG("failed to publish message"); + return RMW_RET_ERROR; + } + return RMW_RET_OK; +} + +rmw_ret_t +rmw_publish_raw(const rmw_publisher_t * publisher, const rmw_message_raw_t * raw_message) +{ + if (!publisher) { + RMW_SET_ERROR_MSG("publisher handle is null"); + return RMW_RET_ERROR; + } + if (publisher->implementation_identifier != rti_connext_identifier) { + RMW_SET_ERROR_MSG("publisher handle is not from this rmw implementation"); + return RMW_RET_ERROR; + } + if (!raw_message) { + RMW_SET_ERROR_MSG("raw message handle is null"); + return RMW_RET_ERROR; + } + + ConnextStaticPublisherInfo * publisher_info = + static_cast(publisher->data); + if (!publisher_info) { + RMW_SET_ERROR_MSG("publisher info handle is null"); + return RMW_RET_ERROR; + } + const message_type_support_callbacks_t * callbacks = publisher_info->callbacks_; + if (!callbacks) { + RMW_SET_ERROR_MSG("callbacks handle is null"); + return RMW_RET_ERROR; + } + DDSDataWriter * topic_writer = publisher_info->topic_writer_; + if (!topic_writer) { + RMW_SET_ERROR_MSG("topic writer handle is null"); + return RMW_RET_ERROR; + } + + ConnextStaticMessageHandle message_handle; + message_handle.raw_message = raw_message->buffer; + message_handle.raw_message_length = &raw_message->buffer_length; + bool published = callbacks->publish(topic_writer, &message_handle); if (!published) { RMW_SET_ERROR_MSG("failed to publish message"); return RMW_RET_ERROR; diff --git a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em index f3d3fdce..7e45b312 100644 --- a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em +++ b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em @@ -28,6 +28,7 @@ #include "rosidl_typesupport_connext_c/identifier.h" // Provides the definition of the message_type_support_callbacks_t struct. #include "rosidl_typesupport_connext_cpp/message_type_support.h" +#include "rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp" #include "@(pkg)/msg/rosidl_typesupport_connext_c__visibility_control.h" @{header_file_name = get_header_filename_from_msg_name(type)}@ @@ -255,8 +256,9 @@ convert_ros_to_dds(const void * untyped_ros_message, void * untyped_dds_message) } static bool -publish(void * dds_data_writer, const void * untyped_ros_message) +publish(void * dds_data_writer, ConnextStaticMessageHandle* untyped_ros_messageFIX) { + void *untyped_ros_message = (void *) untyped_ros_messageFIX; if (!dds_data_writer) { fprintf(stderr, "data writer handle is null\n"); return false; diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp new file mode 100644 index 00000000..ccc1550b --- /dev/null +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp @@ -0,0 +1,29 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_MESSAGE_HANDLE_HPP_ +#define ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_MESSAGE_HANDLE_HPP_ + +extern "C" +{ +struct ConnextStaticMessageHandle +{ + const void * untyped_ros_message = nullptr; + void * untyped_dds_message = nullptr; + const char * raw_message = nullptr; // making this void for not including rmw dep + const size_t * raw_message_length; // making this void for not including rmw dep +}; +} // extern "C" + +#endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_MESSAGE_HANDLE_HPP_ diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h index 10208f96..a693b859 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h @@ -17,6 +17,8 @@ #include "rosidl_generator_c/message_type_support_struct.h" +#include "rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp" + typedef struct message_type_support_callbacks_t { const char * package_name; @@ -24,7 +26,7 @@ typedef struct message_type_support_callbacks_t // Function to register type with given dds_participant bool (* register_type)(void * dds_participant, const char * type_name); // Function to publish a ROS message with a given DDS data_writer - bool (* publish)(void * dds_data_writer, const void * ros_message); + bool (* publish)(void * dds_data_writer, ConnextStaticMessageHandle * ros_message); // Function to take a ROS message from a dds data reader bool (* take)( void * dds_data_reader, bool ignore_local_publications, void * ros_message, bool * taken, diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index 85c8fbfa..11f0596d 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -149,35 +149,52 @@ convert_ros_message_to_dds( bool publish__@(spec.base_type.type)( void * untyped_topic_writer, - const void * untyped_ros_message) + ConnextStaticMessageHandle * message_handle) { DDSDataWriter * topic_writer = static_cast(untyped_topic_writer); + bool success = false; + //@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * sample = nullptr; + + // in case we have an untyped ros message (classic publish) + if (message_handle->untyped_ros_message) { + const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message = + *(const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *)message_handle->untyped_ros_message; + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); + message_handle->untyped_dds_message = (void *)dds_message; + if (!dds_message) { + return false; + } - const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message = - *(const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *)untyped_ros_message; - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); - if (!dds_message) { - return false; + success = convert_ros_message_to_dds(ros_message, *dds_message); + // in case we have a raw CDR message + // HACK only print data for now, this has no action to be done + } else if (message_handle->raw_message) { + fprintf(stderr, "connext publish callback\n"); + for (size_t i = 0; i < *(message_handle->raw_message_length); ++i) { + fprintf(stderr, "%02x ", message_handle->raw_message[i]); + } + fprintf(stderr, "\n"); + success = true; } - bool success = convert_ros_message_to_dds(ros_message, *dds_message); if (success) { - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter * data_writer = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter::narrow(topic_writer); - if (!data_writer) { - fprintf(stderr, "failed to narrow data writer\n"); - success = false; - } else { - DDS_ReturnCode_t status = data_writer->write(*dds_message, DDS_HANDLE_NIL); - success = status == DDS_RETCODE_OK; + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter * data_writer = + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter::narrow(topic_writer); + if (!data_writer) { + fprintf(stderr, "failed to narrow data writer\n"); + success = false; + } else { + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * sample = reinterpret_cast<@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ *>(message_handle); + DDS_ReturnCode_t status = data_writer->write(*sample, DDS_HANDLE_NIL); + success = status == DDS_RETCODE_OK; + } } - } - - DDS_ReturnCode_t status = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message); - success &= status == DDS_RETCODE_OK; + if (message_handle->untyped_dds_message) { + DDS_ReturnCode_t status = + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(reinterpret_cast<@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ *>(message_handle->untyped_dds_message)); + success &= status == DDS_RETCODE_OK; + } return success; } From 4b2691d6a91fcc0d8cd93218db35fb78a08ce177 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 23 Oct 2017 11:10:05 -0700 Subject: [PATCH 02/34] modify generated serialize function to cope with type castings --- .../__init__.py | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py index d8e65180..6869f52f 100644 --- a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py +++ b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py @@ -110,11 +110,72 @@ def generate_dds_connext_cpp( # modify generated code to avoid unsed global variable warning # which can't be suppressed non-globally with gcc msg_filename = os.path.join(output_path, msg_name + '.h') + # TODO(karsten1987): Modify should take array of callbacks + # to avoid multiple file readings _modify(msg_filename, pkg_name, msg_name, _inject_unused_attribute) + plugin_filename = os.path.join(output_path, msg_name + 'Plugin.cxx') + _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_serialize_function) + return 0 +def _get_serialization_code(msg_name, indentation): + val = ("{{\n" + "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" + "{indentation}const ConnextStaticMessageHandle * message_handle =\n" + "{indentation} reinterpret_cast(fake_sample);\n" + "{indentation}if (message_handle->raw_message) {{\n" + "{indentation} memcpy(stream->_buffer, message_handle->raw_message, " + "*(message_handle->raw_message_length));\n" + "{indentation} stream->_relativeBuffer = stream->_buffer;\n" + "{indentation} stream->_tmpRelativeBuffer = stream->_buffer;\n" + "{indentation} stream->_buffer = stream->_buffer;\n" + "{indentation} //stream->_endian = \'\\x01\';\n" + "{indentation} //stream->_nativeEndian = \'\\x01\';\n" + "{indentation} //stream->_encapsulationKind = 1;\n" + "{indentation} //stream->_zeroOnAlign = 0;\n" + "{indentation} stream->_currentPosition = " + "stream->_buffer + *(message_handle->raw_message_length);\n" + "{indentation} return RTI_TRUE;\n" + "{indentation}}}\n" + "{indentation}const {msg_name} * sample = reinterpret_cast " + "(message_handle->untyped_dds_message);\n" + .format(indentation=indentation, msg_name=msg_name)) + return val + + +def _modify_plugin_serialize_function(pkg_name, msg_name, lines): + # set include correctly - line 49 is the last generated include + if lines[49] == '': + lines[49] = ('\n// MODIFIED FOR ROS2 PURPOSES\n#include \"' + 'rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp\"\n') + + serialize_fcn_signature = msg_name + 'Plugin_serialize(' + print("looking for '%s' serialize function" % serialize_fcn_signature) + signature_found = False + injection_start = None + for index, line in enumerate(lines): + if not signature_found: + if line.lstrip().startswith(serialize_fcn_signature): + signature_found = True + else: + if '{' in line.lstrip(): + print("found %s serialize function in line: %d" % (msg_name, index)) + injection_start = index + break + if not signature_found: + raise RuntimeError('failed to locate %sPlugin_serialize function' % msg_name) + + # rename message argument from sample to fake_sample + # this eases the modification within the serialize function + print(lines[injection_start - 6]) + lines[injection_start - 6] = lines[injection_start - 6].replace('sample', 'fake_sample') + indentation = ' ' * 16 + lines[injection_start] = line.replace('{', _get_serialization_code(msg_name, indentation)) + return True + + def _inject_unused_attribute(pkg_name, msg_name, lines): # prepend attribute before constants of string type prefix = 'static const DDS_Char * Constants__' From dd840c75c9079fb4d70d6a35b4c3e738882fab4b Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 14 Nov 2017 15:29:48 -0800 Subject: [PATCH 03/34] WIP publish and subscribe raw --- .../connext_static_client_info.hpp | 1 + .../connext_static_publisher_info.hpp | 1 + .../connext_static_service_info.hpp | 1 + .../connext_static_subscriber_info.hpp | 1 + rmw_connext_cpp/src/rmw_client.cpp | 8 + rmw_connext_cpp/src/rmw_publish.cpp | 33 ++- rmw_connext_cpp/src/rmw_publisher.cpp | 26 ++- rmw_connext_cpp/src/rmw_request.cpp | 2 +- rmw_connext_cpp/src/rmw_service.cpp | 25 +- rmw_connext_cpp/src/rmw_subscription.cpp | 27 ++- rmw_connext_cpp/src/rmw_take.cpp | 116 +++++++++- .../resource/msg__type_support_c.cpp.em | 38 +++- .../connext_static_cdr_stream.hpp | 27 +++ .../message_type_support.h | 14 +- ...msg__rosidl_typesupport_connext_cpp.hpp.em | 16 +- .../resource/msg__type_support.cpp.em | 146 +++++++----- .../__init__.py | 214 +++++++++++++----- 17 files changed, 547 insertions(+), 149 deletions(-) create mode 100644 rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_client_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_client_info.hpp index 3fc8728d..e282d0c3 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_client_info.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_client_info.hpp @@ -27,6 +27,7 @@ struct ConnextStaticClientInfo DDSDataReader * response_datareader_; DDSReadCondition * read_condition_; const service_type_support_callbacks_t * callbacks_; + bool raw_stream_subscriber = true; }; } // extern "C" diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp index 11ce127c..b6f6185c 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp @@ -27,6 +27,7 @@ struct ConnextStaticPublisherInfo DDSDataWriter * topic_writer_; const message_type_support_callbacks_t * callbacks_; rmw_gid_t publisher_gid; + bool raw_stream_publisher = true; }; } // extern "C" diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_service_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_service_info.hpp index b57e28bd..8621532f 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_service_info.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_service_info.hpp @@ -27,6 +27,7 @@ struct ConnextStaticServiceInfo DDSDataReader * request_datareader_; DDSReadCondition * read_condition_; const service_type_support_callbacks_t * callbacks_; + bool raw_stream_subscriber = true; }; } // extern "C" diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp index c513f384..2b2702a6 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp @@ -28,6 +28,7 @@ struct ConnextStaticSubscriberInfo DDSReadCondition * read_condition_; bool ignore_local_publications; const message_type_support_callbacks_t * callbacks_; + bool raw_stream_subscriber = true; }; } // extern "C" diff --git a/rmw_connext_cpp/src/rmw_client.cpp b/rmw_connext_cpp/src/rmw_client.cpp index 14d14bec..f5ea5263 100644 --- a/rmw_connext_cpp/src/rmw_client.cpp +++ b/rmw_connext_cpp/src/rmw_client.cpp @@ -78,6 +78,7 @@ rmw_create_client( DDS_SubscriberQos subscriber_qos; DDS_ReturnCode_t status; DDS_PublisherQos publisher_qos; + DDS_TypeSupportQosPolicy ts; DDS_DataReaderQos datareader_qos; DDS_DataWriterQos datawriter_qos; DDS::Publisher * dds_publisher = nullptr; @@ -121,6 +122,13 @@ rmw_create_client( { goto fail; } + // Set the plugin typesupport to the connext info + // If this is true, a raw CDR Stream is enabled + //ts.plugin_data = &client_info->raw_stream_subscriber; + //ts.cdr_padding_kind = DDS_AUTO_CDR_PADDING; + //datareader_qos.type_support = ts; + //datawriter_qos.type_support = ts; + (void) ts; requester = callbacks->create_requester( participant, request_topic_str, response_topic_str, diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 304991c0..8f0b8fa3 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -56,13 +56,27 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) return RMW_RET_ERROR; } - ConnextStaticMessageHandle message_handle; - message_handle.untyped_ros_message = ros_message; - bool published = callbacks->publish(topic_writer, &message_handle); - if (!published) { + + ConnextStaticCDRStream * my_cdr = new ConnextStaticCDRStream(); + ConnextStaticCDRStream & cdr_stream = *my_cdr; + if (!callbacks->to_cdr_stream(ros_message, &cdr_stream)) { + RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); + return RMW_RET_ERROR; + } + fprintf(stderr, "successfully converted to cdr stream\n"); + if (cdr_stream.message_length == 0) { + RMW_SET_ERROR_MSG("no message length set"); + return RMW_RET_ERROR; + } + if (!cdr_stream.raw_message) { + RMW_SET_ERROR_MSG("no raw message attached"); + return RMW_RET_ERROR; + } + if (!callbacks->publish(topic_writer, &cdr_stream)) { RMW_SET_ERROR_MSG("failed to publish message"); return RMW_RET_ERROR; } + free(cdr_stream.raw_message); return RMW_RET_OK; } @@ -99,10 +113,13 @@ rmw_publish_raw(const rmw_publisher_t * publisher, const rmw_message_raw_t * raw return RMW_RET_ERROR; } - ConnextStaticMessageHandle message_handle; - message_handle.raw_message = raw_message->buffer; - message_handle.raw_message_length = &raw_message->buffer_length; - bool published = callbacks->publish(topic_writer, &message_handle); + ConnextStaticCDRStream cdr_stream; + cdr_stream.raw_message = DDS_String_dup(raw_message->buffer); + cdr_stream.message_length = static_cast(raw_message->buffer_length); + //ConnextStaticMessageHandle message_handle; + //message_handle.raw_message = raw_message->buffer; + //message_handle.raw_message_length = &raw_message->buffer_length; + bool published = callbacks->publish(topic_writer, &cdr_stream); if (!published) { RMW_SET_ERROR_MSG("failed to publish message"); return RMW_RET_ERROR; diff --git a/rmw_connext_cpp/src/rmw_publisher.cpp b/rmw_connext_cpp/src/rmw_publisher.cpp index a10f8fc0..74c6f580 100644 --- a/rmw_connext_cpp/src/rmw_publisher.cpp +++ b/rmw_connext_cpp/src/rmw_publisher.cpp @@ -84,6 +84,7 @@ rmw_create_publisher( // Past this point, a failure results in unrolling code in the goto fail block. bool registered; DDS_DataWriterQos datawriter_qos; + DDS_TypeSupportQosPolicy ts; DDS_PublisherQos publisher_qos; DDS_ReturnCode_t status; DDSPublisher * dds_publisher = nullptr; @@ -159,27 +160,34 @@ rmw_create_publisher( DDS_String_free(topic_str); topic_str = nullptr; + // Allocate memory for the ConnextStaticPublisherInfo object. + buf = rmw_allocate(sizeof(ConnextStaticPublisherInfo)); + if (!buf) { + RMW_SET_ERROR_MSG("failed to allocate memory"); + goto fail; + } + // Use a placement new to construct the ConnextStaticPublisherInfo in the preallocated buffer. + RMW_TRY_PLACEMENT_NEW(publisher_info, buf, goto fail, ConnextStaticPublisherInfo, ) + buf = nullptr; // Only free the publisher_info pointer; don't need the buf pointer anymore. + if (!get_datawriter_qos(participant, *qos_profile, datawriter_qos)) { // error string was set within the function goto fail; } + // Set the plugin typesupport to the connext info + // If this is true, a raw CDR Stream is enabled + ts.plugin_data = &publisher_info->raw_stream_publisher; + ts.cdr_padding_kind = DDS_AUTO_CDR_PADDING; + datawriter_qos.type_support = ts; topic_writer = dds_publisher->create_datawriter( topic, datawriter_qos, NULL, DDS_STATUS_MASK_NONE); if (!topic_writer) { RMW_SET_ERROR_MSG("failed to create datawriter"); goto fail; } + fprintf(stderr, "created datawriter\n"); - // Allocate memory for the ConnextStaticPublisherInfo object. - buf = rmw_allocate(sizeof(ConnextStaticPublisherInfo)); - if (!buf) { - RMW_SET_ERROR_MSG("failed to allocate memory"); - goto fail; - } - // Use a placement new to construct the ConnextStaticPublisherInfo in the preallocated buffer. - RMW_TRY_PLACEMENT_NEW(publisher_info, buf, goto fail, ConnextStaticPublisherInfo, ) - buf = nullptr; // Only free the publisher_info pointer; don't need the buf pointer anymore. publisher_info->dds_publisher_ = dds_publisher; publisher_info->topic_writer_ = topic_writer; publisher_info->callbacks_ = callbacks; diff --git a/rmw_connext_cpp/src/rmw_request.cpp b/rmw_connext_cpp/src/rmw_request.cpp index 772d958e..d6e0de7b 100644 --- a/rmw_connext_cpp/src/rmw_request.cpp +++ b/rmw_connext_cpp/src/rmw_request.cpp @@ -111,7 +111,7 @@ rmw_take_request( } *taken = callbacks->take_request(replier, request_header, ros_request); - + fprintf(stderr, "Correctly taken the request\n"); return RMW_RET_OK; } } // extern "C" diff --git a/rmw_connext_cpp/src/rmw_service.cpp b/rmw_connext_cpp/src/rmw_service.cpp index 3d552997..87385680 100644 --- a/rmw_connext_cpp/src/rmw_service.cpp +++ b/rmw_connext_cpp/src/rmw_service.cpp @@ -75,6 +75,7 @@ rmw_create_service( } // Past this point, a failure results in unrolling code in the goto fail block. + DDS_TypeSupportQosPolicy ts; DDS_DataReaderQos datareader_qos; DDS_DataWriterQos datawriter_qos; DDS_SubscriberQos subscriber_qos; @@ -95,6 +96,15 @@ rmw_create_service( char * request_topic_str = nullptr; char * response_topic_str = nullptr; + buf = rmw_allocate(sizeof(ConnextStaticServiceInfo)); + if (!buf) { + RMW_SET_ERROR_MSG("failed to allocate memory"); + goto fail; + } + // Use a placement new to construct the ConnextStaticServiceInfo in the preallocated buffer. + RMW_TRY_PLACEMENT_NEW(service_info, buf, goto fail, ConnextStaticServiceInfo, ) + buf = nullptr; // Only free the service_info pointer; don't need the buf pointer anymore. + // Begin initializing elements. service = rmw_service_allocate(); if (!service) { @@ -121,6 +131,13 @@ rmw_create_service( { goto fail; } + // Set the plugin typesupport to the connext info + // If this is true, a raw CDR Stream is enabled + //ts.plugin_data = &service_info->raw_stream_subscriber; + //ts.cdr_padding_kind = DDS_AUTO_CDR_PADDING; + //datareader_qos.type_support = ts; + //datawriter_qos.type_support = ts; + (void) ts; replier = callbacks->create_replier( participant, request_topic_str, response_topic_str, @@ -174,14 +191,6 @@ rmw_create_service( // update attached publisher dds_publisher->set_qos(publisher_qos); - buf = rmw_allocate(sizeof(ConnextStaticServiceInfo)); - if (!buf) { - RMW_SET_ERROR_MSG("failed to allocate memory"); - goto fail; - } - // Use a placement new to construct the ConnextStaticServiceInfo in the preallocated buffer. - RMW_TRY_PLACEMENT_NEW(service_info, buf, goto fail, ConnextStaticServiceInfo, ) - buf = nullptr; // Only free the service_info pointer; don't need the buf pointer anymore. service_info->replier_ = replier; service_info->callbacks_ = callbacks; service_info->request_datareader_ = request_datareader; diff --git a/rmw_connext_cpp/src/rmw_subscription.cpp b/rmw_connext_cpp/src/rmw_subscription.cpp index affa408e..9da25371 100644 --- a/rmw_connext_cpp/src/rmw_subscription.cpp +++ b/rmw_connext_cpp/src/rmw_subscription.cpp @@ -79,6 +79,7 @@ rmw_create_subscription( // Past this point, a failure results in unrolling code in the goto fail block. bool registered; DDS_DataReaderQos datareader_qos; + DDS_TypeSupportQosPolicy ts; DDS_SubscriberQos subscriber_qos; DDS_ReturnCode_t status; DDSSubscriber * dds_subscriber = nullptr; @@ -155,11 +156,27 @@ rmw_create_subscription( DDS_String_free(topic_str); topic_str = nullptr; + // Allocate memory for the ConnextStaticSubscriberInfo object. + buf = rmw_allocate(sizeof(ConnextStaticSubscriberInfo)); + if (!buf) { + RMW_SET_ERROR_MSG("failed to allocate memory"); + goto fail; + } + // Use a placement new to construct the ConnextStaticSubscriberInfo in the preallocated buffer. + RMW_TRY_PLACEMENT_NEW(subscriber_info, buf, goto fail, ConnextStaticSubscriberInfo, ) + buf = nullptr; // Only free the subscriber_info pointer; don't need the buf pointer anymore. + if (!get_datareader_qos(participant, *qos_profile, datareader_qos)) { // error string was set within the function goto fail; } + // Set the plugin typesupport to the connext info + // If this is true, a raw CDR Stream is enabled + ts.plugin_data = &subscriber_info->raw_stream_subscriber; + ts.cdr_padding_kind = DDS_AUTO_CDR_PADDING; + datareader_qos.type_support = ts; + topic_reader = dds_subscriber->create_datareader( topic, datareader_qos, NULL, DDS_STATUS_MASK_NONE); @@ -167,6 +184,7 @@ rmw_create_subscription( RMW_SET_ERROR_MSG("failed to create datareader"); goto fail; } + fprintf(stderr, "data reader created\n"); read_condition = topic_reader->create_readcondition( DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE); @@ -175,15 +193,6 @@ rmw_create_subscription( goto fail; } - // Allocate memory for the ConnextStaticSubscriberInfo object. - buf = rmw_allocate(sizeof(ConnextStaticSubscriberInfo)); - if (!buf) { - RMW_SET_ERROR_MSG("failed to allocate memory"); - goto fail; - } - // Use a placement new to construct the ConnextStaticSubscriberInfo in the preallocated buffer. - RMW_TRY_PLACEMENT_NEW(subscriber_info, buf, goto fail, ConnextStaticSubscriberInfo, ) - buf = nullptr; // Only free the subscriber_info pointer; don't need the buf pointer anymore. subscriber_info->dds_subscriber_ = dds_subscriber; subscriber_info->topic_reader_ = topic_reader; subscriber_info->read_condition_ = read_condition; diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index fef16a6b..a3ffcb2b 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -21,8 +21,11 @@ #include "rmw_connext_cpp/identifier.hpp" #include "rmw_connext_cpp/connext_static_subscriber_info.hpp" +#include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" + extern "C" { + rmw_ret_t _take( const rmw_subscription_t * subscription, @@ -65,11 +68,22 @@ _take( return RMW_RET_ERROR; } - bool success = callbacks->take( - topic_reader, subscriber_info->ignore_local_publications, ros_message, taken, - sending_publication_handle); + // fetch the incoming message as cdr stream + ConnextStaticCDRStream cdr_stream; + if (!callbacks->take( + topic_reader, subscriber_info->ignore_local_publications, &cdr_stream, taken, + sending_publication_handle)) + { + RMW_SET_ERROR_MSG("error occured while taking message"); + return RMW_RET_ERROR; + } + // convert the cdr stream to the message + if (!callbacks->to_message(&cdr_stream, ros_message)) { + RMW_SET_ERROR_MSG("can't convert cdr stream to ros message"); + return RMW_RET_ERROR; + } - return success ? RMW_RET_OK : RMW_RET_ERROR; + return RMW_RET_OK; } rmw_ret_t @@ -104,4 +118,98 @@ rmw_take_with_info( return RMW_RET_OK; } + +rmw_ret_t +_take_raw( + const rmw_subscription_t * subscription, + rmw_message_raw_t * raw_message, + bool * taken, + DDS_InstanceHandle_t * sending_publication_handle) +{ + if (!subscription) { + RMW_SET_ERROR_MSG("subscription handle is null"); + return RMW_RET_ERROR; + } + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, rti_connext_identifier, + return RMW_RET_ERROR) + + if (!raw_message) { + RMW_SET_ERROR_MSG("ros message handle is null"); + return RMW_RET_ERROR; + } + if (!taken) { + RMW_SET_ERROR_MSG("taken handle is null"); + return RMW_RET_ERROR; + } + + ConnextStaticSubscriberInfo * subscriber_info = + static_cast(subscription->data); + if (!subscriber_info) { + RMW_SET_ERROR_MSG("subscriber info handle is null"); + return RMW_RET_ERROR; + } + DDSDataReader * topic_reader = subscriber_info->topic_reader_; + if (!topic_reader) { + RMW_SET_ERROR_MSG("topic reader handle is null"); + return RMW_RET_ERROR; + } + const message_type_support_callbacks_t * callbacks = subscriber_info->callbacks_; + if (!callbacks) { + RMW_SET_ERROR_MSG("callbacks handle is null"); + return RMW_RET_ERROR; + } + + // fetch the incoming message as cdr stream + ConnextStaticCDRStream cdr_stream; + if (!callbacks->take( + topic_reader, subscriber_info->ignore_local_publications, &cdr_stream, taken, + sending_publication_handle)) + { + RMW_SET_ERROR_MSG("error occured while taking message"); + return RMW_RET_ERROR; + } + + raw_message->buffer_length = cdr_stream.message_length; + raw_message->buffer = cdr_stream.raw_message; + + return RMW_RET_OK; +} + +rmw_ret_t +rmw_take_raw( + const rmw_subscription_t * subscription, + rmw_message_raw_t * raw_message, + bool * taken) +{ + return _take_raw(subscription, raw_message, taken, nullptr); +} + +rmw_ret_t +rmw_take_raw_with_info( + const rmw_subscription_t * subscription, + rmw_message_raw_t * raw_message, + bool * taken, + rmw_message_info_t * message_info) +{ + if (!message_info) { + RMW_SET_ERROR_MSG("message info is null"); + return RMW_RET_ERROR; + } + DDS_InstanceHandle_t sending_publication_handle; + auto ret = _take_raw(subscription, raw_message, taken, &sending_publication_handle); + if (ret != RMW_RET_OK) { + // Error string is already set. + return RMW_RET_ERROR; + } + + rmw_gid_t * sender_gid = &message_info->publisher_gid; + sender_gid->implementation_identifier = rti_connext_identifier; + memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); + auto detail = reinterpret_cast(sender_gid->data); + detail->publication_handle = sending_publication_handle; + + return RMW_RET_OK; +} } // extern "C" diff --git a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em index 7e45b312..5dcc22d8 100644 --- a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em +++ b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em @@ -28,7 +28,7 @@ #include "rosidl_typesupport_connext_c/identifier.h" // Provides the definition of the message_type_support_callbacks_t struct. #include "rosidl_typesupport_connext_cpp/message_type_support.h" -#include "rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp" +#include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" #include "@(pkg)/msg/rosidl_typesupport_connext_c__visibility_control.h" @{header_file_name = get_header_filename_from_msg_name(type)}@ @@ -44,6 +44,7 @@ # endif #endif #include "@(spec.base_type.pkg_name)/@(subfolder)/dds_connext/@(spec.base_type.type)_Support.h" +#include "@(spec.base_type.pkg_name)/@(subfolder)/dds_connext/@(spec.base_type.type)_Plugin.h" #ifndef _WIN32 # pragma GCC diagnostic pop #endif @@ -256,7 +257,7 @@ convert_ros_to_dds(const void * untyped_ros_message, void * untyped_dds_message) } static bool -publish(void * dds_data_writer, ConnextStaticMessageHandle* untyped_ros_messageFIX) +publish(void * dds_data_writer, ConnextStaticCDRStream * untyped_ros_messageFIX) { void *untyped_ros_message = (void *) untyped_ros_messageFIX; if (!dds_data_writer) { @@ -425,10 +426,11 @@ static bool take( void * dds_data_reader, bool ignore_local_publications, - void * untyped_ros_message, + ConnextStaticCDRStream * cdr_streamFIX, bool * taken, void * sending_publication_handle) { + void * untyped_ros_message = reinterpret_cast(cdr_streamFIX); if (untyped_ros_message == 0) { fprintf(stderr, "invalid ros message pointer\n"); return false; @@ -562,6 +564,34 @@ finally: return false; } + +static bool +to_cdr_stream( + const void * untyped_ros_message, + ConnextStaticCDRStream * cdr_stream) +{ + if (!untyped_ros_message) { + return false; + } + if (!cdr_stream) { + return false; + } + return true; +} + +static bool +to_message( + const ConnextStaticCDRStream * cdr_stream, + void * untyped_ros_message) +{ + if (!cdr_stream) { + return false; + } + if (!untyped_ros_message) { + return false; + } + return true; +} @ @# // Collect the callback functions and provide a function to get the type support struct. @@ -573,6 +603,8 @@ static message_type_support_callbacks_t __callbacks = { take, // take convert_ros_to_dds, // convert_ros_to_dds convert_dds_to_ros, // convert_dds_to_ros + to_cdr_stream, // to_cdr_stream + to_message // to_message }; static rosidl_message_type_support_t __type_support = { diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp new file mode 100644 index 00000000..422e6e8f --- /dev/null +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp @@ -0,0 +1,27 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_CDR_STREAM_HPP_ +#define ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_CDR_STREAM_HPP_ + +extern "C" +{ +struct ConnextStaticCDRStream +{ + char * raw_message = nullptr; + unsigned int message_length = 0; +}; +} // extern "C" + +#endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_CDR_STREAM_HPP_ diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h index a693b859..a52b400c 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h @@ -17,7 +17,7 @@ #include "rosidl_generator_c/message_type_support_struct.h" -#include "rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp" +#include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" typedef struct message_type_support_callbacks_t { @@ -26,10 +26,10 @@ typedef struct message_type_support_callbacks_t // Function to register type with given dds_participant bool (* register_type)(void * dds_participant, const char * type_name); // Function to publish a ROS message with a given DDS data_writer - bool (* publish)(void * dds_data_writer, ConnextStaticMessageHandle * ros_message); + bool (* publish)(void * dds_data_writer, ConnextStaticCDRStream * cdr_stream); // Function to take a ROS message from a dds data reader bool (* take)( - void * dds_data_reader, bool ignore_local_publications, void * ros_message, bool * taken, + void * dds_data_reader, bool ignore_local_publications, ConnextStaticCDRStream * cdr_stream, bool * taken, void * sending_publication_handle); bool (* convert_ros_to_dds)( const void * untyped_ros_message, @@ -37,6 +37,14 @@ typedef struct message_type_support_callbacks_t bool (* convert_dds_to_ros)( const void * untyped_data_message, void * untyped_ros_message); + // Function to serialize a ROS message to a CDR stream + bool (* to_cdr_stream)( + const void * untyped_ros_message, + ConnextStaticCDRStream * cdr_stream); + // Function to deserialize a CDR message to a ROS message + bool (* to_message)( + const ConnextStaticCDRStream * cdr_stream, + void * untyped_ros_message); } message_type_support_callbacks_t; #endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__MESSAGE_TYPE_SUPPORT_H_ diff --git a/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em b/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em index 6b95e9ed..cd927fb3 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em @@ -39,10 +39,14 @@ header_guard_variable = '__'.join([x.upper() for x in header_guard_parts]) + '_' # endif #endif #include "@(spec.base_type.pkg_name)/@(subfolder)/dds_connext/@(spec.base_type.type)_Support.h" +#include "@(spec.base_type.pkg_name)/@(subfolder)/dds_connext/@(spec.base_type.type)_Plugin.h" #ifndef _WIN32 # pragma GCC diagnostic pop #endif +// forward declaration of internal CDR Stream +struct ConnextStaticCDRStream; + // forward declaration of DDS types class DDSDomainParticipant; class DDSDataWriter; @@ -83,9 +87,19 @@ bool take__@(spec.base_type.type)( DDSDataReader * topic_reader, bool ignore_local_publications, - void * untyped_ros_message, + ConnextStaticCDRStream * untyped_ros_message, bool * taken); +bool +to_cdr_stream__@(spec.base_type.type)( + const void * untyped_ros_message, + ConnextStaticCDRStream * cdr_stream); + +bool +to_message__@(spec.base_type.type)( + const ConnextStaticCDRStream * cdr_stream, + void * untyped_ros_message); + } // namespace typesupport_connext_cpp } // namespace @(subfolder) diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index 11f0596d..e8306eb9 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -20,6 +20,7 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" #include "rosidl_typesupport_connext_cpp/identifier.hpp" #include "rosidl_typesupport_connext_cpp/message_type_support.h" #include "rosidl_typesupport_connext_cpp/message_type_support_decl.hpp" @@ -149,52 +150,21 @@ convert_ros_message_to_dds( bool publish__@(spec.base_type.type)( void * untyped_topic_writer, - ConnextStaticMessageHandle * message_handle) + ConnextStaticCDRStream * cdr_stream) { + bool success = true; DDSDataWriter * topic_writer = static_cast(untyped_topic_writer); - bool success = false; - //@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * sample = nullptr; - - // in case we have an untyped ros message (classic publish) - if (message_handle->untyped_ros_message) { - const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message = - *(const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *)message_handle->untyped_ros_message; - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); - message_handle->untyped_dds_message = (void *)dds_message; - if (!dds_message) { - return false; - } - success = convert_ros_message_to_dds(ros_message, *dds_message); - // in case we have a raw CDR message - // HACK only print data for now, this has no action to be done - } else if (message_handle->raw_message) { - fprintf(stderr, "connext publish callback\n"); - for (size_t i = 0; i < *(message_handle->raw_message_length); ++i) { - fprintf(stderr, "%02x ", message_handle->raw_message[i]); - } - fprintf(stderr, "\n"); - success = true; + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter * data_writer = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter::narrow(topic_writer); + if (!data_writer) { + fprintf(stderr, "failed to narrow data writer\n"); + success = false; + } else { + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * sample = reinterpret_cast<@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ *>(cdr_stream); + DDS_ReturnCode_t status = data_writer->write(*sample, DDS_HANDLE_NIL); + success = status == DDS_RETCODE_OK; } - if (success) { - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter * data_writer = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter::narrow(topic_writer); - if (!data_writer) { - fprintf(stderr, "failed to narrow data writer\n"); - success = false; - } else { - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * sample = reinterpret_cast<@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ *>(message_handle); - DDS_ReturnCode_t status = data_writer->write(*sample, DDS_HANDLE_NIL); - success = status == DDS_RETCODE_OK; - } - } - - if (message_handle->untyped_dds_message) { - DDS_ReturnCode_t status = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(reinterpret_cast<@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ *>(message_handle->untyped_dds_message)); - success &= status == DDS_RETCODE_OK; - } return success; } @@ -253,15 +223,15 @@ bool take__@(spec.base_type.type)( void * untyped_topic_reader, bool ignore_local_publications, - void * untyped_ros_message, + ConnextStaticCDRStream * cdr_stream, bool * taken, void * sending_publication_handle) { if (!untyped_topic_reader) { throw std::runtime_error("topic reader handle is null"); } - if (!untyped_ros_message) { - throw std::runtime_error("ros message handle is null"); + if (!cdr_stream) { + throw std::runtime_error("cdr stream handle is null"); } if (!taken) { throw std::runtime_error("taken handle is null"); @@ -319,19 +289,89 @@ take__@(spec.base_type.type)( sample_info.publication_handle; } - bool success = true; if (!ignore_sample) { - @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message = - *(@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *)untyped_ros_message; - success = convert_dds_message_to_ros(dds_messages[0], ros_message); - if (success) { - *taken = true; - } + ConnextStaticCDRStream * input_stream = reinterpret_cast(&dds_messages[0]); + cdr_stream->raw_message = input_stream->raw_message; + cdr_stream->message_length = input_stream->message_length; + fprintf(stderr, "Received stream of length %u\n", cdr_stream->message_length); + *taken = true; } else { *taken = false; } data_reader->return_loan(dds_messages, sample_infos); + return true; +} + +bool +to_cdr_stream__@(spec.base_type.type)( + const void * untyped_ros_message, + ConnextStaticCDRStream * cdr_stream) +{ + if (!cdr_stream) { + return false; + } + if (!untyped_ros_message) { + return false; + } + + // cast the untyped to the known ros message + const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message = + *(const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *)untyped_ros_message; + // create a respective connext dds type + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); + if (!dds_message) { + return false; + } + // convert ros to dds + if (!convert_ros_message_to_dds(ros_message, *dds_message)) { + return false; + } + + // call the serialize function for the first time to get the expected length of the message + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(NULL, &cdr_stream->message_length, dds_message) != RTI_TRUE) { + return false; + } + fprintf(stderr, "message length: %d\n", cdr_stream->message_length); + // allocate enough memory for the CDR stream + cdr_stream->raw_message = (char *)malloc(sizeof(char) * cdr_stream->message_length); + // call the function again and fill the buffer this time + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->raw_message, &cdr_stream->message_length, dds_message) != RTI_TRUE) { + return false; + } + if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK){ + return false; + } + return true; +} + +bool +to_message__@(spec.base_type.type)( + const ConnextStaticCDRStream * cdr_stream, + void * untyped_ros_message) +{ + if (!cdr_stream) { + return false; + } + if (!cdr_stream->raw_message) { + fprintf(stderr, "cdr stream doesn't contain data\n"); + } + if (!untyped_ros_message) { + return false; + } + + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); + if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer(dds_message, cdr_stream->raw_message, cdr_stream->message_length) != RTI_TRUE) { + fprintf(stderr, "deserialize from cdr buffer failed\n"); + return false; + } + + @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message = + *(@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *)untyped_ros_message; + bool success = convert_dds_message_to_ros(*dds_message, ros_message); + if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK){ + return false; + } return success; } @@ -342,7 +382,9 @@ static message_type_support_callbacks_t callbacks = { &publish__@(spec.base_type.type), &take__@(spec.base_type.type), nullptr, - nullptr + nullptr, + &to_cdr_stream__@(spec.base_type.type), + &to_message__@(spec.base_type.type) }; static rosidl_message_type_support_t handle = { diff --git a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py index 6869f52f..5d490055 100644 --- a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py +++ b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py @@ -86,25 +86,25 @@ def generate_dds_connext_cpp( count = 1 max_count = 5 while True: - subprocess.check_call(cmd) - - # fail safe if the generator does not work as expected - any_missing = False - for suffix in ['.h', '.cxx', 'Plugin.h', 'Plugin.cxx', 'Support.h', 'Support.cxx']: - filename = os.path.join(output_path, msg_name + suffix) - if not os.path.exists(filename): - any_missing = True - break - if not any_missing: - break - print("'%s' failed to generate the expected files for '%s/%s'" % - (idl_pp, pkg_name, msg_name), file=sys.stderr) - if count < max_count: - count += 1 - print('Running code generator again (retry %d of %d)...' % - (count, max_count), file=sys.stderr) - continue - raise RuntimeError('failed to generate the expected files') +# subprocess.check_call(cmd) + + # fail safe if the generator does not work as expected + any_missing = False + for suffix in ['.h', '.cxx', 'Plugin.h', 'Plugin.cxx', 'Support.h', 'Support.cxx']: + filename = os.path.join(output_path, msg_name + suffix) + if not os.path.exists(filename): + any_missing = True + break + if not any_missing: + break + print("'%s' failed to generate the expected files for '%s/%s'" % + (idl_pp, pkg_name, msg_name), file=sys.stderr) + if count < max_count: + count += 1 + print('Running code generator again (retry %d of %d)...' % + (count, max_count), file=sys.stderr) + continue + raise RuntimeError('failed to generate the expected files') if os.name != 'nt': # modify generated code to avoid unsed global variable warning @@ -114,43 +114,145 @@ def generate_dds_connext_cpp( # to avoid multiple file readings _modify(msg_filename, pkg_name, msg_name, _inject_unused_attribute) - plugin_filename = os.path.join(output_path, msg_name + 'Plugin.cxx') - _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_serialize_function) +# plugin_filename = os.path.join(output_path, msg_name + 'Plugin.cxx') +# if not "Request" in msg_name and not "Response" in msg_name: +# _modify(plugin_filename, pkg_name, msg_name, _modify_include_headers) +# _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_create_data_function) +# _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_destroy_data_function) +# _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_serialize_function) +# _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_deserialize_function) return 0 +def _get_create_data_code(msg_name, indentation): + val = (\ + "{{\n" + "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" + "{indentation}ConnextStaticCDRStream * cdr_stream = NULL;\n" + "{indentation}RTIOsapiHeap_allocateStructure(&cdr_stream, ConnextStaticCDRStream);\n" + "{indentation}return reinterpret_cast<{msg_name} *>(cdr_stream);\n" + "\n".format(msg_name=msg_name, indentation=indentation)) + return val + + +def _modify_plugin_create_data_function(pkg_name, msg_name, lines): + create_data_fcn_signature = msg_name + 'PluginSupport_create_data_ex(' + print("looking for '%s' create_data function" % create_data_fcn_signature) + signature_found = False + injection_start = None + for index, line in enumerate(lines): + if line.lstrip().startswith(create_data_fcn_signature): + signature_found = True + injection_start = index + break + if not signature_found: + raise RuntimeError('failed to locate %sPlugin_create_data function' % msg_name) + + indentation = ' ' * 16 + lines[injection_start] = line.replace('{', _get_create_data_code(msg_name, indentation)) + return True + + +def _get_destroy_data_code(msg_name, indentation): + val = (\ + "{{\n" + "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" + "{indentation}RTIOsapiHeap_freeStructure(sample);\n" + "{indentation}return;\n".format(indentation=indentation)) + return val + + +def _modify_plugin_destroy_data_function(pkg_name, msg_name, lines): + destroy_fcn_signature = msg_name + 'PluginSupport_destroy_data_ex(' + print("looking for '%s' destroy function" % destroy_fcn_signature) + signature_found = False + injection_start = None + for index, line in enumerate(lines): + if not signature_found: + if line.lstrip().startswith(destroy_fcn_signature): + signature_found = True + else: + if '{' in line.lstrip(): + print("found %s destroy function in line: %d" % (msg_name, index)) + injection_start = index + break + if not signature_found: + raise RuntimeError('failed to locate %sPlugin_destroy function' % msg_name) + + indentation = ' ' * 16 + lines[injection_start] = line.replace('{', _get_destroy_data_code(msg_name, indentation)) + return True + + +def _get_deserialization_code(msg_name, indentation): + val = (\ + "{{\n" + "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" + "{indentation}if (endpoint_plugin_qos) {{\n" + "{indentation} if (!reinterpret_cast(endpoint_plugin_qos)) {{\n" + "{indentation} return RTI_FALSE;\n" + "{indentation} }}\n" + "{indentation} ConnextStaticCDRStream * cdr_stream =\n" + "{indentation} reinterpret_cast(sample);\n" + "{indentation} cdr_stream->raw_message = stream->_buffer;\n" + "{indentation} cdr_stream->message_length = stream->_bufferLength;\n" + "{indentation} return RTI_TRUE;\n" + "{indentation}}}\n" + .format(indentation=indentation)) + return val + + +def _modify_plugin_deserialize_function(pkg_name, msg_name, lines): + deserialize_fcn_signature = msg_name + 'Plugin_deserialize_sample(' + print("looking for '%s' deserialize function" % deserialize_fcn_signature) + signature_found = False + injection_start = None + for index, line in enumerate(lines): + if not signature_found: + if line.lstrip().startswith(deserialize_fcn_signature): + signature_found = True + else: + if '{' in line.lstrip(): + print("found %s deserialize function in line: %d" % (msg_name, index)) + injection_start = index + break + if not signature_found: + raise RuntimeError('failed to locate %sPlugin_deserialize function' % msg_name) + + indentation = ' ' * 16 + lines[injection_start] = line.replace('{', _get_deserialization_code(msg_name, indentation)) + return True + + def _get_serialization_code(msg_name, indentation): - val = ("{{\n" - "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" - "{indentation}const ConnextStaticMessageHandle * message_handle =\n" - "{indentation} reinterpret_cast(fake_sample);\n" - "{indentation}if (message_handle->raw_message) {{\n" - "{indentation} memcpy(stream->_buffer, message_handle->raw_message, " - "*(message_handle->raw_message_length));\n" - "{indentation} stream->_relativeBuffer = stream->_buffer;\n" - "{indentation} stream->_tmpRelativeBuffer = stream->_buffer;\n" - "{indentation} stream->_buffer = stream->_buffer;\n" - "{indentation} //stream->_endian = \'\\x01\';\n" - "{indentation} //stream->_nativeEndian = \'\\x01\';\n" - "{indentation} //stream->_encapsulationKind = 1;\n" - "{indentation} //stream->_zeroOnAlign = 0;\n" - "{indentation} stream->_currentPosition = " - "stream->_buffer + *(message_handle->raw_message_length);\n" - "{indentation} return RTI_TRUE;\n" - "{indentation}}}\n" - "{indentation}const {msg_name} * sample = reinterpret_cast " - "(message_handle->untyped_dds_message);\n" - .format(indentation=indentation, msg_name=msg_name)) + val = (\ + "{{\n" + "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" + "{indentation}if (endpoint_plugin_qos) {{\n" + "{indentation} if (!reinterpret_cast(endpoint_plugin_qos)) {{\n" + "{indentation} return RTI_FALSE;\n" + "{indentation} }}\n" + "{indentation} const ConnextStaticCDRStream * cdr_stream =\n" + "{indentation} reinterpret_cast(sample);\n" + "{indentation} memcpy(stream->_buffer, cdr_stream->raw_message, " + "cdr_stream->message_length);\n" + "{indentation} stream->_relativeBuffer = stream->_buffer;\n" + "{indentation} stream->_tmpRelativeBuffer = stream->_buffer;\n" + "{indentation} stream->_buffer = stream->_buffer;\n" + "{indentation} //stream->_endian = \'\\x01\';\n" + "{indentation} //stream->_nativeEndian = \'\\x01\';\n" + "{indentation} //stream->_encapsulationKind = 1;\n" + "{indentation} //stream->_zeroOnAlign = 0;\n" + "{indentation} stream->_currentPosition = " + "stream->_buffer + cdr_stream->message_length;\n" + "{indentation} return RTI_TRUE;\n" + "{indentation}}}\n" + .format(indentation=indentation, msg_name=msg_name)) return val def _modify_plugin_serialize_function(pkg_name, msg_name, lines): - # set include correctly - line 49 is the last generated include - if lines[49] == '': - lines[49] = ('\n// MODIFIED FOR ROS2 PURPOSES\n#include \"' - 'rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp\"\n') - serialize_fcn_signature = msg_name + 'Plugin_serialize(' print("looking for '%s' serialize function" % serialize_fcn_signature) signature_found = False @@ -167,15 +269,25 @@ def _modify_plugin_serialize_function(pkg_name, msg_name, lines): if not signature_found: raise RuntimeError('failed to locate %sPlugin_serialize function' % msg_name) - # rename message argument from sample to fake_sample - # this eases the modification within the serialize function - print(lines[injection_start - 6]) - lines[injection_start - 6] = lines[injection_start - 6].replace('sample', 'fake_sample') indentation = ' ' * 16 lines[injection_start] = line.replace('{', _get_serialization_code(msg_name, indentation)) return True +def _get_include_headers(): + val = (\ + "\n// MODIFIED FOR ROS2 PURPOSES\n#include \"" + "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp\"\n") + return val + + +def _modify_include_headers(pkg_name, msg_name, lines): + # set include correctly - line 49 is the last generated include + if lines[49] == '': + lines[49] = _get_include_headers() + return True + + def _inject_unused_attribute(pkg_name, msg_name, lines): # prepend attribute before constants of string type prefix = 'static const DDS_Char * Constants__' From e9f37e7143871511af4291be62b0ca32308401e5 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 16 Nov 2017 09:29:28 -0800 Subject: [PATCH 04/34] changes for version 5.3.0 --- .../__init__.py | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py index 5d490055..7c8ba07f 100644 --- a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py +++ b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py @@ -86,7 +86,7 @@ def generate_dds_connext_cpp( count = 1 max_count = 5 while True: -# subprocess.check_call(cmd) + subprocess.check_call(cmd) # fail safe if the generator does not work as expected any_missing = False @@ -114,13 +114,13 @@ def generate_dds_connext_cpp( # to avoid multiple file readings _modify(msg_filename, pkg_name, msg_name, _inject_unused_attribute) -# plugin_filename = os.path.join(output_path, msg_name + 'Plugin.cxx') -# if not "Request" in msg_name and not "Response" in msg_name: -# _modify(plugin_filename, pkg_name, msg_name, _modify_include_headers) -# _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_create_data_function) -# _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_destroy_data_function) -# _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_serialize_function) -# _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_deserialize_function) + plugin_filename = os.path.join(output_path, msg_name + 'Plugin.cxx') + if not "Request" in msg_name and not "Response" in msg_name: + _modify(plugin_filename, pkg_name, msg_name, _modify_include_headers) + _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_create_data_function) + _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_destroy_data_function) + _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_serialize_function) + _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_deserialize_function) return 0 @@ -282,9 +282,15 @@ def _get_include_headers(): def _modify_include_headers(pkg_name, msg_name, lines): - # set include correctly - line 49 is the last generated include - if lines[49] == '': - lines[49] = _get_include_headers() + last_include_found = False + include_line = '#include \"{msg_name}Plugin.h\"'.format(msg_name=msg_name) + for index, line in enumerate(lines): + if include_line in line: + lines[index] = lines[index] + _get_include_headers() + last_include_found = True + break + if not last_include_found: + raise RuntimeError('failed to find last include line', include_line) return True From 95cfa46e91f30c4044efaf89434bd9ba685b1241 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 16 Nov 2017 09:30:14 -0800 Subject: [PATCH 05/34] wip - memory corruption --- rmw_connext_cpp/src/rmw_publish.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 8f0b8fa3..109903f3 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -57,8 +57,7 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) } - ConnextStaticCDRStream * my_cdr = new ConnextStaticCDRStream(); - ConnextStaticCDRStream & cdr_stream = *my_cdr; + ConnextStaticCDRStream cdr_stream; if (!callbacks->to_cdr_stream(ros_message, &cdr_stream)) { RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); return RMW_RET_ERROR; From dd5f792ab9233665c6d1be0364f8e319fe2da8b5 Mon Sep 17 00:00:00 2001 From: karsten1987 Date: Sun, 19 Nov 2017 23:34:19 -0800 Subject: [PATCH 06/34] rtiddsgen 2.5 --- .../rosidl_typesupport_connext_cpp/__init__.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py index 7c8ba07f..5cf76496 100644 --- a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py +++ b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py @@ -142,10 +142,14 @@ def _modify_plugin_create_data_function(pkg_name, msg_name, lines): signature_found = False injection_start = None for index, line in enumerate(lines): - if line.lstrip().startswith(create_data_fcn_signature): - signature_found = True - injection_start = index - break + if not signature_found: + if line.lstrip().startswith(create_data_fcn_signature): + signature_found = True + else: + if '{' in line.lstrip(): + print("fpund %s create ex function in line %d" % (msg_name, index)) + injection_start = index + break if not signature_found: raise RuntimeError('failed to locate %sPlugin_create_data function' % msg_name) From f4a4b804df5779488ed86a22ac4e3a6a73594da7 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 22 Nov 2017 16:30:43 -0800 Subject: [PATCH 07/34] make it work --- rmw_connext_cpp/src/rmw_publish.cpp | 3 +- rmw_connext_shared_cpp/src/qos.cpp | 18 ++++++------ .../connext_static_cdr_stream.hpp | 6 ++-- .../connext_static_message_handle.hpp | 29 ------------------- 4 files changed, 14 insertions(+), 42 deletions(-) delete mode 100644 rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 109903f3..8405b473 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -19,7 +19,7 @@ #include "rmw_connext_cpp/identifier.hpp" #include "rmw_connext_cpp/connext_static_publisher_info.hpp" -#include "rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp" +#include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" extern "C" { @@ -56,7 +56,6 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) return RMW_RET_ERROR; } - ConnextStaticCDRStream cdr_stream; if (!callbacks->to_cdr_stream(ros_message, &cdr_stream)) { RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); diff --git a/rmw_connext_shared_cpp/src/qos.cpp b/rmw_connext_shared_cpp/src/qos.cpp index f855ee8a..1cd0c7b8 100644 --- a/rmw_connext_shared_cpp/src/qos.cpp +++ b/rmw_connext_shared_cpp/src/qos.cpp @@ -65,15 +65,15 @@ get_datawriter_qos( return false; } - status = DDSPropertyQosPolicyHelper::add_property( - datawriter_qos.property, - "dds.data_writer.history.memory_manager.fast_pool.pool_buffer_max_size", - "4096", - DDS_BOOLEAN_FALSE); - if (status != DDS_RETCODE_OK) { - RMW_SET_ERROR_MSG("failed to add qos property"); - return false; - } +// status = DDSPropertyQosPolicyHelper::add_property( +// datawriter_qos.property, +// "dds.data_writer.history.memory_manager.fast_pool.pool_buffer_max_size", +// "4096", +// DDS_BOOLEAN_FALSE); +// if (status != DDS_RETCODE_OK) { +// RMW_SET_ERROR_MSG("failed to add qos property"); +// return false; +// } if (!set_entity_qos_from_profile(qos_profile, datawriter_qos)) { return false; diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp index 422e6e8f..69bb2319 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp @@ -17,11 +17,13 @@ extern "C" { -struct ConnextStaticCDRStream + +typedef struct ConnextStaticCDRStream { char * raw_message = nullptr; unsigned int message_length = 0; -}; +} ConnextStaticCDRStream; + } // extern "C" #endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_CDR_STREAM_HPP_ diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp deleted file mode 100644 index ccc1550b..00000000 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_message_handle.hpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2017 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_MESSAGE_HANDLE_HPP_ -#define ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_MESSAGE_HANDLE_HPP_ - -extern "C" -{ -struct ConnextStaticMessageHandle -{ - const void * untyped_ros_message = nullptr; - void * untyped_dds_message = nullptr; - const char * raw_message = nullptr; // making this void for not including rmw dep - const size_t * raw_message_length; // making this void for not including rmw dep -}; -} // extern "C" - -#endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_MESSAGE_HANDLE_HPP_ From 7825f9cc7bda301f7eb477e4b3b3f43b01845d72 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 8 Feb 2018 09:47:16 -0800 Subject: [PATCH 08/34] comment about pre-allocation --- rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em | 1 + 1 file changed, 1 insertion(+) diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index e8306eb9..c9c0134a 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -334,6 +334,7 @@ to_cdr_stream__@(spec.base_type.type)( } fprintf(stderr, "message length: %d\n", cdr_stream->message_length); // allocate enough memory for the CDR stream + // TODO(karsten1987): This allocation has to be preallocated cdr_stream->raw_message = (char *)malloc(sizeof(char) * cdr_stream->message_length); // call the function again and fill the buffer this time if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->raw_message, &cdr_stream->message_length, dds_message) != RTI_TRUE) { From 4704116eb049b20c3026ea21885546e5cfcc77a8 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 15 Feb 2018 16:33:13 -0800 Subject: [PATCH 09/34] publish raw with connext cpp --- .../connext_static_client_info.hpp | 1 - .../connext_static_publisher_info.hpp | 1 - .../connext_static_service_info.hpp | 1 - .../connext_static_subscriber_info.hpp | 1 - rmw_connext_cpp/src/rmw_client.cpp | 8 - rmw_connext_cpp/src/rmw_publish.cpp | 8 +- rmw_connext_cpp/src/rmw_publisher.cpp | 26 +- rmw_connext_cpp/src/rmw_request.cpp | 2 +- rmw_connext_cpp/src/rmw_service.cpp | 25 +- rmw_connext_cpp/src/rmw_subscription.cpp | 27 +- rmw_connext_shared_cpp/src/qos.cpp | 18 +- rosidl_typesupport_connext_cpp/CMakeLists.txt | 12 +- rosidl_typesupport_connext_cpp/RawData.idl | 7 + .../connext_static_raw_data.h | 106 ++ .../connext_static_raw_data_plugin.h | 344 ++++ .../connext_static_raw_data_support.h | 152 ++ .../ndds_include.hpp | 32 + ...msg__rosidl_typesupport_connext_cpp.hpp.em | 2 + .../resource/msg__type_support.cpp.em | 82 +- .../__init__.py | 221 +-- .../src/RawData_publisher.cxx | 280 +++ .../src/RawData_subscriber.cxx | 270 +++ .../src/connext_static_raw_data.cxx | 313 ++++ .../src/connext_static_raw_data_plugin.cxx | 1497 +++++++++++++++++ .../src/connext_static_raw_data_support.cxx | 182 ++ 25 files changed, 3315 insertions(+), 303 deletions(-) create mode 100644 rosidl_typesupport_connext_cpp/RawData.idl create mode 100644 rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data.h create mode 100644 rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h create mode 100644 rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h create mode 100644 rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/ndds_include.hpp create mode 100644 rosidl_typesupport_connext_cpp/src/RawData_publisher.cxx create mode 100644 rosidl_typesupport_connext_cpp/src/RawData_subscriber.cxx create mode 100644 rosidl_typesupport_connext_cpp/src/connext_static_raw_data.cxx create mode 100644 rosidl_typesupport_connext_cpp/src/connext_static_raw_data_plugin.cxx create mode 100644 rosidl_typesupport_connext_cpp/src/connext_static_raw_data_support.cxx diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_client_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_client_info.hpp index e282d0c3..3fc8728d 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_client_info.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_client_info.hpp @@ -27,7 +27,6 @@ struct ConnextStaticClientInfo DDSDataReader * response_datareader_; DDSReadCondition * read_condition_; const service_type_support_callbacks_t * callbacks_; - bool raw_stream_subscriber = true; }; } // extern "C" diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp index b6f6185c..11ce127c 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_publisher_info.hpp @@ -27,7 +27,6 @@ struct ConnextStaticPublisherInfo DDSDataWriter * topic_writer_; const message_type_support_callbacks_t * callbacks_; rmw_gid_t publisher_gid; - bool raw_stream_publisher = true; }; } // extern "C" diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_service_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_service_info.hpp index 8621532f..b57e28bd 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_service_info.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_service_info.hpp @@ -27,7 +27,6 @@ struct ConnextStaticServiceInfo DDSDataReader * request_datareader_; DDSReadCondition * read_condition_; const service_type_support_callbacks_t * callbacks_; - bool raw_stream_subscriber = true; }; } // extern "C" diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp index 2b2702a6..c513f384 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_subscriber_info.hpp @@ -28,7 +28,6 @@ struct ConnextStaticSubscriberInfo DDSReadCondition * read_condition_; bool ignore_local_publications; const message_type_support_callbacks_t * callbacks_; - bool raw_stream_subscriber = true; }; } // extern "C" diff --git a/rmw_connext_cpp/src/rmw_client.cpp b/rmw_connext_cpp/src/rmw_client.cpp index f5ea5263..14d14bec 100644 --- a/rmw_connext_cpp/src/rmw_client.cpp +++ b/rmw_connext_cpp/src/rmw_client.cpp @@ -78,7 +78,6 @@ rmw_create_client( DDS_SubscriberQos subscriber_qos; DDS_ReturnCode_t status; DDS_PublisherQos publisher_qos; - DDS_TypeSupportQosPolicy ts; DDS_DataReaderQos datareader_qos; DDS_DataWriterQos datawriter_qos; DDS::Publisher * dds_publisher = nullptr; @@ -122,13 +121,6 @@ rmw_create_client( { goto fail; } - // Set the plugin typesupport to the connext info - // If this is true, a raw CDR Stream is enabled - //ts.plugin_data = &client_info->raw_stream_subscriber; - //ts.cdr_padding_kind = DDS_AUTO_CDR_PADDING; - //datareader_qos.type_support = ts; - //datawriter_qos.type_support = ts; - (void) ts; requester = callbacks->create_requester( participant, request_topic_str, response_topic_str, diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 8405b473..50044a1d 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -61,7 +61,6 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); return RMW_RET_ERROR; } - fprintf(stderr, "successfully converted to cdr stream\n"); if (cdr_stream.message_length == 0) { RMW_SET_ERROR_MSG("no message length set"); return RMW_RET_ERROR; @@ -112,11 +111,8 @@ rmw_publish_raw(const rmw_publisher_t * publisher, const rmw_message_raw_t * raw } ConnextStaticCDRStream cdr_stream; - cdr_stream.raw_message = DDS_String_dup(raw_message->buffer); - cdr_stream.message_length = static_cast(raw_message->buffer_length); - //ConnextStaticMessageHandle message_handle; - //message_handle.raw_message = raw_message->buffer; - //message_handle.raw_message_length = &raw_message->buffer_length; + cdr_stream.raw_message = raw_message->buffer; + cdr_stream.message_length = raw_message->buffer_length; bool published = callbacks->publish(topic_writer, &cdr_stream); if (!published) { RMW_SET_ERROR_MSG("failed to publish message"); diff --git a/rmw_connext_cpp/src/rmw_publisher.cpp b/rmw_connext_cpp/src/rmw_publisher.cpp index 74c6f580..a10f8fc0 100644 --- a/rmw_connext_cpp/src/rmw_publisher.cpp +++ b/rmw_connext_cpp/src/rmw_publisher.cpp @@ -84,7 +84,6 @@ rmw_create_publisher( // Past this point, a failure results in unrolling code in the goto fail block. bool registered; DDS_DataWriterQos datawriter_qos; - DDS_TypeSupportQosPolicy ts; DDS_PublisherQos publisher_qos; DDS_ReturnCode_t status; DDSPublisher * dds_publisher = nullptr; @@ -160,34 +159,27 @@ rmw_create_publisher( DDS_String_free(topic_str); topic_str = nullptr; - // Allocate memory for the ConnextStaticPublisherInfo object. - buf = rmw_allocate(sizeof(ConnextStaticPublisherInfo)); - if (!buf) { - RMW_SET_ERROR_MSG("failed to allocate memory"); - goto fail; - } - // Use a placement new to construct the ConnextStaticPublisherInfo in the preallocated buffer. - RMW_TRY_PLACEMENT_NEW(publisher_info, buf, goto fail, ConnextStaticPublisherInfo, ) - buf = nullptr; // Only free the publisher_info pointer; don't need the buf pointer anymore. - if (!get_datawriter_qos(participant, *qos_profile, datawriter_qos)) { // error string was set within the function goto fail; } - // Set the plugin typesupport to the connext info - // If this is true, a raw CDR Stream is enabled - ts.plugin_data = &publisher_info->raw_stream_publisher; - ts.cdr_padding_kind = DDS_AUTO_CDR_PADDING; - datawriter_qos.type_support = ts; topic_writer = dds_publisher->create_datawriter( topic, datawriter_qos, NULL, DDS_STATUS_MASK_NONE); if (!topic_writer) { RMW_SET_ERROR_MSG("failed to create datawriter"); goto fail; } - fprintf(stderr, "created datawriter\n"); + // Allocate memory for the ConnextStaticPublisherInfo object. + buf = rmw_allocate(sizeof(ConnextStaticPublisherInfo)); + if (!buf) { + RMW_SET_ERROR_MSG("failed to allocate memory"); + goto fail; + } + // Use a placement new to construct the ConnextStaticPublisherInfo in the preallocated buffer. + RMW_TRY_PLACEMENT_NEW(publisher_info, buf, goto fail, ConnextStaticPublisherInfo, ) + buf = nullptr; // Only free the publisher_info pointer; don't need the buf pointer anymore. publisher_info->dds_publisher_ = dds_publisher; publisher_info->topic_writer_ = topic_writer; publisher_info->callbacks_ = callbacks; diff --git a/rmw_connext_cpp/src/rmw_request.cpp b/rmw_connext_cpp/src/rmw_request.cpp index d6e0de7b..772d958e 100644 --- a/rmw_connext_cpp/src/rmw_request.cpp +++ b/rmw_connext_cpp/src/rmw_request.cpp @@ -111,7 +111,7 @@ rmw_take_request( } *taken = callbacks->take_request(replier, request_header, ros_request); - fprintf(stderr, "Correctly taken the request\n"); + return RMW_RET_OK; } } // extern "C" diff --git a/rmw_connext_cpp/src/rmw_service.cpp b/rmw_connext_cpp/src/rmw_service.cpp index 87385680..3d552997 100644 --- a/rmw_connext_cpp/src/rmw_service.cpp +++ b/rmw_connext_cpp/src/rmw_service.cpp @@ -75,7 +75,6 @@ rmw_create_service( } // Past this point, a failure results in unrolling code in the goto fail block. - DDS_TypeSupportQosPolicy ts; DDS_DataReaderQos datareader_qos; DDS_DataWriterQos datawriter_qos; DDS_SubscriberQos subscriber_qos; @@ -96,15 +95,6 @@ rmw_create_service( char * request_topic_str = nullptr; char * response_topic_str = nullptr; - buf = rmw_allocate(sizeof(ConnextStaticServiceInfo)); - if (!buf) { - RMW_SET_ERROR_MSG("failed to allocate memory"); - goto fail; - } - // Use a placement new to construct the ConnextStaticServiceInfo in the preallocated buffer. - RMW_TRY_PLACEMENT_NEW(service_info, buf, goto fail, ConnextStaticServiceInfo, ) - buf = nullptr; // Only free the service_info pointer; don't need the buf pointer anymore. - // Begin initializing elements. service = rmw_service_allocate(); if (!service) { @@ -131,13 +121,6 @@ rmw_create_service( { goto fail; } - // Set the plugin typesupport to the connext info - // If this is true, a raw CDR Stream is enabled - //ts.plugin_data = &service_info->raw_stream_subscriber; - //ts.cdr_padding_kind = DDS_AUTO_CDR_PADDING; - //datareader_qos.type_support = ts; - //datawriter_qos.type_support = ts; - (void) ts; replier = callbacks->create_replier( participant, request_topic_str, response_topic_str, @@ -191,6 +174,14 @@ rmw_create_service( // update attached publisher dds_publisher->set_qos(publisher_qos); + buf = rmw_allocate(sizeof(ConnextStaticServiceInfo)); + if (!buf) { + RMW_SET_ERROR_MSG("failed to allocate memory"); + goto fail; + } + // Use a placement new to construct the ConnextStaticServiceInfo in the preallocated buffer. + RMW_TRY_PLACEMENT_NEW(service_info, buf, goto fail, ConnextStaticServiceInfo, ) + buf = nullptr; // Only free the service_info pointer; don't need the buf pointer anymore. service_info->replier_ = replier; service_info->callbacks_ = callbacks; service_info->request_datareader_ = request_datareader; diff --git a/rmw_connext_cpp/src/rmw_subscription.cpp b/rmw_connext_cpp/src/rmw_subscription.cpp index 9da25371..affa408e 100644 --- a/rmw_connext_cpp/src/rmw_subscription.cpp +++ b/rmw_connext_cpp/src/rmw_subscription.cpp @@ -79,7 +79,6 @@ rmw_create_subscription( // Past this point, a failure results in unrolling code in the goto fail block. bool registered; DDS_DataReaderQos datareader_qos; - DDS_TypeSupportQosPolicy ts; DDS_SubscriberQos subscriber_qos; DDS_ReturnCode_t status; DDSSubscriber * dds_subscriber = nullptr; @@ -156,27 +155,11 @@ rmw_create_subscription( DDS_String_free(topic_str); topic_str = nullptr; - // Allocate memory for the ConnextStaticSubscriberInfo object. - buf = rmw_allocate(sizeof(ConnextStaticSubscriberInfo)); - if (!buf) { - RMW_SET_ERROR_MSG("failed to allocate memory"); - goto fail; - } - // Use a placement new to construct the ConnextStaticSubscriberInfo in the preallocated buffer. - RMW_TRY_PLACEMENT_NEW(subscriber_info, buf, goto fail, ConnextStaticSubscriberInfo, ) - buf = nullptr; // Only free the subscriber_info pointer; don't need the buf pointer anymore. - if (!get_datareader_qos(participant, *qos_profile, datareader_qos)) { // error string was set within the function goto fail; } - // Set the plugin typesupport to the connext info - // If this is true, a raw CDR Stream is enabled - ts.plugin_data = &subscriber_info->raw_stream_subscriber; - ts.cdr_padding_kind = DDS_AUTO_CDR_PADDING; - datareader_qos.type_support = ts; - topic_reader = dds_subscriber->create_datareader( topic, datareader_qos, NULL, DDS_STATUS_MASK_NONE); @@ -184,7 +167,6 @@ rmw_create_subscription( RMW_SET_ERROR_MSG("failed to create datareader"); goto fail; } - fprintf(stderr, "data reader created\n"); read_condition = topic_reader->create_readcondition( DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE); @@ -193,6 +175,15 @@ rmw_create_subscription( goto fail; } + // Allocate memory for the ConnextStaticSubscriberInfo object. + buf = rmw_allocate(sizeof(ConnextStaticSubscriberInfo)); + if (!buf) { + RMW_SET_ERROR_MSG("failed to allocate memory"); + goto fail; + } + // Use a placement new to construct the ConnextStaticSubscriberInfo in the preallocated buffer. + RMW_TRY_PLACEMENT_NEW(subscriber_info, buf, goto fail, ConnextStaticSubscriberInfo, ) + buf = nullptr; // Only free the subscriber_info pointer; don't need the buf pointer anymore. subscriber_info->dds_subscriber_ = dds_subscriber; subscriber_info->topic_reader_ = topic_reader; subscriber_info->read_condition_ = read_condition; diff --git a/rmw_connext_shared_cpp/src/qos.cpp b/rmw_connext_shared_cpp/src/qos.cpp index 1cd0c7b8..f855ee8a 100644 --- a/rmw_connext_shared_cpp/src/qos.cpp +++ b/rmw_connext_shared_cpp/src/qos.cpp @@ -65,15 +65,15 @@ get_datawriter_qos( return false; } -// status = DDSPropertyQosPolicyHelper::add_property( -// datawriter_qos.property, -// "dds.data_writer.history.memory_manager.fast_pool.pool_buffer_max_size", -// "4096", -// DDS_BOOLEAN_FALSE); -// if (status != DDS_RETCODE_OK) { -// RMW_SET_ERROR_MSG("failed to add qos property"); -// return false; -// } + status = DDSPropertyQosPolicyHelper::add_property( + datawriter_qos.property, + "dds.data_writer.history.memory_manager.fast_pool.pool_buffer_max_size", + "4096", + DDS_BOOLEAN_FALSE); + if (status != DDS_RETCODE_OK) { + RMW_SET_ERROR_MSG("failed to add qos property"); + return false; + } if (!set_entity_qos_from_profile(qos_profile, datawriter_qos)) { return false; diff --git a/rosidl_typesupport_connext_cpp/CMakeLists.txt b/rosidl_typesupport_connext_cpp/CMakeLists.txt index d2c420de..c25a3ad5 100644 --- a/rosidl_typesupport_connext_cpp/CMakeLists.txt +++ b/rosidl_typesupport_connext_cpp/CMakeLists.txt @@ -41,7 +41,13 @@ ament_export_include_directories(include) ament_python_install_package(${PROJECT_NAME}) -add_library(${PROJECT_NAME} SHARED src/identifier.cpp) +add_library( + ${PROJECT_NAME} + SHARED + src/connext_static_raw_data.cxx + src/connext_static_raw_data_plugin.cxx + src/connext_static_raw_data_support.cxx + src/identifier.cpp) if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSIDL_TYPESUPPORT_CONNEXT_CPP_BUILDING_DLL") @@ -50,6 +56,10 @@ target_include_directories(${PROJECT_NAME} PUBLIC include ) +ament_target_dependencies( + ${PROJECT_NAME} + "Connext" +) ament_export_libraries(${PROJECT_NAME}) ament_index_register_resource("rosidl_typesupport_cpp") diff --git a/rosidl_typesupport_connext_cpp/RawData.idl b/rosidl_typesupport_connext_cpp/RawData.idl new file mode 100644 index 00000000..da9f7131 --- /dev/null +++ b/rosidl_typesupport_connext_cpp/RawData.idl @@ -0,0 +1,7 @@ +const long KEY_HASH_LENGTH_16 = 16; + +struct RawData { + @key octet key_hash[KEY_HASH_LENGTH_16]; + sequence key_data; + sequence raw_data; +}; diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data.h b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data.h new file mode 100644 index 00000000..33b79ab7 --- /dev/null +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data.h @@ -0,0 +1,106 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef ConnextStaticRawData_1689213465_h +#define ConnextStaticRawData_1689213465_h + +#ifndef NDDS_STANDALONE_TYPE +#ifndef ndds_cpp_h +#include "ndds_include.hpp" +#endif +#else +#include "ndds_standalone_type.h" +#endif + +static const DDS_Long KEY_HASH_LENGTH_16= 16; +extern "C" { + + extern const char *ConnextStaticRawDataTYPENAME; + +} + +struct ConnextStaticRawDataSeq; +#ifndef NDDS_STANDALONE_TYPE +class ConnextStaticRawDataTypeSupport; +class ConnextStaticRawDataDataWriter; +class ConnextStaticRawDataDataReader; +#endif + +class ConnextStaticRawData +{ + public: + typedef struct ConnextStaticRawDataSeq Seq; + #ifndef NDDS_STANDALONE_TYPE + typedef ConnextStaticRawDataTypeSupport TypeSupport; + typedef ConnextStaticRawDataDataWriter DataWriter; + typedef ConnextStaticRawDataDataReader DataReader; + #endif + + DDS_Octet key_hash [(KEY_HASH_LENGTH_16)]; + DDS_OctetSeq serialized_key ; + DDS_OctetSeq serialized_data ; + +}; +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) +#endif + +NDDSUSERDllExport DDS_TypeCode* ConnextStaticRawData_get_typecode(void); /* Type code */ + +DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize( + ConnextStaticRawData* self); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize_ex( + ConnextStaticRawData* self,RTIBool allocatePointers,RTIBool allocateMemory); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize_w_params( + ConnextStaticRawData* self, + const struct DDS_TypeAllocationParams_t * allocParams); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize( + ConnextStaticRawData* self); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_ex( + ConnextStaticRawData* self,RTIBool deletePointers); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_w_params( + ConnextStaticRawData* self, + const struct DDS_TypeDeallocationParams_t * deallocParams); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_optional_members( + ConnextStaticRawData* self, RTIBool deletePointers); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_copy( + ConnextStaticRawData* dst, + const ConnextStaticRawData* src); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* ConnextStaticRawData */ + diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h new file mode 100644 index 00000000..ae1556f1 --- /dev/null +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h @@ -0,0 +1,344 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef ConnextStaticRawDataPlugin_1689213465_h +#define ConnextStaticRawDataPlugin_1689213465_h + +#include "connext_static_raw_data.h" + +struct RTICdrStream; + +#ifndef pres_typePlugin_h +#include "pres/pres_typePlugin.h" +#endif + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) +#endif + +extern "C" { + + /* The type used to store keys for instances of type struct + * AnotherSimple. + * + * By default, this type is struct ConnextStaticRawData + * itself. However, if for some reason this choice is not practical for your + * system (e.g. if sizeof(struct ConnextStaticRawData) + * is very large), you may redefine this typedef in terms of another type of + * your choosing. HOWEVER, if you define the KeyHolder type to be something + * other than struct AnotherSimple, the + * following restriction applies: the key of struct + * ConnextStaticRawData must consist of a + * single field of your redefined KeyHolder type and that field must be the + * first field in struct ConnextStaticRawData. + */ + typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; + + #define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample + #define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer + #define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer + + #define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey + #define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey + + #define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample + #define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample + + /* -------------------------------------------------------------------------------------- + Support functions: + * -------------------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data(void); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData *out, + const ConnextStaticRawData *in); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData *sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData *sample,RTIBool deallocate_pointers); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData *sample); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData *sample, + const char *desc, + unsigned int indent); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_key(void); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder *key); + + /* ---------------------------------------------------------------------------- + Callback functions: + * ---------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern PRESTypePluginParticipantData + ConnextStaticRawDataPlugin_on_participant_attached( + void *registration_data, + const struct PRESTypePluginParticipantInfo *participant_info, + RTIBool top_level_registration, + void *container_plugin_context, + RTICdrTypeCode *typeCode); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data); + + NDDSUSERDllExport extern PRESTypePluginEndpointData + ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo *endpoint_info, + RTIBool top_level_registration, + void *container_plugin_context); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + void *handle); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *out, + const ConnextStaticRawData *in); + + /* ---------------------------------------------------------------------------- + (De)Serialize functions: + * ------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData *sample); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData *sample, + const char * buffer, + unsigned int length); + NDDSUSERDllExport extern DDS_ReturnCode_t + ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample); + + /* -------------------------------------------------------------------------------------- + Key Management functions: + * -------------------------------------------------------------------------------------- */ + NDDSUSERDllExport extern PRESTypePluginKeyKind + ConnextStaticRawDataPlugin_get_key_kind(void); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData ** sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder *key, + const ConnextStaticRawData *instance); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *instance, + const ConnextStaticRawDataKeyHolder *key); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t *keyhash, + const ConnextStaticRawData *instance); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + DDS_KeyHash_t *keyhash, + RTIBool deserialize_encapsulation, + void *endpoint_plugin_qos); + + /* Plugin Functions */ + NDDSUSERDllExport extern struct PRESTypePlugin* + ConnextStaticRawDataPlugin_new(void); + + NDDSUSERDllExport + extern struct PRESTypePlugin * + ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); + +} + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* ConnextStaticRawDataPlugin_1689213465_h */ + diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h new file mode 100644 index 00000000..9e60bed1 --- /dev/null +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h @@ -0,0 +1,152 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef ConnextStaticRawDataSupport_1689213465_h +#define ConnextStaticRawDataSupport_1689213465_h + +/* Uses */ +#include "connext_static_raw_data.h" + +#ifndef ndds_cpp_h +#include "ndds_include.hpp" +#endif + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) + +class __declspec(dllimport) DDSTypeSupport; +class __declspec(dllimport) DDSDataWriter; +class __declspec(dllimport) DDSDataReader; + +#endif + +/* ========================================================================= */ +/** +Uses: T + +Defines: TTypeSupport, TDataWriter, TDataReader + +Organized using the well-documented "Generics Pattern" for +implementing generics in C and C++. +*/ + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) + +#endif + +// DDS_TYPESUPPORT_CPP( +// ConnextStaticRawDataTypeSupport, +// ConnextStaticRawData); + +class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport +{ +public: + ConnextStaticRawDataTypeSupport(bool osrf) + { + (void) osrf; + }; + + ~ConnextStaticRawDataTypeSupport(); + + static DDS_ReturnCode_t register_type( + DDSDomainParticipant* participant, + const char* type_name = "ConnextStaticRawData"); + + static DDS_ReturnCode_t unregister_type( + DDSDomainParticipant* participant, + const char* type_name = "ConnextStaticRawData"); + + static const char* get_type_name(); + + static ConnextStaticRawData* create_data_ex(DDS_Boolean allocatePointers); + + static ConnextStaticRawData* create_data( + const DDS_TypeAllocationParams_t & alloc_params = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + + static DDS_ReturnCode_t delete_data_ex(ConnextStaticRawData* a_data, + DDS_Boolean deletePointers); + + static DDS_ReturnCode_t delete_data( + ConnextStaticRawData* a_data, + const DDS_TypeDeallocationParams_t & dealloc_params = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + + static void print_data(const ConnextStaticRawData* a_data); + + static DDS_ReturnCode_t copy_data(ConnextStaticRawData* dst_data, const ConnextStaticRawData* src_data); + + static DDS_ReturnCode_t initialize_data_ex(ConnextStaticRawData* a_data, + DDS_Boolean allocatePointers); + + static DDS_ReturnCode_t initialize_data( + ConnextStaticRawData* a_data, + const DDS_TypeAllocationParams_t & alloc_params = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + + static DDS_ReturnCode_t finalize_data_ex(ConnextStaticRawData* a_data, + DDS_Boolean deletePointers); + static DDS_ReturnCode_t finalize_data( + ConnextStaticRawData* a_data, + const DDS_TypeDeallocationParams_t & dealloc_params = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + + DDSDataReader* create_datareaderI(DDSDataReader* dataReader); + DDS_ReturnCode_t destroy_datareaderI(DDSDataReader* dataReader); + DDSDataWriter* create_datawriterI(DDSDataWriter* dataWriter); + DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter* dataWriter); + + static DDS_TypeCode* get_typecode(); + + static DDS_ReturnCode_t serialize_data_to_cdr_buffer( + char * buffer, + unsigned int & length, + const ConnextStaticRawData *a_data); + + static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( + ConnextStaticRawData *a_data, + const char * buffer, + unsigned int length); + + static DDS_ReturnCode_t data_to_string( + ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong& str_size, + const DDS_PrintFormatProperty& property); + + static void finalize(); + +private: + ConnextStaticRawDataTypeSupport(); +}; + + +DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); +DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); + +NDDSUSERDllExport +DDS_ReturnCode_t +ConnextStaticRawDataSupport_register_external_type( + DDSDomainParticipant * participant, + const char * type_name, + struct DDS_TypeCode * type_code); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* ConnextStaticRawDataSupport_1689213465_h */ + diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/ndds_include.hpp b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/ndds_include.hpp new file mode 100644 index 00000000..a02d0e93 --- /dev/null +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/ndds_include.hpp @@ -0,0 +1,32 @@ +// Copyright 2015-2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_HPP_ +#define ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_HPP_ + +#ifndef _WIN32 +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-parameter" +# ifdef __clang__ +# pragma clang diagnostic ignored "-Wdeprecated-register" +# pragma clang diagnostic ignored "-Wreturn-type-c-linkage" +# endif +#endif +#include "ndds/ndds_cpp.h" +#include "ndds/ndds_requestreply_cpp.h" +#ifndef _WIN32 +# pragma GCC diagnostic pop +#endif + +#endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_HPP_ diff --git a/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em b/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em index cd927fb3..935b31ad 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em @@ -40,6 +40,8 @@ header_guard_variable = '__'.join([x.upper() for x in header_guard_parts]) + '_' #endif #include "@(spec.base_type.pkg_name)/@(subfolder)/dds_connext/@(spec.base_type.type)_Support.h" #include "@(spec.base_type.pkg_name)/@(subfolder)/dds_connext/@(spec.base_type.type)_Plugin.h" + +#include "ndds/ndds_cpp.h" #ifndef _WIN32 # pragma GCC diagnostic pop #endif diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index c9c0134a..be784486 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -21,6 +21,7 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" +#include "rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h" #include "rosidl_typesupport_connext_cpp/identifier.hpp" #include "rosidl_typesupport_connext_cpp/message_type_support.h" #include "rosidl_typesupport_connext_cpp/message_type_support_decl.hpp" @@ -66,9 +67,21 @@ register_type__@(spec.base_type.type)( const char * type_name) { DDSDomainParticipant * participant = static_cast(untyped_participant); - DDS_ReturnCode_t status = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::register_type(participant, type_name); - + DDS_TypeCode * type_code = + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::get_typecode(); + // This is a non-standard RTI Connext function + // It allows to register an external type to a static data writer + // In this case, we register the custom message type to a data writer, + // which only publishes DDS_Octets + // The purpose of this is to send only raw data DDS_Octets over the wire, + // advertise the topic however with a type of the message, e.g. std_msgs::msg::dds_::String + DDS_ReturnCode_t status = ConnextStaticRawDataSupport_register_external_type( + participant, + type_name, + type_code); + if (status != DDS_RETCODE_OK) { + fprintf(stderr, "Failed to register external type\n"); + } return status == DDS_RETCODE_OK; } @@ -152,20 +165,46 @@ publish__@(spec.base_type.type)( void * untyped_topic_writer, ConnextStaticCDRStream * cdr_stream) { - bool success = true; DDSDataWriter * topic_writer = static_cast(untyped_topic_writer); - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter * data_writer = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataWriter::narrow(topic_writer); + ConnextStaticRawDataDataWriter * data_writer = + ConnextStaticRawDataDataWriter::narrow(topic_writer); if (!data_writer) { fprintf(stderr, "failed to narrow data writer\n"); - success = false; - } else { - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * sample = reinterpret_cast<@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ *>(cdr_stream); - DDS_ReturnCode_t status = data_writer->write(*sample, DDS_HANDLE_NIL); - success = status == DDS_RETCODE_OK; + return false; } - return success; + ConnextStaticRawData * instance = ConnextStaticRawDataTypeSupport::create_data(); + if (!instance) { + fprintf(stderr, "failed to create dds message instance\n"); + return false; + } + + DDS_ReturnCode_t status = DDS_RETCODE_ERROR; + + instance->serialized_data.maximum(0); + if (!instance->serialized_data.loan_contiguous( + reinterpret_cast(cdr_stream->raw_message), + cdr_stream->message_length, cdr_stream->message_length)) { + fprintf(stderr, "failed to loan memory for message\n"); + return false; + } + + fprintf(stderr, "raw data size: %d\n", instance->serialized_data.length()); + for (DDS_Long i = 0; i < instance->serialized_data.length(); ++i) { + fprintf(stderr, "%02x ", instance->serialized_data[i]); + } + fprintf(stderr, "\n"); + + status = data_writer->write(*instance, DDS_HANDLE_NIL); + + if (!instance->serialized_data.unloan()) { + fprintf(stderr, "failed to return loaned memory\n"); + return false; + } + status = ConnextStaticRawDataTypeSupport::delete_data(instance); + + return status == DDS_RETCODE_OK; } bool @@ -239,14 +278,14 @@ take__@(spec.base_type.type)( DDSDataReader * topic_reader = static_cast(untyped_topic_reader); - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader * data_reader = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader::narrow(topic_reader); + ConnextStaticRawDataDataReader * data_reader = + ConnextStaticRawDataDataReader::narrow(topic_reader); if (!data_reader) { fprintf(stderr, "failed to narrow data reader\n"); return false; } - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_Seq dds_messages; + ConnextStaticRawDataSeq dds_messages; DDS_SampleInfoSeq sample_infos; DDS_ReturnCode_t status = data_reader->take( dds_messages, @@ -290,10 +329,11 @@ take__@(spec.base_type.type)( } if (!ignore_sample) { - ConnextStaticCDRStream * input_stream = reinterpret_cast(&dds_messages[0]); - cdr_stream->raw_message = input_stream->raw_message; - cdr_stream->message_length = input_stream->message_length; - fprintf(stderr, "Received stream of length %u\n", cdr_stream->message_length); + cdr_stream->message_length = dds_messages[0].serialized_data.length(); + cdr_stream->raw_message = (char *) malloc(cdr_stream->message_length * sizeof(char)); + for (unsigned int i = 0; i < cdr_stream->message_length; ++i) { + cdr_stream->raw_message[i] = dds_messages[0].serialized_data[i]; + } *taken = true; } else { *taken = false; @@ -318,11 +358,13 @@ to_cdr_stream__@(spec.base_type.type)( // cast the untyped to the known ros message const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message = *(const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *)untyped_ros_message; + // create a respective connext dds type @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); if (!dds_message) { return false; } + // convert ros to dds if (!convert_ros_message_to_dds(ros_message, *dds_message)) { return false; @@ -332,9 +374,9 @@ to_cdr_stream__@(spec.base_type.type)( if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(NULL, &cdr_stream->message_length, dds_message) != RTI_TRUE) { return false; } - fprintf(stderr, "message length: %d\n", cdr_stream->message_length); // allocate enough memory for the CDR stream // TODO(karsten1987): This allocation has to be preallocated + // or at least bring in a custom allocator cdr_stream->raw_message = (char *)malloc(sizeof(char) * cdr_stream->message_length); // call the function again and fill the buffer this time if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->raw_message, &cdr_stream->message_length, dds_message) != RTI_TRUE) { @@ -360,7 +402,7 @@ to_message__@(spec.base_type.type)( if (!untyped_ros_message) { return false; } - + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer(dds_message, cdr_stream->raw_message, cdr_stream->message_length) != RTI_TRUE) { fprintf(stderr, "deserialize from cdr buffer failed\n"); diff --git a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py index 5cf76496..d8e65180 100644 --- a/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py +++ b/rosidl_typesupport_connext_cpp/rosidl_typesupport_connext_cpp/__init__.py @@ -86,218 +86,35 @@ def generate_dds_connext_cpp( count = 1 max_count = 5 while True: - subprocess.check_call(cmd) - - # fail safe if the generator does not work as expected - any_missing = False - for suffix in ['.h', '.cxx', 'Plugin.h', 'Plugin.cxx', 'Support.h', 'Support.cxx']: - filename = os.path.join(output_path, msg_name + suffix) - if not os.path.exists(filename): - any_missing = True - break - if not any_missing: - break - print("'%s' failed to generate the expected files for '%s/%s'" % - (idl_pp, pkg_name, msg_name), file=sys.stderr) - if count < max_count: - count += 1 - print('Running code generator again (retry %d of %d)...' % - (count, max_count), file=sys.stderr) - continue - raise RuntimeError('failed to generate the expected files') + subprocess.check_call(cmd) + + # fail safe if the generator does not work as expected + any_missing = False + for suffix in ['.h', '.cxx', 'Plugin.h', 'Plugin.cxx', 'Support.h', 'Support.cxx']: + filename = os.path.join(output_path, msg_name + suffix) + if not os.path.exists(filename): + any_missing = True + break + if not any_missing: + break + print("'%s' failed to generate the expected files for '%s/%s'" % + (idl_pp, pkg_name, msg_name), file=sys.stderr) + if count < max_count: + count += 1 + print('Running code generator again (retry %d of %d)...' % + (count, max_count), file=sys.stderr) + continue + raise RuntimeError('failed to generate the expected files') if os.name != 'nt': # modify generated code to avoid unsed global variable warning # which can't be suppressed non-globally with gcc msg_filename = os.path.join(output_path, msg_name + '.h') - # TODO(karsten1987): Modify should take array of callbacks - # to avoid multiple file readings _modify(msg_filename, pkg_name, msg_name, _inject_unused_attribute) - plugin_filename = os.path.join(output_path, msg_name + 'Plugin.cxx') - if not "Request" in msg_name and not "Response" in msg_name: - _modify(plugin_filename, pkg_name, msg_name, _modify_include_headers) - _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_create_data_function) - _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_destroy_data_function) - _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_serialize_function) - _modify(plugin_filename, pkg_name, msg_name, _modify_plugin_deserialize_function) - return 0 -def _get_create_data_code(msg_name, indentation): - val = (\ - "{{\n" - "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" - "{indentation}ConnextStaticCDRStream * cdr_stream = NULL;\n" - "{indentation}RTIOsapiHeap_allocateStructure(&cdr_stream, ConnextStaticCDRStream);\n" - "{indentation}return reinterpret_cast<{msg_name} *>(cdr_stream);\n" - "\n".format(msg_name=msg_name, indentation=indentation)) - return val - - -def _modify_plugin_create_data_function(pkg_name, msg_name, lines): - create_data_fcn_signature = msg_name + 'PluginSupport_create_data_ex(' - print("looking for '%s' create_data function" % create_data_fcn_signature) - signature_found = False - injection_start = None - for index, line in enumerate(lines): - if not signature_found: - if line.lstrip().startswith(create_data_fcn_signature): - signature_found = True - else: - if '{' in line.lstrip(): - print("fpund %s create ex function in line %d" % (msg_name, index)) - injection_start = index - break - if not signature_found: - raise RuntimeError('failed to locate %sPlugin_create_data function' % msg_name) - - indentation = ' ' * 16 - lines[injection_start] = line.replace('{', _get_create_data_code(msg_name, indentation)) - return True - - -def _get_destroy_data_code(msg_name, indentation): - val = (\ - "{{\n" - "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" - "{indentation}RTIOsapiHeap_freeStructure(sample);\n" - "{indentation}return;\n".format(indentation=indentation)) - return val - - -def _modify_plugin_destroy_data_function(pkg_name, msg_name, lines): - destroy_fcn_signature = msg_name + 'PluginSupport_destroy_data_ex(' - print("looking for '%s' destroy function" % destroy_fcn_signature) - signature_found = False - injection_start = None - for index, line in enumerate(lines): - if not signature_found: - if line.lstrip().startswith(destroy_fcn_signature): - signature_found = True - else: - if '{' in line.lstrip(): - print("found %s destroy function in line: %d" % (msg_name, index)) - injection_start = index - break - if not signature_found: - raise RuntimeError('failed to locate %sPlugin_destroy function' % msg_name) - - indentation = ' ' * 16 - lines[injection_start] = line.replace('{', _get_destroy_data_code(msg_name, indentation)) - return True - - -def _get_deserialization_code(msg_name, indentation): - val = (\ - "{{\n" - "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" - "{indentation}if (endpoint_plugin_qos) {{\n" - "{indentation} if (!reinterpret_cast(endpoint_plugin_qos)) {{\n" - "{indentation} return RTI_FALSE;\n" - "{indentation} }}\n" - "{indentation} ConnextStaticCDRStream * cdr_stream =\n" - "{indentation} reinterpret_cast(sample);\n" - "{indentation} cdr_stream->raw_message = stream->_buffer;\n" - "{indentation} cdr_stream->message_length = stream->_bufferLength;\n" - "{indentation} return RTI_TRUE;\n" - "{indentation}}}\n" - .format(indentation=indentation)) - return val - - -def _modify_plugin_deserialize_function(pkg_name, msg_name, lines): - deserialize_fcn_signature = msg_name + 'Plugin_deserialize_sample(' - print("looking for '%s' deserialize function" % deserialize_fcn_signature) - signature_found = False - injection_start = None - for index, line in enumerate(lines): - if not signature_found: - if line.lstrip().startswith(deserialize_fcn_signature): - signature_found = True - else: - if '{' in line.lstrip(): - print("found %s deserialize function in line: %d" % (msg_name, index)) - injection_start = index - break - if not signature_found: - raise RuntimeError('failed to locate %sPlugin_deserialize function' % msg_name) - - indentation = ' ' * 16 - lines[injection_start] = line.replace('{', _get_deserialization_code(msg_name, indentation)) - return True - - -def _get_serialization_code(msg_name, indentation): - val = (\ - "{{\n" - "{indentation}// MODIFIED FOR ROS2 PURPOSES\n" - "{indentation}if (endpoint_plugin_qos) {{\n" - "{indentation} if (!reinterpret_cast(endpoint_plugin_qos)) {{\n" - "{indentation} return RTI_FALSE;\n" - "{indentation} }}\n" - "{indentation} const ConnextStaticCDRStream * cdr_stream =\n" - "{indentation} reinterpret_cast(sample);\n" - "{indentation} memcpy(stream->_buffer, cdr_stream->raw_message, " - "cdr_stream->message_length);\n" - "{indentation} stream->_relativeBuffer = stream->_buffer;\n" - "{indentation} stream->_tmpRelativeBuffer = stream->_buffer;\n" - "{indentation} stream->_buffer = stream->_buffer;\n" - "{indentation} //stream->_endian = \'\\x01\';\n" - "{indentation} //stream->_nativeEndian = \'\\x01\';\n" - "{indentation} //stream->_encapsulationKind = 1;\n" - "{indentation} //stream->_zeroOnAlign = 0;\n" - "{indentation} stream->_currentPosition = " - "stream->_buffer + cdr_stream->message_length;\n" - "{indentation} return RTI_TRUE;\n" - "{indentation}}}\n" - .format(indentation=indentation, msg_name=msg_name)) - return val - - -def _modify_plugin_serialize_function(pkg_name, msg_name, lines): - serialize_fcn_signature = msg_name + 'Plugin_serialize(' - print("looking for '%s' serialize function" % serialize_fcn_signature) - signature_found = False - injection_start = None - for index, line in enumerate(lines): - if not signature_found: - if line.lstrip().startswith(serialize_fcn_signature): - signature_found = True - else: - if '{' in line.lstrip(): - print("found %s serialize function in line: %d" % (msg_name, index)) - injection_start = index - break - if not signature_found: - raise RuntimeError('failed to locate %sPlugin_serialize function' % msg_name) - - indentation = ' ' * 16 - lines[injection_start] = line.replace('{', _get_serialization_code(msg_name, indentation)) - return True - - -def _get_include_headers(): - val = (\ - "\n// MODIFIED FOR ROS2 PURPOSES\n#include \"" - "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp\"\n") - return val - - -def _modify_include_headers(pkg_name, msg_name, lines): - last_include_found = False - include_line = '#include \"{msg_name}Plugin.h\"'.format(msg_name=msg_name) - for index, line in enumerate(lines): - if include_line in line: - lines[index] = lines[index] + _get_include_headers() - last_include_found = True - break - if not last_include_found: - raise RuntimeError('failed to find last include line', include_line) - return True - - def _inject_unused_attribute(pkg_name, msg_name, lines): # prepend attribute before constants of string type prefix = 'static const DDS_Char * Constants__' diff --git a/rosidl_typesupport_connext_cpp/src/RawData_publisher.cxx b/rosidl_typesupport_connext_cpp/src/RawData_publisher.cxx new file mode 100644 index 00000000..31dd0f7b --- /dev/null +++ b/rosidl_typesupport_connext_cpp/src/RawData_publisher.cxx @@ -0,0 +1,280 @@ + +/* RawData_publisher.cxx + +A publication of data of type RawData + +This file is derived from code automatically generated by the rtiddsgen +command: + +rtiddsgen -language C++ -example RawData.idl + +Example publication of type RawData automatically generated by +'rtiddsgen'. To test them follow these steps: + +(1) Compile this file and the example subscription. + +(2) Start the subscription with the command +objs//RawData_subscriber + +(3) Start the publication with the command +objs//RawData_publisher + +(4) [Optional] Specify the list of discovery initial peers and +multicast receive addresses via an environment variable or a file +(in the current working directory) called NDDS_DISCOVERY_PEERS. + +You can run any number of publishers and subscribers programs, and can +add and remove them dynamically from the domain. + +Example: + +To run the example application on domain : + +On Unix: + +objs//RawData_publisher o +objs//RawData_subscriber + +On Windows: + +objs\\RawData_publisher +objs\\RawData_subscriber + +*/ + +#include +#include + +#include "RawData.h" +#include "RawDataSupport.h" +#include "ndds/ndds_cpp.h" + +#include "DynamicArrayPrimitives_.h" +#include "DynamicArrayPrimitives_Plugin.h" +#include "DynamicArrayPrimitives_Support.h" + +/* Delete all entities */ +static int publisher_shutdown( + DDSDomainParticipant *participant) +{ + DDS_ReturnCode_t retcode; + int status = 0; + + if (participant != NULL) { + retcode = participant->delete_contained_entities(); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "delete_contained_entities error %d\n", retcode); + status = -1; + } + + retcode = DDSTheParticipantFactory->delete_participant(participant); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "delete_participant error %d\n", retcode); + status = -1; + } + } + + /* RTI Connext provides finalize_instance() method on + domain participant factory for people who want to release memory used + by the participant factory. Uncomment the following block of code for + clean destruction of the singleton. */ + /* + + retcode = DDSDomainParticipantFactory::finalize_instance(); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "finalize_instance error %d\n", retcode); + status = -1; + } + */ + + return status; +} + +extern "C" int publisher_main(int domainId, int sample_count) +{ + DDSDomainParticipant *participant = NULL; + DDSPublisher *publisher = NULL; + DDSTopic *topic = NULL; + DDSDataWriter *writer = NULL; + RawDataDataWriter * RawData_writer = NULL; + RawData *instance = NULL; + DDS_ReturnCode_t retcode; + DDS_InstanceHandle_t instance_handle = DDS_HANDLE_NIL; + const char *type_name = NULL; + struct DDS_TypeCode *type_code = NULL; + int count = 0; + DDS_Duration_t send_period = {4,0}; + + /* To customize participant QoS, use + the configuration file USER_QOS_PROFILES.xml */ + participant = DDSTheParticipantFactory->create_participant( + domainId, DDS_PARTICIPANT_QOS_DEFAULT, + NULL /* listener */, DDS_STATUS_MASK_NONE); + if (participant == NULL) { + fprintf(stderr, "create_participant error\n"); + publisher_shutdown(participant); + return -1; + } + + /* To customize publisher QoS, use + the configuration file USER_QOS_PROFILES.xml */ + publisher = participant->create_publisher( + DDS_PUBLISHER_QOS_DEFAULT, NULL /* listener */, DDS_STATUS_MASK_NONE); + if (publisher == NULL) { + fprintf(stderr, "create_publisher error\n"); + publisher_shutdown(participant); + return -1; + } + + /* Register type before creating topic */ + type_name = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::get_type_name(); + type_code = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::get_typecode(); + retcode = RawDataSupport_register_external_type( + participant, type_name, type_code); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "register_type error %d\n", retcode); + publisher_shutdown(participant); + return -1; + } + + fprintf(stderr, "%s\n", type_name); + + /* To customize topic QoS, use + the configuration file USER_QOS_PROFILES.xml */ + topic = participant->create_topic( + "my_generic_type", + type_name, + DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, + DDS_STATUS_MASK_NONE); + if (topic == NULL) { + fprintf(stderr, "create_topic error\n"); + publisher_shutdown(participant); + return -1; + } + + /* To customize data writer QoS, use + the configuration file USER_QOS_PROFILES.xml */ + writer = publisher->create_datawriter( + topic, DDS_DATAWRITER_QOS_DEFAULT, NULL /* listener */, + DDS_STATUS_MASK_NONE); + if (writer == NULL) { + fprintf(stderr, "create_datawriter error\n"); + publisher_shutdown(participant); + return -1; + } + RawData_writer = RawDataDataWriter::narrow(writer); + if (RawData_writer == NULL) { + fprintf(stderr, "DataWriter narrow error\n"); + publisher_shutdown(participant); + return -1; + } + + /* Create data sample for writing */ + instance = RawDataTypeSupport::create_data(); + instance->serialized_data.maximum(0); + if (instance == NULL) { + fprintf(stderr, "RawDataTypeSupport::create_data error\n"); + publisher_shutdown(participant); + return -1; + } + + /* For a data type that has a key, if the same instance is going to be + written multiple times, initialize the key here + and register the keyed instance prior to writing */ + instance_handle = RawData_writer->register_instance(*instance); + + + test_msgs_msg_dds__DynamicArrayPrimitives_ * dynamic_array_instance = + test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::create_data(); + size_t length = 4; + dynamic_array_instance->int32_values_.ensure_length(length, length); + for (size_t i = 0; i < length; ++i) { + dynamic_array_instance->int32_values_[i] = i; + } + strcpy(dynamic_array_instance->my_key, "abc"); + + unsigned int serialization_buffer_size = test_msgs_msg_dds__DynamicArrayPrimitives_Plugin_get_serialized_sample_max_size(NULL, RTI_TRUE, 0, 0); + fprintf(stderr, "serialization buffer size %u\n", serialization_buffer_size); + if (serialization_buffer_size == RTI_CDR_MAX_SERIALIZED_SIZE) { + fprintf(stderr, "max size ohje ohje\n"); + serialization_buffer_size = 1024; + } + DDS_Octet * serialization_buffer = (DDS_Octet *)malloc(serialization_buffer_size); + for (unsigned int i = 0; i < serialization_buffer_size; ++i) { + serialization_buffer[i] = 0; + } + + if (!test_msgs_msg_dds__DynamicArrayPrimitives_Plugin_serialize_to_cdr_buffer( + (char *)serialization_buffer, &serialization_buffer_size, dynamic_array_instance)) { + fprintf(stderr, "Serialization of DyanmicArrayType failed\n"); + return false; + } + + for (unsigned int i = 0; i < serialization_buffer_size; ++i) { + fprintf(stderr, "%02x ", serialization_buffer[i]); + } + fprintf(stderr, "\n"); + instance->serialized_data.loan_contiguous( + serialization_buffer, serialization_buffer_size, serialization_buffer_size); + + for (unsigned int i = 0; i < serialization_buffer_size; ++i) { + fprintf(stderr, "%02x ", instance->serialized_data[i]); + } + fprintf(stderr, "\n"); + + /* Main loop */ + for (count=0; (sample_count == 0) || (count < sample_count); ++count) { + + printf("Writing RawData, count %d\n", count); + + /* Modify the data to be sent here */ + + retcode = RawData_writer->write(*instance, instance_handle); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "write error %d\n", retcode); + } + + NDDSUtility::sleep(send_period); + } + + instance->serialized_data.unloan(); + free(serialization_buffer); + /* + retcode = RawData_writer->unregister_instance( + *instance, instance_handle); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "unregister instance error %d\n", retcode); + } + */ + + /* Delete data sample */ + retcode = RawDataTypeSupport::delete_data(instance); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "RawDataTypeSupport::delete_data error %d\n", retcode); + } + + /* Delete all entities */ + return publisher_shutdown(participant); +} + +int main(int argc, char *argv[]) +{ + int domainId = 0; + int sample_count = 0; /* infinite loop */ + + if (argc >= 2) { + domainId = atoi(argv[1]); + } + if (argc >= 3) { + sample_count = atoi(argv[2]); + } + + /* Uncomment this to turn on additional logging + NDDSConfigLogger::get_instance()-> + set_verbosity_by_category(NDDS_CONFIG_LOG_CATEGORY_API, + NDDS_CONFIG_LOG_VERBOSITY_STATUS_ALL); + */ + + return publisher_main(domainId, sample_count); +} + diff --git a/rosidl_typesupport_connext_cpp/src/RawData_subscriber.cxx b/rosidl_typesupport_connext_cpp/src/RawData_subscriber.cxx new file mode 100644 index 00000000..429ce0b9 --- /dev/null +++ b/rosidl_typesupport_connext_cpp/src/RawData_subscriber.cxx @@ -0,0 +1,270 @@ + +/* RawData_subscriber.cxx + +A subscription example + +This file is derived from code automatically generated by the rtiddsgen +command: + +rtiddsgen -language C++ -example RawData.idl + +Example subscription of type RawData automatically generated by +'rtiddsgen'. To test them follow these steps: + +(1) Compile this file and the example publication. + +(2) Start the subscription with the command +objs//RawData_subscriber + +(3) Start the publication with the command +objs//RawData_publisher + +(4) [Optional] Specify the list of discovery initial peers and +multicast receive addresses via an environment variable or a file +(in the current working directory) called NDDS_DISCOVERY_PEERS. + +You can run any number of publishers and subscribers programs, and can +add and remove them dynamically from the domain. + +Example: + +To run the example application on domain : + +On Unix: + +objs//RawData_publisher +objs//RawData_subscriber + +On Windows: + +objs\\RawData_publisher +objs\\RawData_subscriber + +*/ + +#include +#include + +#include "RawData.h" +#include "RawDataSupport.h" +#include "ndds/ndds_cpp.h" + +#include "DynamicArrayPrimitives_.h" +#include "DynamicArrayPrimitives_Plugin.h" +#include "DynamicArrayPrimitives_Support.h" + +class RawDataListener : public DDSDataReaderListener { + public: + virtual void on_requested_deadline_missed( + DDSDataReader* /*reader*/, + const DDS_RequestedDeadlineMissedStatus& /*status*/) {} + + virtual void on_requested_incompatible_qos( + DDSDataReader* /*reader*/, + const DDS_RequestedIncompatibleQosStatus& /*status*/) {} + + virtual void on_sample_rejected( + DDSDataReader* /*reader*/, + const DDS_SampleRejectedStatus& /*status*/) {} + + virtual void on_liveliness_changed( + DDSDataReader* /*reader*/, + const DDS_LivelinessChangedStatus& /*status*/) {} + + virtual void on_sample_lost( + DDSDataReader* /*reader*/, + const DDS_SampleLostStatus& /*status*/) {} + + virtual void on_subscription_matched( + DDSDataReader* /*reader*/, + const DDS_SubscriptionMatchedStatus& /*status*/) {} + + virtual void on_data_available(DDSDataReader* reader); +}; + +void RawDataListener::on_data_available(DDSDataReader* reader) +{ + RawDataDataReader *RawData_reader = NULL; + RawDataSeq data_seq; + DDS_SampleInfoSeq info_seq; + DDS_ReturnCode_t retcode; + int i; + + RawData_reader = RawDataDataReader::narrow(reader); + if (RawData_reader == NULL) { + fprintf(stderr, "DataReader narrow error\n"); + return; + } + + retcode = RawData_reader->take( + data_seq, info_seq, DDS_LENGTH_UNLIMITED, + DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE); + + if (retcode == DDS_RETCODE_NO_DATA) { + return; + } else if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "take error %d\n", retcode); + return; + } + + for (i = 0; i < data_seq.length(); ++i) { + if (info_seq[i].valid_data) { + for (size_t j = 0; j < data_seq[i].serialized_data.length(); ++j) { + printf("%02x ", data_seq[i].serialized_data[j]); + //RawDataTypeSupport::print_data(&data_seq[i]); + } + } + } + + retcode = RawData_reader->return_loan(data_seq, info_seq); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "return loan error %d\n", retcode); + } +} + +/* Delete all entities */ +static int subscriber_shutdown( + DDSDomainParticipant *participant) +{ + DDS_ReturnCode_t retcode; + int status = 0; + + if (participant != NULL) { + retcode = participant->delete_contained_entities(); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "delete_contained_entities error %d\n", retcode); + status = -1; + } + + retcode = DDSTheParticipantFactory->delete_participant(participant); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "delete_participant error %d\n", retcode); + status = -1; + } + } + + /* RTI Connext provides the finalize_instance() method on + domain participant factory for people who want to release memory used + by the participant factory. Uncomment the following block of code for + clean destruction of the singleton. */ + /* + + retcode = DDSDomainParticipantFactory::finalize_instance(); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "finalize_instance error %d\n", retcode); + status = -1; + } + */ + return status; +} + +extern "C" int subscriber_main(int domainId, int sample_count) +{ + DDSDomainParticipant *participant = NULL; + DDSSubscriber *subscriber = NULL; + DDSTopic *topic = NULL; + RawDataListener *reader_listener = NULL; + DDSDataReader *reader = NULL; + DDS_ReturnCode_t retcode; + const char *type_name = NULL; + DDS_TypeCode *type_code = NULL; + int count = 0; + DDS_Duration_t receive_period = {4,0}; + int status = 0; + + /* To customize the participant QoS, use + the configuration file USER_QOS_PROFILES.xml */ + participant = DDSTheParticipantFactory->create_participant( + domainId, DDS_PARTICIPANT_QOS_DEFAULT, + NULL /* listener */, DDS_STATUS_MASK_NONE); + if (participant == NULL) { + fprintf(stderr, "create_participant error\n"); + subscriber_shutdown(participant); + return -1; + } + + /* To customize the subscriber QoS, use + the configuration file USER_QOS_PROFILES.xml */ + subscriber = participant->create_subscriber( + DDS_SUBSCRIBER_QOS_DEFAULT, NULL /* listener */, DDS_STATUS_MASK_NONE); + if (subscriber == NULL) { + fprintf(stderr, "create_subscriber error\n"); + subscriber_shutdown(participant); + return -1; + } + + /* Register the type before creating the topic */ + type_name = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::get_type_name(); + type_code = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::get_typecode(); + retcode = RawDataSupport_register_external_type( + participant, type_name, type_code); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "register_type error %d\n", retcode); + subscriber_shutdown(participant); + return -1; + } + + /* To customize the topic QoS, use + the configuration file USER_QOS_PROFILES.xml */ + topic = participant->create_topic( + "my_generic_type", + type_name, DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, + DDS_STATUS_MASK_NONE); + if (topic == NULL) { + fprintf(stderr, "create_topic error\n"); + subscriber_shutdown(participant); + return -1; + } + + /* Create a data reader listener */ + reader_listener = new RawDataListener(); + + /* To customize the data reader QoS, use + the configuration file USER_QOS_PROFILES.xml */ + reader = subscriber->create_datareader( + topic, DDS_DATAREADER_QOS_DEFAULT, reader_listener, + DDS_STATUS_MASK_ALL); + if (reader == NULL) { + fprintf(stderr, "create_datareader error\n"); + subscriber_shutdown(participant); + delete reader_listener; + return -1; + } + + /* Main loop */ + for (count=0; (sample_count == 0) || (count < sample_count); ++count) { + + printf("RawData subscriber sleeping for %d sec...\n", + receive_period.sec); + + NDDSUtility::sleep(receive_period); + } + + /* Delete all entities */ + status = subscriber_shutdown(participant); + delete reader_listener; + + return status; +} + +int main(int argc, char *argv[]) +{ + int domainId = 0; + int sample_count = 0; /* infinite loop */ + + if (argc >= 2) { + domainId = atoi(argv[1]); + } + if (argc >= 3) { + sample_count = atoi(argv[2]); + } + + /* Uncomment this to turn on additional logging + NDDSConfigLogger::get_instance()-> + set_verbosity_by_category(NDDS_CONFIG_LOG_CATEGORY_API, + NDDS_CONFIG_LOG_VERBOSITY_STATUS_ALL); + */ + + return subscriber_main(domainId, sample_count); +} + diff --git a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data.cxx b/rosidl_typesupport_connext_cpp/src/connext_static_raw_data.cxx new file mode 100644 index 00000000..0e2f38c4 --- /dev/null +++ b/rosidl_typesupport_connext_cpp/src/connext_static_raw_data.cxx @@ -0,0 +1,313 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef NDDS_STANDALONE_TYPE +#ifndef ndds_cpp_h +#include "rosidl_typesupport_connext_cpp/ndds_include.hpp" +#endif +#ifndef dds_c_log_impl_h +#include "dds_c/dds_c_log_impl.h" +#endif + +#ifndef cdr_type_h +#include "cdr/cdr_type.h" +#endif + +#ifndef osapi_heap_h +#include "osapi/osapi_heap.h" +#endif +#else +#include "ndds_standalone_type.h" +#endif + +#include "rosidl_typesupport_connext_cpp/connext_static_raw_data.h" + +#include + +/* ========================================================================= */ +const char *ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; + +DDS_TypeCode* ConnextStaticRawData_get_typecode() +{ + static RTIBool is_initialized = RTI_FALSE; + + static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array =DDS_INITIALIZE_ARRAY_TYPECODE(1,(KEY_HASH_LENGTH_16), NULL,NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); + static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3]= + { + + { + (char *)"key_hash",/* Member name */ + { + 0,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_KEY_MEMBER , /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + }, + { + (char *)"serialized_key",/* Member name */ + { + 1,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + }, + { + (char *)"serialized_data",/* Member name */ + { + 2,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + } + }; + + static DDS_TypeCode ConnextStaticRawData_g_tc = + {{ + DDS_TK_STRUCT,/* Kind */ + DDS_BOOLEAN_FALSE, /* Ignored */ + -1, /*Ignored*/ + (char *)"ConnextStaticRawData", /* Name */ + NULL, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + 3, /* Number of members */ + ConnextStaticRawData_g_tc_members, /* Members */ + DDS_VM_NONE /* Ignored */ + }}; /* Type code for ConnextStaticRawData*/ + + if (is_initialized) { + return &ConnextStaticRawData_g_tc; + } + + ConnextStaticRawData_g_tc_key_hash_array._data._typeCode =(RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_members[0]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_key_hash_array; + ConnextStaticRawData_g_tc_members[1]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_key_sequence; + ConnextStaticRawData_g_tc_members[2]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_data_sequence; + + is_initialized = RTI_TRUE; + + return &ConnextStaticRawData_g_tc; +} + +RTIBool ConnextStaticRawData_initialize( + ConnextStaticRawData* sample) { + return ConnextStaticRawData_initialize_ex(sample,RTI_TRUE,RTI_TRUE); +} + +RTIBool ConnextStaticRawData_initialize_ex( + ConnextStaticRawData* sample,RTIBool allocatePointers, RTIBool allocateMemory) +{ + + struct DDS_TypeAllocationParams_t allocParams = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; + + allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; + allocParams.allocate_memory = (DDS_Boolean)allocateMemory; + + return ConnextStaticRawData_initialize_w_params( + sample,&allocParams); + +} + +RTIBool ConnextStaticRawData_initialize_w_params( + ConnextStaticRawData* sample, const struct DDS_TypeAllocationParams_t * allocParams) +{ + + void* buffer = NULL; + if (buffer) {} /* To avoid warnings */ + + if (sample == NULL) { + return RTI_FALSE; + } + if (allocParams == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_initArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_key ); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key , RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { + return RTI_FALSE; + } + } else { + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_data ); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data , RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { + return RTI_FALSE; + } + } else { + DDS_OctetSeq_set_length(&sample->serialized_data, 0); + } + return RTI_TRUE; +} + +void ConnextStaticRawData_finalize( + ConnextStaticRawData* sample) +{ + + ConnextStaticRawData_finalize_ex(sample,RTI_TRUE); +} + +void ConnextStaticRawData_finalize_ex( + ConnextStaticRawData* sample,RTIBool deletePointers) +{ + struct DDS_TypeDeallocationParams_t deallocParams = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; + + if (sample==NULL) { + return; + } + + deallocParams.delete_pointers = (DDS_Boolean)deletePointers; + + ConnextStaticRawData_finalize_w_params( + sample,&deallocParams); +} + +void ConnextStaticRawData_finalize_w_params( + ConnextStaticRawData* sample,const struct DDS_TypeDeallocationParams_t * deallocParams) +{ + + if (sample==NULL) { + return; + } + + if (deallocParams == NULL) { + return; + } + + DDS_OctetSeq_finalize(&sample->serialized_key); + + DDS_OctetSeq_finalize(&sample->serialized_data); + +} + +void ConnextStaticRawData_finalize_optional_members( + ConnextStaticRawData* sample, RTIBool deletePointers) +{ + struct DDS_TypeDeallocationParams_t deallocParamsTmp = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; + struct DDS_TypeDeallocationParams_t * deallocParams = + &deallocParamsTmp; + + if (sample==NULL) { + return; + } + if (deallocParams) {} /* To avoid warnings */ + + deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; + deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; + +} + +RTIBool ConnextStaticRawData_copy( + ConnextStaticRawData* dst, + const ConnextStaticRawData* src) +{ + try { + + if (dst == NULL || src == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_key , + &src->serialized_key )) { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_data , + &src->serialized_data )) { + return RTI_FALSE; + } + + return RTI_TRUE; + + } catch (std::bad_alloc&) { + return RTI_FALSE; + } +} + +/** +* <> +* +* Defines: TSeq, T +* +* Configure and implement 'ConnextStaticRawData' sequence class. +*/ +#define T ConnextStaticRawData +#define TSeq ConnextStaticRawDataSeq + +#define T_initialize_w_params ConnextStaticRawData_initialize_w_params + +#define T_finalize_w_params ConnextStaticRawData_finalize_w_params +#define T_copy ConnextStaticRawData_copy + +#ifndef NDDS_STANDALONE_TYPE +#include "dds_c/generic/dds_c_sequence_TSeq.gen" +#include "dds_cpp/generic/dds_cpp_sequence_TSeq.gen" +#else +#include "dds_c_sequence_TSeq.gen" +#include "dds_cpp_sequence_TSeq.gen" +#endif + +#undef T_copy +#undef T_finalize_w_params + +#undef T_initialize_w_params + +#undef TSeq +#undef T + diff --git a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_plugin.cxx b/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_plugin.cxx new file mode 100644 index 00000000..825551ee --- /dev/null +++ b/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_plugin.cxx @@ -0,0 +1,1497 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#include + +#ifndef ndds_cpp_h +#include "rosidl_typesupport_connext_cpp/ndds_include.hpp" +#endif + +#ifndef osapi_type_h +#include "osapi/osapi_type.h" +#endif +#ifndef osapi_heap_h +#include "osapi/osapi_heap.h" +#endif + +#ifndef osapi_utility_h +#include "osapi/osapi_utility.h" +#endif + +#ifndef cdr_type_h +#include "cdr/cdr_type.h" +#endif + +#ifndef cdr_type_object_h +#include "cdr/cdr_typeObject.h" +#endif + +#ifndef cdr_encapsulation_h +#include "cdr/cdr_encapsulation.h" +#endif + +#ifndef cdr_stream_h +#include "cdr/cdr_stream.h" +#endif + +#ifndef cdr_log_h +#include "cdr/cdr_log.h" +#endif + +#ifndef pres_typePlugin_h +#include "pres/pres_typePlugin.h" +#endif + +#define RTI_CDR_CURRENT_SUBMODULE RTI_CDR_SUBMODULE_MASK_STREAM + +#include + +#include "rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h" + +/* ---------------------------------------------------------------------------- +* Type ConnextStaticRawData +* -------------------------------------------------------------------------- */ + +/* ----------------------------------------------------------------------------- +Support functions: +* -------------------------------------------------------------------------- */ + +ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params) +{ + ConnextStaticRawData *sample = NULL; + + sample = new (std::nothrow) ConnextStaticRawData ; + if (sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_w_params(sample,alloc_params)) { + delete sample; + sample=NULL; + } + return sample; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) +{ + ConnextStaticRawData *sample = NULL; + + sample = new (std::nothrow) ConnextStaticRawData ; + + if(sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_ex(sample,allocate_pointers, RTI_TRUE)) { + delete sample; + sample=NULL; + } + + return sample; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data(void) +{ + return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); +} + +void +ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData *sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params) { + + ConnextStaticRawData_finalize_w_params(sample,dealloc_params); + + delete sample; + sample=NULL; +} + +void +ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData *sample,RTIBool deallocate_pointers) { + + ConnextStaticRawData_finalize_ex(sample,deallocate_pointers); + + delete sample; + sample=NULL; +} + +void +ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData *sample) { + + ConnextStaticRawDataPluginSupport_destroy_data_ex(sample,RTI_TRUE); + +} + +RTIBool +ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData *dst, + const ConnextStaticRawData *src) +{ + return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); +} + +void +ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData *sample, + const char *desc, + unsigned int indent_level) +{ + + RTICdrType_printIndent(indent_level); + + if (desc != NULL) { + RTILog_debug("%s:\n", desc); + } else { + RTILog_debug("\n"); + } + + if (sample == NULL) { + RTILog_debug("NULL\n"); + return; + } + + RTICdrType_printArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "key_hash", indent_level + 1); + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key ), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data ), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } + +} +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers){ + ConnextStaticRawData *key = NULL; + + key = new (std::nothrow) ConnextStaticRawDataKeyHolder ; + + ConnextStaticRawData_initialize_ex(key,allocate_pointers, RTI_TRUE); + + return key; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key(void) +{ + return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); +} + +void +ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) +{ + ConnextStaticRawData_finalize_ex(key,deallocate_pointers); + + delete key; + key=NULL; + +} + +void +ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder *key) { + + ConnextStaticRawDataPluginSupport_destroy_key_ex(key,RTI_TRUE); + +} + +/* ---------------------------------------------------------------------------- +Callback functions: +* ---------------------------------------------------------------------------- */ + +PRESTypePluginParticipantData +ConnextStaticRawDataPlugin_on_participant_attached( + void *registration_data, + const struct PRESTypePluginParticipantInfo *participant_info, + RTIBool top_level_registration, + void *container_plugin_context, + RTICdrTypeCode *type_code) +{ + if (registration_data) {} /* To avoid warnings */ + if (participant_info) {} /* To avoid warnings */ + if (top_level_registration) {} /* To avoid warnings */ + if (container_plugin_context) {} /* To avoid warnings */ + if (type_code) {} /* To avoid warnings */ + + return PRESTypePluginDefaultParticipantData_new(participant_info); + +} + +void +ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data) +{ + + PRESTypePluginDefaultParticipantData_delete(participant_data); +} + +PRESTypePluginEndpointData +ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo *endpoint_info, + RTIBool top_level_registration, + void *containerPluginContext) +{ + PRESTypePluginEndpointData epd = NULL; + + unsigned int serializedSampleMaxSize; + + unsigned int serializedKeyMaxSize; + + if (top_level_registration) {} /* To avoid warnings */ + if (containerPluginContext) {} /* To avoid warnings */ + + epd = PRESTypePluginDefaultEndpointData_new( + participant_data, + endpoint_info, + (PRESTypePluginDefaultEndpointDataCreateSampleFunction) + ConnextStaticRawDataPluginSupport_create_data, + (PRESTypePluginDefaultEndpointDataDestroySampleFunction) + ConnextStaticRawDataPluginSupport_destroy_data, + (PRESTypePluginDefaultEndpointDataCreateKeyFunction) + ConnextStaticRawDataPluginSupport_create_key , + (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) + ConnextStaticRawDataPluginSupport_destroy_key); + + if (epd == NULL) { + return NULL; + } + serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( + epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); + + if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( + epd,endpoint_info,serializedKeyMaxSize)) + { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + + if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { + serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); + + PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); + + if (PRESTypePluginDefaultEndpointData_createWriterPool( + epd, + endpoint_info, + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size, + epd) == RTI_FALSE) { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + } + + return epd; +} + +void +ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data) +{ + + PRESTypePluginDefaultEndpointData_delete(endpoint_data); +} + +void +ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + void *handle) +{ + + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + + PRESTypePluginDefaultEndpointData_returnSample( + endpoint_data, sample, handle); +} + +RTIBool +ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *dst, + const ConnextStaticRawData *src) +{ + if (endpoint_data) {} /* To avoid warnings */ + return ConnextStaticRawDataPluginSupport_copy_data(dst,src); +} + +/* ---------------------------------------------------------------------------- +(De)Serialize functions: +* ------------------------------------------------------------------------- */ +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +RTIBool +ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + RTIBool retval = RTI_TRUE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + /* This plugin can only be used to publish the top-level DDS Topic-Type + * in which case serialize_encapsulation==TRUE. If that is not + * the case then it is an error. + */ + if (!serialize_encapsulation) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + + if(serialize_sample) { + /* The sample->serialized_data contains the serialized encapsulation followed by the serialized + * data, so we only need to copy that into + * the CDR stream. Not the key_hash, not the length of the data itself + * The SerializedType sample->serialized_data is always a contiguous buffer + */ + DDS_Octet *buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); + if ( buffer == NULL ) { + return RTI_FALSE; + } + + /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian + * byte order + */ + if ( encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { + return RTI_FALSE; + } + + /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ + if (!RTICdrStream_serializePrimitiveArray( + stream, (void*)buffer, + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } + + RTICdrStream_restoreAlignment(stream ,position); + + return retval; +} + +/** + TODO. The code-block below does not belong here. + It should be pushed to the CDR module, perhaps inside + RTICdrStream_deserializeAndSetCdrEncapsulation so that the + stream size is alredy correct when SerializedTypePlugin_deserialize_sample + is called. + + Adjust the size of the CDR stream to not include the alignment + padding. See http://issues.omg.org/browse/DDSXTY12-10 + + @precondition The RTICdrStream *stream has alreadt processed + the encapsulation header and therefore has set the + encapsulation options returned by + RTICdrStream_getEncapsulationOptions() +*/ +void +ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream *stream) +{ + /* See http://issues.omg.org/browse/DDSXTY12-10 */ + DDS_UnsignedShort padding_size_mask = 0x0003; + DDS_UnsignedShort padding_size; + int adjustedBufferLength; + + padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; + adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; + RTICdrStream_setBufferLength(stream, adjustedBufferLength); +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + RTIBool done = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + /* This plugin can only be used to publish the top-level DDS Topic-Type + * in which case deserialize_encapsulation==TRUE. If that is not + * the case then it is an error. + */ + if (!deserialize_encapsulation) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + + /* TODO. The call does not belong here. It should be pushed + * inside RTICdrStream_deserializeAndSetCdrEncapsulation + */ + ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); + + if (deserialize_sample) { + /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() + it is done there because SerializedTypePlugin_deserialize_sample does not + have access to the SampleInfo where that information is + */ + + /* We do not set the serialized_key on deserialization */ + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + + /* We copy everything that remains in the CDR stream */ + int bytesLeftInStream = RTICdrStream_getRemainder(stream); + DDS_Octet *cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); + if (cdrBufferPtr == NULL) { + goto fin; + } + + /* Do not call SerializedType_initialize_ex initialize here + because it would override the key_hash field + SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); + */ + if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { + goto fin; + } + RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); + } + + done = RTI_TRUE; + + fin: + if ( (done != RTI_TRUE) && + (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) { + return RTI_FALSE; + } + + RTICdrStream_restoreAlignment(stream,position); + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData *sample) +{ + struct RTICdrStream stream; + struct PRESTypePluginDefaultEndpointData epd; + RTIBool result; + + if (length == NULL) { + return RTI_FALSE; + } + + epd._maxSizeSerializedSample = + ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); + + if (buffer == NULL) { + *length = + ConnextStaticRawDataPlugin_get_serialized_sample_size( + (PRESTypePluginEndpointData)&epd, + RTI_TRUE, + RTICdrEncapsulation_getNativeCdrEncapsulationId(), + 0, + sample); + + if (*length == 0) { + return RTI_FALSE; + } + + return RTI_TRUE; + } + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, *length); + + result = ConnextStaticRawDataPlugin_serialize( + (PRESTypePluginEndpointData)&epd, sample, &stream, + RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), + RTI_TRUE, NULL); + + *length = RTICdrStream_getCurrentPositionOffset(&stream); + return result; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData *sample, + const char * buffer, + unsigned int length) +{ + struct RTICdrStream stream; + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, length); + + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + return ConnextStaticRawDataPlugin_deserialize_sample( + NULL, sample, + &stream, RTI_TRUE, RTI_TRUE, + NULL); +} + +DDS_ReturnCode_t +ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property) +{ + DDS_DynamicData *data = NULL; + char *buffer = NULL; + unsigned int length = 0; + struct DDS_PrintFormat printFormat; + DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; + + if (sample == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (str_size == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (property == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + NULL, + &length, + sample)) { + return DDS_RETCODE_ERROR; + } + + RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); + if (buffer == NULL) { + return DDS_RETCODE_ERROR; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + buffer, + &length, + sample)) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } + + data = DDS_DynamicData_new( + ConnextStaticRawData_get_typecode(), + &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); + if (data == NULL) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } + + retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_PrintFormatProperty_to_print_format( + property, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_DynamicDataFormatter_to_string_w_format( + data, + str, + str_size, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return DDS_RETCODE_OK; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos) +{ + + RTIBool result; + const char *METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; + if (drop_sample) {} /* To avoid warnings */ + + stream->_xTypesState.unassignable = RTI_FALSE; + result= ConnextStaticRawDataPlugin_deserialize_sample( + endpoint_data, (sample != NULL)?*sample:NULL, + stream, deserialize_encapsulation, deserialize_sample, + endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; + } + } + if (!result && stream->_xTypesState.unassignable ) { + + RTICdrLog_exception( + METHOD_NAME, + &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, + "ConnextStaticRawData"); + + } + + return result; +} + +RTIBool ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(skip_encapsulation) { + if (!RTICdrStream_skipEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if (skip_sample) { + + if (!RTICdrStream_skipPrimitiveArray( + stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + goto fin; + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + } + + done = RTI_TRUE; + fin: + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + if(skip_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + if (endpoint_data) {} /* To avoid warnings */ + if (include_encapsulation) {} + if (encapsulation_id) {} + if (current_alignment) {} + + if (overflow != NULL) { + *overflow = RTI_TRUE; + } + + return RTI_CDR_MAX_SERIALIZED_SIZE; + +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + unsigned int size; + RTIBool overflow = RTI_FALSE; + + size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } + + return size; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + + if (endpoint_data) {} /* To avoid warnings */ + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } + + current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment,0, RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment,0, RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +/* Returns the size of the sample in its serialized form (in bytes). +* It can also be an estimation in excess of the real buffer needed +* during a call to the serialize() function. +* The value reported does not have to include the space for the +* encapsulation flags. +*/ +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + struct PRESTypePluginDefaultEndpointData epd; + + if (sample==NULL) { + return 0; + } + if (endpoint_data == NULL) { + endpoint_data = (PRESTypePluginEndpointData) &epd; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +/* -------------------------------------------------------------------------------------- +Key Management functions: +* -------------------------------------------------------------------------------------- */ + +PRESTypePluginKeyKind +ConnextStaticRawDataPlugin_get_key_kind(void) +{ + return PRES_TYPEPLUGIN_NO_KEY; +} + +RTIBool +ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(serialize_encapsulation) { + if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if(serialize_key) { + + if (!RTICdrStream_serializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + } + + if(serialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + try { + + char * position = NULL; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(deserialize_encapsulation) { + + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + if (deserialize_key) { + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; + + } catch (std::bad_alloc&) { + return RTI_FALSE; + } +} + +RTIBool ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + RTIBool result; + if (drop_sample) {} /* To avoid warnings */ + stream->_xTypesState.unassignable = RTI_FALSE; + result= ConnextStaticRawDataPlugin_deserialize_key_sample( + endpoint_data, (sample != NULL)?*sample:NULL, stream, + deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; + } + } + + return result; + +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + + if (endpoint_data) {} /* To avoid warnings */ + if (overflow) {} /* To avoid warnings */ + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } + + current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + unsigned int size; + RTIBool overflow = RTI_FALSE; + + size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } + + return size; +} + +RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + if(deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + position = RTICdrStream_resetAlignment(stream); + } + + if (deserialize_key) { + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + + } + + done = RTI_TRUE; + fin: + if(!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + } else { + return RTI_FALSE; + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder *dst, + const ConnextStaticRawData *src) +{ + + if (endpoint_data) {} /* To avoid warnings */ + + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *dst, const + ConnextStaticRawDataKeyHolder *src) +{ + + if (endpoint_data) {} /* To avoid warnings */ + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t *keyhash, + const ConnextStaticRawData *instance) +{ + struct RTICdrStream * md5Stream = NULL; + struct RTICdrStreamState cdrState; + char * buffer = NULL; + + RTICdrStreamState_init(&cdrState); + md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); + + if (md5Stream == NULL) { + return RTI_FALSE; + } + + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + int size; + + RTICdrStream_pushState(md5Stream, &cdrState, -1); + + size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( + endpoint_data, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + 0, + instance); + + if (size <= RTICdrStream_getBufferLength(md5Stream)) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTIOsapiHeap_allocateBuffer(&buffer,size,0); + + if (buffer == NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTICdrStream_set(md5Stream, buffer, size); + RTIOsapiMemory_zero( + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getBufferLength(md5Stream)); + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + return RTI_FALSE; + } + } + + if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > + (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || + PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { + RTICdrStream_computeMD5(md5Stream, keyhash->value); + } else { + RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); + RTIOsapiMemory_copy( + keyhash->value, + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getCurrentPositionOffset(md5Stream)); + } + + keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; + + if (buffer != NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + } + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + DDS_KeyHash_t *keyhash, + RTIBool deserialize_encapsulation, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + ConnextStaticRawData * sample=NULL; + + if (endpoint_plugin_qos) {} /* To avoid warnings */ + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + + if(deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + sample = (ConnextStaticRawData *) + PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); + + if (sample == NULL) { + return RTI_FALSE; + } + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + done = RTI_TRUE; + fin: + if(!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + } else { + return RTI_FALSE; + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + if (!ConnextStaticRawDataPlugin_instance_to_keyhash( + endpoint_data, keyhash, sample)) { + return RTI_FALSE; + } + + return RTI_TRUE; +} + +/* ------------------------------------------------------------------------ +* Plug-in Installation Methods +* ------------------------------------------------------------------------ */ +struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) +{ + return NULL; +} + +struct PRESTypePlugin * +ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) +{ + if (external_type_code == NULL) { + return NULL; + } + + struct PRESTypePlugin *plugin = NULL; + const struct PRESTypePluginVersion PLUGIN_VERSION = + PRES_TYPE_PLUGIN_VERSION_2_0; + + RTIOsapiHeap_allocateStructure( + &plugin, struct PRESTypePlugin); + + if (plugin == NULL) { + return NULL; + } + + plugin->version = PLUGIN_VERSION; + + /* set up parent's function pointers */ + plugin->onParticipantAttached = + (PRESTypePluginOnParticipantAttachedCallback) + ConnextStaticRawDataPlugin_on_participant_attached; + plugin->onParticipantDetached = + (PRESTypePluginOnParticipantDetachedCallback) + ConnextStaticRawDataPlugin_on_participant_detached; + plugin->onEndpointAttached = + (PRESTypePluginOnEndpointAttachedCallback) + ConnextStaticRawDataPlugin_on_endpoint_attached; + plugin->onEndpointDetached = + (PRESTypePluginOnEndpointDetachedCallback) + ConnextStaticRawDataPlugin_on_endpoint_detached; + + plugin->copySampleFnc = + (PRESTypePluginCopySampleFunction) + ConnextStaticRawDataPlugin_copy_sample; + plugin->createSampleFnc = + (PRESTypePluginCreateSampleFunction) + ConnextStaticRawDataPlugin_create_sample; + plugin->destroySampleFnc = + (PRESTypePluginDestroySampleFunction) + ConnextStaticRawDataPlugin_destroy_sample; + + plugin->serializeFnc = + (PRESTypePluginSerializeFunction) + ConnextStaticRawDataPlugin_serialize; + plugin->deserializeFnc = + (PRESTypePluginDeserializeFunction) + ConnextStaticRawDataPlugin_deserialize; + plugin->getSerializedSampleMaxSizeFnc = + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size; + plugin->getSerializedSampleMinSizeFnc = + (PRESTypePluginGetSerializedSampleMinSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_min_size; + + plugin->getSampleFnc = + (PRESTypePluginGetSampleFunction) + ConnextStaticRawDataPlugin_get_sample; + plugin->returnSampleFnc = + (PRESTypePluginReturnSampleFunction) + ConnextStaticRawDataPlugin_return_sample; + + //plugin->getKeyKindFnc = ConnextStaticRawDataPlugin_get_external_key_kind(external_type_code); + plugin->getKeyKindFnc = (PRESTypePluginGetKeyKindFunction) + ConnextStaticRawDataPlugin_get_key_kind; + + plugin->getSerializedKeyMaxSizeFnc = + (PRESTypePluginGetSerializedKeyMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_key_max_size; + plugin->serializeKeyFnc = + (PRESTypePluginSerializeKeyFunction) + ConnextStaticRawDataPlugin_serialize_key; + plugin->deserializeKeyFnc = + (PRESTypePluginDeserializeKeyFunction) + ConnextStaticRawDataPlugin_deserialize_key; + plugin->deserializeKeySampleFnc = + (PRESTypePluginDeserializeKeySampleFunction) + ConnextStaticRawDataPlugin_deserialize_key_sample; + + plugin-> instanceToKeyHashFnc = + (PRESTypePluginInstanceToKeyHashFunction) + ConnextStaticRawDataPlugin_instance_to_keyhash; + plugin->serializedSampleToKeyHashFnc = + (PRESTypePluginSerializedSampleToKeyHashFunction) + ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; + + plugin->getKeyFnc = + (PRESTypePluginGetKeyFunction) + ConnextStaticRawDataPlugin_get_key; + plugin->returnKeyFnc = + (PRESTypePluginReturnKeyFunction) + ConnextStaticRawDataPlugin_return_key; + + plugin->instanceToKeyFnc = + (PRESTypePluginInstanceToKeyFunction) + ConnextStaticRawDataPlugin_instance_to_key; + plugin->keyToInstanceFnc = + (PRESTypePluginKeyToInstanceFunction) + ConnextStaticRawDataPlugin_key_to_instance; + plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ + //plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); + plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; + + plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; + + /* Serialized buffer */ + plugin->getBuffer = + (PRESTypePluginGetBufferFunction) + ConnextStaticRawDataPlugin_get_buffer; + plugin->returnBuffer = + (PRESTypePluginReturnBufferFunction) + ConnextStaticRawDataPlugin_return_buffer; + plugin->getSerializedSampleSizeFnc = + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size; + + plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; + + return plugin; +} + +void +ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) +{ + RTIOsapiHeap_freeStructure(plugin); +} +#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_support.cxx b/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_support.cxx new file mode 100644 index 00000000..4a526e43 --- /dev/null +++ b/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_support.cxx @@ -0,0 +1,182 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#include "rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h" +#include "rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h" + +#ifndef dds_c_log_impl_h +#include "dds_c/dds_c_log_impl.h" +#endif + +/* ========================================================================= */ +/** +<> + +Defines: TData, +TDataWriter, +TDataReader, +TTypeSupport + +Configure and implement 'ConnextStaticRawData' support classes. + +Note: Only the #defined classes get defined +*/ + +/* ----------------------------------------------------------------- */ +/* DDSDataWriter +*/ + +/** +<> + +Defines: TDataWriter, TData +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME + +/* Defines */ +#define TDataWriter ConnextStaticRawDataDataWriter +#define TData ConnextStaticRawData + +#include "dds_cpp/generic/dds_cpp_data_TDataWriter.gen" + +#undef TDataWriter +#undef TData + +#undef TTYPENAME + +/* ----------------------------------------------------------------- */ +/* DDSDataReader +*/ + +/** +<> + +Defines: TDataReader, TDataSeq, TData +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME + +/* Defines */ +#define TDataReader ConnextStaticRawDataDataReader +#define TDataSeq ConnextStaticRawDataSeq +#define TData ConnextStaticRawData + +#include "dds_cpp/generic/dds_cpp_data_TDataReader.gen" + +#undef TDataReader +#undef TDataSeq +#undef TData + +#undef TTYPENAME + +/* ----------------------------------------------------------------- */ +/* TypeSupport + +<> + +Requires: TTYPENAME, +TPlugin_new +TPlugin_delete +Defines: TTypeSupport, TData, TDataReader, TDataWriter +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME +#define TPlugin_new ConnextStaticRawDataPlugin_new +#define TPlugin_delete ConnextStaticRawDataPlugin_delete + +/* Defines */ +#define TTypeSupport ConnextStaticRawDataTypeSupport +#define TData ConnextStaticRawData +#define TDataReader ConnextStaticRawDataDataReader +#define TDataWriter ConnextStaticRawDataDataWriter +#define TGENERATE_SER_CODE +#define TGENERATE_TYPECODE + +#include "dds_cpp/generic/dds_cpp_data_TTypeSupport.gen" + +#undef TTypeSupport +#undef TData +#undef TDataReader +#undef TDataWriter +#undef TGENERATE_TYPECODE +#undef TGENERATE_SER_CODE +#undef TTYPENAME +#undef TPlugin_new +#undef TPlugin_delete + +DDS_ReturnCode_t +ConnextStaticRawDataSupport_register_external_type( + DDSDomainParticipant * participant, + const char * type_name, + struct DDS_TypeCode * type_code) +{ + DDSTypeSupport * dds_data_type = NULL; + struct PRESTypePlugin * presTypePlugin = NULL; + DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; + DDS_Boolean delete_data_type = DDS_BOOLEAN_FALSE; + RTIBool already_registered = RTI_FALSE; + + if (type_code == NULL) { + goto finError; + } + + if (participant == NULL) { + goto finError; + } + + /* TODO pass the type_code */ + presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); + if (presTypePlugin == NULL) { + goto finError; + } + + dds_data_type = new ConnextStaticRawDataTypeSupport(true); + if (dds_data_type == NULL) { + fprintf(stderr, "Error while registering external type\n"); + goto finError; + } + delete_data_type = RTI_TRUE; + + presTypePlugin->_userBuffer = (PRESWord *)dds_data_type; + already_registered = participant->is_type_registered(type_name); + + retcode = participant->register_type(type_name, presTypePlugin, NULL, !already_registered); + //retcode = DDS_DomainParticipant_register_type( + // participant, + // type_name, + // presTypePlugin, + // NULL /* registration_data */); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "error while registering external type\n"); + goto finError; + } + + if (!already_registered) { + delete_data_type = DDS_BOOLEAN_FALSE; + } + + retcode = DDS_RETCODE_OK; + +finError: + if (presTypePlugin != NULL) { + ConnextStaticRawDataPlugin_delete(presTypePlugin); + } + if (delete_data_type) { + delete (ConnextStaticRawDataTypeSupport *)dds_data_type; + dds_data_type = NULL; + } + + return retcode; +} + From aa2aee105fef23d3f6aed060affd79c57a813dc1 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 26 Feb 2018 17:23:31 -0800 Subject: [PATCH 10/34] rewrite type support callback structure --- rmw_connext_cpp/CMakeLists.txt | 3 + .../connext_static_raw_data.hpp | 2 +- .../connext_static_raw_data_plugin.hpp | 2 +- .../connext_static_raw_data_support.hpp | 4 +- .../src/connext_static_raw_data.cpp | 4 +- .../src/connext_static_raw_data_plugin.cpp | 4 +- .../src/connext_static_raw_data_support.cpp | 4 +- rmw_connext_cpp/src/rmw_publish.cpp | 52 +- rmw_connext_cpp/src/rmw_publisher.cpp | 22 +- rmw_connext_cpp/src/rmw_subscription.cpp | 22 +- rmw_connext_cpp/src/rmw_take.cpp | 98 +- rosidl_typesupport_connext_c/CMakeLists.txt | 13 +- .../SerializedType.idl | 7 + .../SerializedType_publisher.c | 274 +++ .../SerializedType_subscriber.c | 263 +++ .../connext_static_raw_data.h | 88 + .../connext_static_raw_data_plugin.h | 339 ++++ .../connext_static_raw_data_support.h | 61 + .../ndds_include.h | 13 +- .../resource/msg__type_support_c.cpp.em | 285 +-- .../src/connext_static_raw_data.c | 302 ++++ .../src/connext_static_raw_data_plugin.c | 1527 +++++++++++++++++ .../src/connext_static_raw_data_support.c | 167 ++ rosidl_typesupport_connext_cpp/CMakeLists.txt | 12 +- .../message_type_support.h | 13 +- ...msg__rosidl_typesupport_connext_cpp.hpp.em | 26 +- .../resource/msg__type_support.cpp.em | 161 +- 27 files changed, 3298 insertions(+), 470 deletions(-) rename rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data.h => rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp (98%) rename rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h => rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp (99%) rename rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h => rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp (97%) rename rosidl_typesupport_connext_cpp/src/connext_static_raw_data.cxx => rmw_connext_cpp/src/connext_static_raw_data.cpp (98%) rename rosidl_typesupport_connext_cpp/src/connext_static_raw_data_plugin.cxx => rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp (99%) rename rosidl_typesupport_connext_cpp/src/connext_static_raw_data_support.cxx => rmw_connext_cpp/src/connext_static_raw_data_support.cpp (96%) create mode 100644 rosidl_typesupport_connext_c/SerializedType.idl create mode 100644 rosidl_typesupport_connext_c/SerializedType_publisher.c create mode 100644 rosidl_typesupport_connext_c/SerializedType_subscriber.c create mode 100644 rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data.h create mode 100644 rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h create mode 100644 rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_support.h rename rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/ndds_include.hpp => rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/ndds_include.h (68%) create mode 100644 rosidl_typesupport_connext_c/src/connext_static_raw_data.c create mode 100644 rosidl_typesupport_connext_c/src/connext_static_raw_data_plugin.c create mode 100644 rosidl_typesupport_connext_c/src/connext_static_raw_data_support.c diff --git a/rmw_connext_cpp/CMakeLists.txt b/rmw_connext_cpp/CMakeLists.txt index 42b21f69..b1971bb6 100644 --- a/rmw_connext_cpp/CMakeLists.txt +++ b/rmw_connext_cpp/CMakeLists.txt @@ -69,6 +69,9 @@ register_rmw_implementation( add_library( rmw_connext_cpp SHARED + src/connext_static_raw_data.cpp + src/connext_static_raw_data_plugin.cpp + src/connext_static_raw_data_support.cpp src/get_client.cpp src/get_participant.cpp src/get_publisher.cpp diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data.h b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp similarity index 98% rename from rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data.h rename to rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp index 33b79ab7..d9b155c5 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data.h +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp @@ -14,7 +14,7 @@ or consult the RTI Connext manual. #ifndef NDDS_STANDALONE_TYPE #ifndef ndds_cpp_h -#include "ndds_include.hpp" +#include "rmw_connext_shared_cpp/ndds_include.hpp" #endif #else #include "ndds_standalone_type.h" diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp similarity index 99% rename from rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h rename to rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp index ae1556f1..855667ef 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp @@ -12,7 +12,7 @@ or consult the RTI Connext manual. #ifndef ConnextStaticRawDataPlugin_1689213465_h #define ConnextStaticRawDataPlugin_1689213465_h -#include "connext_static_raw_data.h" +#include "rmw_connext_cpp/connext_static_raw_data.hpp" struct RTICdrStream; diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp similarity index 97% rename from rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h rename to rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp index 9e60bed1..74f223b9 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp @@ -12,10 +12,10 @@ or consult the RTI Connext manual. #define ConnextStaticRawDataSupport_1689213465_h /* Uses */ -#include "connext_static_raw_data.h" +#include "rmw_connext_cpp/connext_static_raw_data.hpp" #ifndef ndds_cpp_h -#include "ndds_include.hpp" +#include "rmw_connext_shared_cpp/ndds_include.hpp" #endif #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) diff --git a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data.cxx b/rmw_connext_cpp/src/connext_static_raw_data.cpp similarity index 98% rename from rosidl_typesupport_connext_cpp/src/connext_static_raw_data.cxx rename to rmw_connext_cpp/src/connext_static_raw_data.cpp index 0e2f38c4..b8ba2fc1 100644 --- a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data.cxx +++ b/rmw_connext_cpp/src/connext_static_raw_data.cpp @@ -11,7 +11,7 @@ or consult the RTI Connext manual. #ifndef NDDS_STANDALONE_TYPE #ifndef ndds_cpp_h -#include "rosidl_typesupport_connext_cpp/ndds_include.hpp" +#include "rmw_connext_shared_cpp/ndds_include.hpp" #endif #ifndef dds_c_log_impl_h #include "dds_c/dds_c_log_impl.h" @@ -28,7 +28,7 @@ or consult the RTI Connext manual. #include "ndds_standalone_type.h" #endif -#include "rosidl_typesupport_connext_cpp/connext_static_raw_data.h" +#include "rmw_connext_cpp/connext_static_raw_data.hpp" #include diff --git a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_plugin.cxx b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp similarity index 99% rename from rosidl_typesupport_connext_cpp/src/connext_static_raw_data_plugin.cxx rename to rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp index 825551ee..84b37e5c 100644 --- a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_plugin.cxx +++ b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp @@ -11,7 +11,7 @@ or consult the RTI Connext manual. #include #ifndef ndds_cpp_h -#include "rosidl_typesupport_connext_cpp/ndds_include.hpp" +#include "rmw_connext_shared_cpp/ndds_include.hpp" #endif #ifndef osapi_type_h @@ -53,7 +53,7 @@ or consult the RTI Connext manual. #include -#include "rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h" +#include "rmw_connext_cpp/connext_static_raw_data_plugin.hpp" /* ---------------------------------------------------------------------------- * Type ConnextStaticRawData diff --git a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_support.cxx b/rmw_connext_cpp/src/connext_static_raw_data_support.cpp similarity index 96% rename from rosidl_typesupport_connext_cpp/src/connext_static_raw_data_support.cxx rename to rmw_connext_cpp/src/connext_static_raw_data_support.cpp index 4a526e43..2c49ebe9 100644 --- a/rosidl_typesupport_connext_cpp/src/connext_static_raw_data_support.cxx +++ b/rmw_connext_cpp/src/connext_static_raw_data_support.cpp @@ -8,8 +8,8 @@ For more information, type 'rtiddsgen -help' at a command shell or consult the RTI Connext manual. */ -#include "rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h" -#include "rosidl_typesupport_connext_cpp/connext_static_raw_data_plugin.h" +#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" +#include "rmw_connext_cpp/connext_static_raw_data_plugin.hpp" #ifndef dds_c_log_impl_h #include "dds_c/dds_c_log_impl.h" diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 50044a1d..ea3ad584 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -16,11 +16,57 @@ #include "rmw/rmw.h" #include "rmw/types.h" -#include "rmw_connext_cpp/identifier.hpp" +#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" #include "rmw_connext_cpp/connext_static_publisher_info.hpp" +#include "rmw_connext_cpp/identifier.hpp" #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" +bool +publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) +{ + // for (unsigned int i = 0; i < cdr_stream->message_length; ++i) { + // fprintf(stderr, "%x02 ", cdr_stream->raw_message[i]); + // } + // fprintf(stderr, "\n"); + + ConnextStaticRawDataDataWriter * data_writer = + ConnextStaticRawDataDataWriter::narrow(dds_data_writer); + if (!data_writer) { + RMW_SET_ERROR_MSG("failed to narrow data writer"); + return false; + } + + ConnextStaticRawData * instance = ConnextStaticRawDataTypeSupport::create_data(); + if (!instance) { + RMW_SET_ERROR_MSG("failed to create dds message instance"); + return false; + } + + DDS_ReturnCode_t status = DDS_RETCODE_ERROR; + + instance->serialized_data.maximum(0); + if (!instance->serialized_data.loan_contiguous( + reinterpret_cast(cdr_stream->raw_message), + cdr_stream->message_length, cdr_stream->message_length)) { + RMW_SET_ERROR_MSG("failed to loan memory for message"); + goto cleanup; + } + + status = data_writer->write(*instance, DDS_HANDLE_NIL); + +cleanup: + if (instance) { + if (!instance->serialized_data.unloan()) { + fprintf(stderr, "failed to return loaned memory\n"); + status = DDS_RETCODE_ERROR; + } + ConnextStaticRawDataTypeSupport::delete_data(instance); + } + + return status == DDS_RETCODE_OK; +} + extern "C" { rmw_ret_t @@ -69,7 +115,7 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) RMW_SET_ERROR_MSG("no raw message attached"); return RMW_RET_ERROR; } - if (!callbacks->publish(topic_writer, &cdr_stream)) { + if (!publish(topic_writer, &cdr_stream)) { RMW_SET_ERROR_MSG("failed to publish message"); return RMW_RET_ERROR; } @@ -113,7 +159,7 @@ rmw_publish_raw(const rmw_publisher_t * publisher, const rmw_message_raw_t * raw ConnextStaticCDRStream cdr_stream; cdr_stream.raw_message = raw_message->buffer; cdr_stream.message_length = raw_message->buffer_length; - bool published = callbacks->publish(topic_writer, &cdr_stream); + bool published = publish(topic_writer, &cdr_stream); if (!published) { RMW_SET_ERROR_MSG("failed to publish message"); return RMW_RET_ERROR; diff --git a/rmw_connext_cpp/src/rmw_publisher.cpp b/rmw_connext_cpp/src/rmw_publisher.cpp index a10f8fc0..cee7cf69 100644 --- a/rmw_connext_cpp/src/rmw_publisher.cpp +++ b/rmw_connext_cpp/src/rmw_publisher.cpp @@ -23,7 +23,9 @@ #include "rmw_connext_shared_cpp/qos.hpp" #include "rmw_connext_shared_cpp/types.hpp" +#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" #include "rmw_connext_cpp/identifier.hpp" + #include "process_topic_and_service_names.hpp" #include "type_support_common.hpp" #include "rmw_connext_cpp/connext_static_publisher_info.hpp" @@ -82,7 +84,7 @@ rmw_create_publisher( } std::string type_name = _create_type_name(callbacks, "msg"); // Past this point, a failure results in unrolling code in the goto fail block. - bool registered; + DDS_TypeCode * type_code = nullptr; DDS_DataWriterQos datawriter_qos; DDS_PublisherQos publisher_qos; DDS_ReturnCode_t status; @@ -104,9 +106,21 @@ rmw_create_publisher( goto fail; } - registered = callbacks->register_type(participant, type_name.c_str()); - if (!registered) { - RMW_SET_ERROR_MSG("failed to register type"); + type_code = callbacks->get_type_code(); + if (!type_code) { + RMW_SET_ERROR_MSG("failed to fetch type code\n"); + goto fail; + } + // This is a non-standard RTI Connext function + // It allows to register an external type to a static data writer + // In this case, we register the custom message type to a data writer, + // which only publishes DDS_Octets + // The purpose of this is to send only raw data DDS_Octets over the wire, + // advertise the topic however with a type of the message, e.g. std_msgs::msg::dds_::String + status = ConnextStaticRawDataSupport_register_external_type( + participant, type_name.c_str(), type_code); + if (status != DDS_RETCODE_OK) { + RMW_SET_ERROR_MSG("failed to register external type"); goto fail; } diff --git a/rmw_connext_cpp/src/rmw_subscription.cpp b/rmw_connext_cpp/src/rmw_subscription.cpp index affa408e..fac35287 100644 --- a/rmw_connext_cpp/src/rmw_subscription.cpp +++ b/rmw_connext_cpp/src/rmw_subscription.cpp @@ -22,7 +22,9 @@ #include "rmw_connext_shared_cpp/qos.hpp" #include "rmw_connext_shared_cpp/types.hpp" +#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" #include "rmw_connext_cpp/identifier.hpp" + #include "process_topic_and_service_names.hpp" #include "type_support_common.hpp" #include "rmw_connext_cpp/connext_static_subscriber_info.hpp" @@ -77,7 +79,7 @@ rmw_create_subscription( } std::string type_name = _create_type_name(callbacks, "msg"); // Past this point, a failure results in unrolling code in the goto fail block. - bool registered; + DDS_TypeCode * type_code = nullptr; DDS_DataReaderQos datareader_qos; DDS_SubscriberQos subscriber_qos; DDS_ReturnCode_t status; @@ -100,9 +102,21 @@ rmw_create_subscription( goto fail; } - registered = callbacks->register_type(participant, type_name.c_str()); - if (!registered) { - RMW_SET_ERROR_MSG("failed to register type"); + type_code = callbacks->get_type_code(); + if (!type_code) { + RMW_SET_ERROR_MSG("failed to fetch type code\n"); + goto fail; + } + // This is a non-standard RTI Connext function + // It allows to register an external type to a static data writer + // In this case, we register the custom message type to a data writer, + // which only publishes DDS_Octets + // The purpose of this is to send only raw data DDS_Octets over the wire, + // advertise the topic however with a type of the message, e.g. std_msgs::msg::dds_::String + status = ConnextStaticRawDataSupport_register_external_type( + participant, type_name.c_str(), type_code); + if (status != DDS_RETCODE_OK) { + RMW_SET_ERROR_MSG("failed to register external type"); goto fail; } diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index a3ffcb2b..7a2d02bf 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -18,11 +18,103 @@ #include "rmw_connext_shared_cpp/types.hpp" -#include "rmw_connext_cpp/identifier.hpp" #include "rmw_connext_cpp/connext_static_subscriber_info.hpp" +#include "rmw_connext_cpp/connext_static_raw_data.hpp" +#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" +#include "rmw_connext_cpp/identifier.hpp" #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" +static bool +take( + DDSDataReader * dds_data_reader, + bool ignore_local_publications, + ConnextStaticCDRStream * cdr_stream, + bool * taken, + void * sending_publication_handle) +{ + if (!dds_data_reader) { + RMW_SET_ERROR_MSG("dds_data_reader is null"); + return false; + } + if (!cdr_stream) { + RMW_SET_ERROR_MSG("cdr stream handle is null"); + return false; + } + if (!taken) { + RMW_SET_ERROR_MSG("taken handle is null"); + return false; + } + + ConnextStaticRawDataDataReader * data_reader = + ConnextStaticRawDataDataReader::narrow(dds_data_reader); + if (!data_reader) { + RMW_SET_ERROR_MSG("failed to narrow data reader"); + return false; + } + + ConnextStaticRawDataSeq dds_messages; + DDS_SampleInfoSeq sample_infos; + bool ignore_sample = false; + + DDS_ReturnCode_t status = data_reader->take( + dds_messages, + sample_infos, + 1, + DDS_ANY_SAMPLE_STATE, + DDS_ANY_VIEW_STATE, + DDS_ANY_INSTANCE_STATE); + if (status == DDS_RETCODE_NO_DATA) { + data_reader->return_loan(dds_messages, sample_infos); + *taken = false; + return true; + } + if (status != DDS_RETCODE_OK) { + RMW_SET_ERROR_MSG("take failed"); + data_reader->return_loan(dds_messages, sample_infos); + } + + DDS_SampleInfo & sample_info = sample_infos[0]; + if (!sample_info.valid_data) { + // skip sample without data + ignore_sample = true; + } else if (ignore_local_publications) { + // compare the lower 12 octets of the guids from the sender and this receiver + // if they are equal the sample has been sent from this process and should be ignored + DDS_GUID_t sender_guid = sample_info.original_publication_virtual_guid; + DDS_InstanceHandle_t receiver_instance_handle = dds_data_reader->get_instance_handle(); + ignore_sample = true; + for (size_t i = 0; i < 12; ++i) { + DDS_Octet * sender_element = &(sender_guid.value[i]); + DDS_Octet * receiver_element = &(reinterpret_cast(&receiver_instance_handle)[i]); + if (*sender_element != *receiver_element) { + ignore_sample = false; + break; + } + } + } + if (sample_info.valid_data && sending_publication_handle) { + *static_cast(sending_publication_handle) = + sample_info.publication_handle; + } + + if (!ignore_sample) { + cdr_stream->message_length = dds_messages[0].serialized_data.length(); + // TODO(karsten1987): This malloc has to go! + cdr_stream->raw_message = (char *) malloc(cdr_stream->message_length * sizeof(char)); + for (unsigned int i = 0; i < cdr_stream->message_length; ++i) { + cdr_stream->raw_message[i] = dds_messages[0].serialized_data[i]; + } + *taken = true; + } else { + *taken = false; + } + + data_reader->return_loan(dds_messages, sample_infos); + + return status == DDS_RETCODE_OK; +} + extern "C" { @@ -70,7 +162,7 @@ _take( // fetch the incoming message as cdr stream ConnextStaticCDRStream cdr_stream; - if (!callbacks->take( + if (!take( topic_reader, subscriber_info->ignore_local_publications, &cdr_stream, taken, sending_publication_handle)) { @@ -163,7 +255,7 @@ _take_raw( // fetch the incoming message as cdr stream ConnextStaticCDRStream cdr_stream; - if (!callbacks->take( + if (!take( topic_reader, subscriber_info->ignore_local_publications, &cdr_stream, taken, sending_publication_handle)) { diff --git a/rosidl_typesupport_connext_c/CMakeLists.txt b/rosidl_typesupport_connext_c/CMakeLists.txt index bb7b4b92..c728fdc7 100644 --- a/rosidl_typesupport_connext_c/CMakeLists.txt +++ b/rosidl_typesupport_connext_c/CMakeLists.txt @@ -45,13 +45,22 @@ ament_export_include_directories(include) ament_python_install_package(${PROJECT_NAME}) -add_library(${PROJECT_NAME} SHARED src/identifier.cpp) +add_library( + ${PROJECT_NAME} + SHARED + src/connext_static_raw_data.c + src/connext_static_raw_data_plugin.c + src/connext_static_raw_data_support.c + src/identifier.cpp) if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSIDL_TYPESUPPORT_CONNEXT_C_BUILDING_DLL") endif() target_include_directories(${PROJECT_NAME} PUBLIC include) -ament_target_dependencies(${PROJECT_NAME} "rosidl_typesupport_connext_cpp") +ament_target_dependencies( + ${PROJECT_NAME} + "Connext" + "rosidl_typesupport_connext_cpp") ament_export_libraries(${PROJECT_NAME}) ament_index_register_resource("rosidl_typesupport_c") diff --git a/rosidl_typesupport_connext_c/SerializedType.idl b/rosidl_typesupport_connext_c/SerializedType.idl new file mode 100644 index 00000000..89ea3650 --- /dev/null +++ b/rosidl_typesupport_connext_c/SerializedType.idl @@ -0,0 +1,7 @@ +const long KEY_HASH_LENGTH_16 = 16; + +struct SerializedType { + @key octet key_hash[KEY_HASH_LENGTH_16]; + sequence serialized_key; + sequence serialized_data; +}; \ No newline at end of file diff --git a/rosidl_typesupport_connext_c/SerializedType_publisher.c b/rosidl_typesupport_connext_c/SerializedType_publisher.c new file mode 100644 index 00000000..2aec32ca --- /dev/null +++ b/rosidl_typesupport_connext_c/SerializedType_publisher.c @@ -0,0 +1,274 @@ + +/**/ + +#include +#include +#include "ndds/ndds_c.h" +#include "SerializedType.h" +#include "SerializedTypeSupport.h" + +//#include "ShapeType.h" +//#include "ShapeTypePlugin.h" +#include "DynamicArrayPrimitives_.h" +#include "DynamicArrayPrimitives_Plugin.h" +#include "DynamicArrayPrimitives_Support.h" + +/* Delete all entities */ +static int publisher_shutdown( + DDS_DomainParticipant *participant) +{ + DDS_ReturnCode_t retcode; + int status = 0; + + if (participant != NULL) { + retcode = DDS_DomainParticipant_delete_contained_entities(participant); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "delete_contained_entities error %d\n", retcode); + status = -1; + } + + retcode = DDS_DomainParticipantFactory_delete_participant( + DDS_TheParticipantFactory, participant); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "delete_participant error %d\n", retcode); + status = -1; + } + } + + /* RTI Data Distribution Service provides finalize_instance() method on + domain participant factory for people who want to release memory used + by the participant factory. Uncomment the following block of code for + clean destruction of the singleton. */ + /* + retcode = DDS_DomainParticipantFactory_finalize_instance(); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "finalize_instance error %d\n", retcode); + status = -1; + } + */ + + return status; +} + +void strcpy_s(char * str, int lenMax, const char * strToCopy) { + if (!str || !strToCopy) { + return; + } + int strToCopyLen = strlen(strToCopy) + 1; + if(strToCopyLen < lenMax) { + lenMax = strToCopyLen; + } + strncpy(str, strToCopy, lenMax); + str[lenMax - 1] = '\0'; +} + +static int publisher_main(int domainId, int sample_count) +{ + DDS_DomainParticipant *participant = NULL; + DDS_Publisher *publisher = NULL; + DDS_Topic *topic = NULL; + DDS_DataWriter *writer = NULL; + SerializedTypeDataWriter *SerializedType_writer = NULL; + SerializedType *instance = NULL; + DDS_ReturnCode_t retcode; + DDS_InstanceHandle_t instance_handle = DDS_HANDLE_NIL; + int count = 0, i = 0; + struct DDS_Duration_t send_period = {1,0}; + + /* To customize participant QoS, use + the configuration file USER_QOS_PROFILES.xml */ + participant = DDS_DomainParticipantFactory_create_participant( + DDS_TheParticipantFactory, domainId, &DDS_PARTICIPANT_QOS_DEFAULT, + NULL /* listener */, DDS_STATUS_MASK_NONE); + if (participant == NULL) { + fprintf(stderr, "create_participant error\n"); + publisher_shutdown(participant); + return -1; + } + + /* To customize publisher QoS, use + the configuration file USER_QOS_PROFILES.xml */ + publisher = DDS_DomainParticipant_create_publisher( + participant, &DDS_PUBLISHER_QOS_DEFAULT, NULL /* listener */, + DDS_STATUS_MASK_NONE); + if (publisher == NULL) { + fprintf(stderr, "create_publisher error\n"); + publisher_shutdown(participant); + return -1; + } + + /* When regiatering the Data-Type with DDS it is important to use + the correct type code which corresponds to the serilized data-format + otherwise the type would be seen as incompatible by others and tools + like visualization will not work. + + Here since the serialized data corresponds to a ShapeType we + use the ShapeType_get_typecode() + */ + const char * type_name = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport_get_type_name(); + struct DDS_TypeCode * type_code = test_msgs_msg_dds__DynamicArrayPrimitives__get_typecode(); + retcode = SerializedTypeTypeSupport_register_external_type( + participant, type_name, type_code); + + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "register_type error %d\n", retcode); + publisher_shutdown(participant); + return -1; + } + + /* To customize topic QoS, use + the configuration file USER_QOS_PROFILES.xml */ + topic = DDS_DomainParticipant_create_topic( + participant, "my_generic_type", + type_name, &DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, + DDS_STATUS_MASK_NONE); + if (topic == NULL) { + fprintf(stderr, "create_topic error\n"); + publisher_shutdown(participant); + return -1; + } + + /* To customize data writer QoS, use + the configuration file USER_QOS_PROFILES.xml */ + writer = DDS_Publisher_create_datawriter( + publisher, topic, + &DDS_DATAWRITER_QOS_DEFAULT, NULL /* listener */, DDS_STATUS_MASK_NONE); + if (writer == NULL) { + fprintf(stderr, "create_datawriter error\n"); + publisher_shutdown(participant); + return -1; + } + SerializedType_writer = SerializedTypeDataWriter_narrow(writer); + if (SerializedType_writer == NULL) { + fprintf(stderr, "DataWriter narrow error\n"); + publisher_shutdown(participant); + return -1; + } + + /* Create data sample for writing */ + instance = SerializedTypeTypeSupport_create_data_ex(DDS_BOOLEAN_TRUE); + if (instance == NULL) { + fprintf(stderr, "SerializedTypeTypeSupport_create_data error\n"); + publisher_shutdown(participant); + return -1; + } +// /* We take advantage of the ShapeTypePlugin to serialize the data +// * At the application layer +// */ +// ShapeType shapeType; +// ShapeType_initialize(&shapeType); +// +//#define NUMBER_OF_COLORS (4) +// char *colors[NUMBER_OF_COLORS] = { "GREEN", "RED", "BLUE", "YELLOW" }; +// int xbase[NUMBER_OF_COLORS] = { 10, 50, 100, 150 }; +// int ybase[NUMBER_OF_COLORS] = { 0, 0, 0, 0 }; +// +// /* Memory area where to put the serialized (ShapeType) data */ +// unsigned int serializationLength; +// unsigned int serializationBufferSize = ShapeTypePlugin_get_serialized_sample_max_size(NULL, RTI_TRUE, 0, 0); +// +// /* RTI_CDR_MAX_SERIALIZED_SIZE indites the type is unbounded normally the application +// would have some knwledge of the size. Here we print an error in this situation */ +// if (serializationBufferSize == RTI_CDR_MAX_SERIALIZED_SIZE) { +// fprintf(stderr, "Type is unbounded. Please enter buffer size manually here\n"); +// return publisher_shutdown(participant); +// } + + test_msgs_msg_dds__DynamicArrayPrimitives_ dynamic_array_instance; + test_msgs_msg_dds__DynamicArrayPrimitives__initialize(&dynamic_array_instance); + DDS_LongSeq_initialize(&dynamic_array_instance.int32_values_); + size_t length = 100; + DDS_LongSeq_set_length(&dynamic_array_instance.int32_values_, length); + for (size_t i = 0; i < length; ++i) { + DDS_Long * element = DDS_LongSeq_get_reference(&dynamic_array_instance.int32_values_, i); + *element = i; + } + + unsigned int serializationLength; + unsigned int serializationBufferSize = test_msgs_msg_dds__DynamicArrayPrimitives_Plugin_get_serialized_sample_max_size(NULL, RTI_TRUE, 0, 0); + + DDS_Octet *serializationBuffer = (DDS_Octet *)malloc(serializationBufferSize); + + for (count = 0; (sample_count == 0) || (count < sample_count); ++count) { + + printf("Writing DynamicAraryPrimitives, count %d\n", count); + + /* Modify the data to be written here */ + + /* Use ShapeTypePlugin to serialize into an application buffer. + Serialization includes the 4-byte SerializationHeader. + + Note: serializationLength on input it is the maximum size. + On successful output it is the number of bytes used for + the serialization + */ + serializationLength = serializationBufferSize; + if (!test_msgs_msg_dds__DynamicArrayPrimitives_Plugin_serialize_to_cdr_buffer((char *)serializationBuffer, &serializationLength, &dynamic_array_instance)) { + fprintf(stderr, "Serialization of ShapeType failed\n"); + } + else { + /* At this point: + serializationBuffer - contains the serialized shapeType + serializationLength - contains the number of bytes in serializationBuffer used by the serialization + */ + /* Use DDS_OctetSeq_loan_contiguous() instead of DDS_OctetSeq_copy() to save one copy */ + DDS_OctetSeq_loan_contiguous(&(instance->serialized_data), serializationBuffer, + serializationLength, serializationBufferSize); + + /* TODO: Use ShapeType_serialize_key */ +// for (i = 0; i < 16; ++i) { +// instance->key_hash[i] = (char)(count % NUMBER_OF_COLORS); +// } + + retcode = SerializedTypeDataWriter_write( + SerializedType_writer, instance, &instance_handle); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "write error %d\n", retcode); + } + DDS_OctetSeq_unloan(&(instance->serialized_data)); + } + + NDDS_Utility_sleep(&send_period); + } + + /* + retcode = SerializedTypeDataWriter_unregister_instance( + SerializedType_writer, instance, &instance_handle); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "unregister instance error %d\n", retcode); + } + */ + + /* Delete data sample */ + retcode = SerializedTypeTypeSupport_delete_data_ex(instance, DDS_BOOLEAN_TRUE); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "SerializedTypeTypeSupport_delete_data error %d\n", retcode); + } + + /* Cleanup and delete delete all entities */ + free(serializationBuffer); + return publisher_shutdown(participant); +} + +int main(int argc, char *argv[]) +{ + int domainId = 0; + int sample_count = 0; /* infinite loop */ + + if (argc >= 2) { + domainId = atoi(argv[1]); + } + if (argc >= 3) { + sample_count = atoi(argv[2]); + } + + /* Uncomment this to turn on additional logging + NDDS_Config_Logger_set_verbosity_by_category( + NDDS_Config_Logger_get_instance(), + NDDS_CONFIG_LOG_CATEGORY_API, + NDDS_CONFIG_LOG_VERBOSITY_STATUS_ALL); + */ + + return publisher_main(domainId, sample_count); +} + diff --git a/rosidl_typesupport_connext_c/SerializedType_subscriber.c b/rosidl_typesupport_connext_c/SerializedType_subscriber.c new file mode 100644 index 00000000..ca550092 --- /dev/null +++ b/rosidl_typesupport_connext_c/SerializedType_subscriber.c @@ -0,0 +1,263 @@ + +/* +*/ + +#include +#include +#include "ndds/ndds_c.h" +#include "SerializedType.h" +#include "SerializedTypeSupport.h" + +#include "DynamicArrayPrimitives_.h" +#include "DynamicArrayPrimitives_Support.h" + +void SerializedTypeListener_on_requested_deadline_missed( + void* listener_data, + DDS_DataReader* reader, + const struct DDS_RequestedDeadlineMissedStatus *status) +{ +} + +void SerializedTypeListener_on_requested_incompatible_qos( + void* listener_data, + DDS_DataReader* reader, + const struct DDS_RequestedIncompatibleQosStatus *status) +{ +} + +void SerializedTypeListener_on_sample_rejected( + void* listener_data, + DDS_DataReader* reader, + const struct DDS_SampleRejectedStatus *status) +{ +} + +void SerializedTypeListener_on_liveliness_changed( + void* listener_data, + DDS_DataReader* reader, + const struct DDS_LivelinessChangedStatus *status) +{ +} + +void SerializedTypeListener_on_sample_lost( + void* listener_data, + DDS_DataReader* reader, + const struct DDS_SampleLostStatus *status) +{ +} + +void SerializedTypeListener_on_subscription_matched( + void* listener_data, + DDS_DataReader* reader, + const struct DDS_SubscriptionMatchedStatus *status) +{ +} + +void SerializedTypeListener_on_data_available( + void* listener_data, + DDS_DataReader* reader) +{ + SerializedTypeDataReader *SerializedType_reader = NULL; + struct SerializedTypeSeq data_seq = DDS_SEQUENCE_INITIALIZER; + struct DDS_SampleInfoSeq info_seq = DDS_SEQUENCE_INITIALIZER; + struct DDS_SampleInfo *sample_info; + + DDS_ReturnCode_t retcode; + int i; + + SerializedType_reader = SerializedTypeDataReader_narrow(reader); + if (SerializedType_reader == NULL) { + fprintf(stderr, "DataReader narrow error\n"); + return; + } + + retcode = SerializedTypeDataReader_take( + SerializedType_reader, + &data_seq, &info_seq, DDS_LENGTH_UNLIMITED, + DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE); + if (retcode == DDS_RETCODE_NO_DATA) { + return; + } else if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "take error %d\n", retcode); + return; + } + + for (i = 0; i < SerializedTypeSeq_get_length(&data_seq); ++i) { + sample_info = DDS_SampleInfoSeq_get_reference(&info_seq, i); + if (sample_info->valid_data) { + SerializedType *serializedType = SerializedTypeSeq_get_reference(&data_seq, i); + + /* Print information as a serialized buffer */ + printf("\nSerializedTypeSeq (as bytes):"); + SerializedTypeTypeSupport_print_data(serializedType); + + /* Pretty print it using the TypeCode if available*/ + //SerializedTypeTypeSupport_print_data2(serializedType, stdout, "SerializedTypeSeq (as ShapeType)", 0, ShapeType_get_typecode()); + } + } + + retcode = SerializedTypeDataReader_return_loan( + SerializedType_reader, + &data_seq, &info_seq); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "return loan error %d\n", retcode); + } +} + +/* Delete all entities */ +static int subscriber_shutdown( + DDS_DomainParticipant *participant) +{ + DDS_ReturnCode_t retcode; + int status = 0; + + if (participant != NULL) { + retcode = DDS_DomainParticipant_delete_contained_entities(participant); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "delete_contained_entities error %d\n", retcode); + status = -1; + } + + retcode = DDS_DomainParticipantFactory_delete_participant( + DDS_TheParticipantFactory, participant); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "delete_participant error %d\n", retcode); + status = -1; + } + } + + /* RTI Data Distribution Service provides the finalize_instance() method on + domain participant factory for users who want to release memory used + by the participant factory. Uncomment the following block of code for + clean destruction of the singleton. */ + /* + retcode = DDS_DomainParticipantFactory_finalize_instance(); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "finalize_instance error %d\n", retcode); + status = -1; + } + */ + + return status; +} + +static int subscriber_main(int domainId, int sample_count) +{ + DDS_DomainParticipant *participant = NULL; + DDS_Subscriber *subscriber = NULL; + DDS_Topic *topic = NULL; + struct DDS_DataReaderListener reader_listener = + DDS_DataReaderListener_INITIALIZER; + DDS_DataReader *reader = NULL; + DDS_ReturnCode_t retcode; + int count = 0; + struct DDS_Duration_t poll_period = {4,0}; + + /* To customize participant QoS, use + the configuration file USER_QOS_PROFILES.xml */ + participant = DDS_DomainParticipantFactory_create_participant( + DDS_TheParticipantFactory, domainId, &DDS_PARTICIPANT_QOS_DEFAULT, + NULL /* listener */, DDS_STATUS_MASK_NONE); + if (participant == NULL) { + fprintf(stderr, "create_participant error\n"); + subscriber_shutdown(participant); + return -1; + } + + /* To customize subscriber QoS, use + the configuration file USER_QOS_PROFILES.xml */ + subscriber = DDS_DomainParticipant_create_subscriber( + participant, &DDS_SUBSCRIBER_QOS_DEFAULT, NULL /* listener */, + DDS_STATUS_MASK_NONE); + if (subscriber == NULL) { + fprintf(stderr, "create_subscriber error\n"); + subscriber_shutdown(participant); + return -1; + } + + /* Register the type before creating the topic */ + retcode = SerializedTypeTypeSupport_register_external_type( + participant, + test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport_get_type_name(), + test_msgs_msg_dds__DynamicArrayPrimitives__get_typecode()); + + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "register_type error %d\n", retcode); + subscriber_shutdown(participant); + return -1; + } + + /* To customize topic QoS, use + the configuration file USER_QOS_PROFILES.xml */ + topic = DDS_DomainParticipant_create_topic( + participant, "my_generic_type", + test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport_get_type_name(), + &DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, + DDS_STATUS_MASK_NONE); + if (topic == NULL) { + fprintf(stderr, "create_topic error\n"); + subscriber_shutdown(participant); + return -1; + } + + /* Set up a data reader listener */ + reader_listener.on_requested_deadline_missed = + SerializedTypeListener_on_requested_deadline_missed; + reader_listener.on_requested_incompatible_qos = + SerializedTypeListener_on_requested_incompatible_qos; + reader_listener.on_sample_rejected = + SerializedTypeListener_on_sample_rejected; + reader_listener.on_liveliness_changed = + SerializedTypeListener_on_liveliness_changed; + reader_listener.on_sample_lost = + SerializedTypeListener_on_sample_lost; + reader_listener.on_subscription_matched = + SerializedTypeListener_on_subscription_matched; + reader_listener.on_data_available = + SerializedTypeListener_on_data_available; + + /* To customize data reader QoS, use + the configuration file USER_QOS_PROFILES.xml */ + reader = DDS_Subscriber_create_datareader( + subscriber, DDS_Topic_as_topicdescription(topic), + &DDS_DATAREADER_QOS_DEFAULT, &reader_listener, DDS_STATUS_MASK_ALL); + if (reader == NULL) { + fprintf(stderr, "create_datareader error\n"); + subscriber_shutdown(participant); + return -1; + } + + /* Main loop */ + for (count=0; (sample_count == 0) || (count < sample_count); ++count) { + printf("SerializedType subscriber sleeping for %d sec...\n", + poll_period.sec); + + NDDS_Utility_sleep(&poll_period); + } + + /* Cleanup and delete all entities */ + return subscriber_shutdown(participant); +} + +int main(int argc, char *argv[]) +{ + int domainId = 0; + int sample_count = 0; /* infinite loop */ + + if (argc >= 2) { + domainId = atoi(argv[1]); + } + if (argc >= 3) { + sample_count = atoi(argv[2]); + } + + /* Uncomment this to turn on additional logging + NDDS_Config_Logger_set_verbosity_by_category( + NDDS_Config_Logger_get_instance(), + NDDS_CONFIG_LOG_CATEGORY_API, + NDDS_CONFIG_LOG_VERBOSITY_STATUS_ALL); + */ + + return subscriber_main(domainId, sample_count); +} + diff --git a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data.h b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data.h new file mode 100644 index 00000000..5ae09454 --- /dev/null +++ b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data.h @@ -0,0 +1,88 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef ConnextStaticRawData_1419555957_h +#define ConnextStaticRawData_1419555957_h + +#ifndef NDDS_STANDALONE_TYPE +#ifndef ndds_c_h +#include "rosidl_typesupport_connext_c/ndds_include.h" +#endif +#else +#include "ndds_standalone_type.h" +#endif + +#define KEY_HASH_LENGTH_16 (16) + +extern const char *ConnextStaticRawDataTYPENAME; + +typedef struct ConnextStaticRawData { + + DDS_Octet key_hash [(KEY_HASH_LENGTH_16)]; + struct DDS_OctetSeq serialized_key ; + struct DDS_OctetSeq serialized_data ; + +} ConnextStaticRawData ; +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) +#endif + +NDDSUSERDllExport DDS_TypeCode* ConnextStaticRawData_get_typecode(void); /* Type code */ + +DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize( + ConnextStaticRawData* self); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize_ex( + ConnextStaticRawData* self,RTIBool allocatePointers,RTIBool allocateMemory); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize_w_params( + ConnextStaticRawData* self, + const struct DDS_TypeAllocationParams_t * allocParams); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize( + ConnextStaticRawData* self); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_ex( + ConnextStaticRawData* self,RTIBool deletePointers); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_w_params( + ConnextStaticRawData* self, + const struct DDS_TypeDeallocationParams_t * deallocParams); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_optional_members( + ConnextStaticRawData* self, RTIBool deletePointers); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_copy( + ConnextStaticRawData* dst, + const ConnextStaticRawData* src); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* ConnextStaticRawData */ + diff --git a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h new file mode 100644 index 00000000..db05cbfa --- /dev/null +++ b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h @@ -0,0 +1,339 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef ConnextStaticRawDataPlugin_1419555957_h +#define ConnextStaticRawDataPlugin_1419555957_h + +#include "rosidl_typesupport_connext_c/connext_static_raw_data.h" + +struct RTICdrStream; + +#ifndef pres_typePlugin_h +#include "pres/pres_typePlugin.h" +#endif + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) +#endif + +/* The type used to store keys for instances of type struct +* AnotherSimple. +* +* By default, this type is struct ConnextStaticRawData +* itself. However, if for some reason this choice is not practical for your +* system (e.g. if sizeof(struct ConnextStaticRawData) +* is very large), you may redefine this typedef in terms of another type of +* your choosing. HOWEVER, if you define the KeyHolder type to be something +* other than struct AnotherSimple, the +* following restriction applies: the key of struct +* ConnextStaticRawData must consist of a +* single field of your redefined KeyHolder type and that field must be the +* first field in struct ConnextStaticRawData. +*/ +typedef struct ConnextStaticRawData ConnextStaticRawDataKeyHolder; + +#define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample +#define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer +#define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer + +#define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey +#define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey + +#define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample +#define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample + +/* -------------------------------------------------------------------------------------- +Support functions: +* -------------------------------------------------------------------------------------- */ + +NDDSUSERDllExport extern ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params); + +NDDSUSERDllExport extern ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); + +NDDSUSERDllExport extern ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_data(void); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData *out, + const ConnextStaticRawData *in); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData *sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData *sample,RTIBool deallocate_pointers); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData *sample); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData *sample, + const char *desc, + unsigned int indent); + +NDDSUSERDllExport extern ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); + +NDDSUSERDllExport extern ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_key(void); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder *key); + +/* ---------------------------------------------------------------------------- +Callback functions: +* ---------------------------------------------------------------------------- */ + +NDDSUSERDllExport extern PRESTypePluginParticipantData +ConnextStaticRawDataPlugin_on_participant_attached( + void *registration_data, + const struct PRESTypePluginParticipantInfo *participant_info, + RTIBool top_level_registration, + void *container_plugin_context, + RTICdrTypeCode *typeCode); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data); + +NDDSUSERDllExport extern PRESTypePluginEndpointData +ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo *endpoint_info, + RTIBool top_level_registration, + void *container_plugin_context); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + void *handle); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *out, + const ConnextStaticRawData *in); + +/* ---------------------------------------------------------------------------- +(De)Serialize functions: +* ------------------------------------------------------------------------- */ + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void *endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData *sample); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData *sample, + const char * buffer, + unsigned int length); +NDDSUSERDllExport extern DDS_ReturnCode_t +ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void *endpoint_plugin_qos); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample); + +/* -------------------------------------------------------------------------------------- +Key Management functions: +* -------------------------------------------------------------------------------------- */ +NDDSUSERDllExport extern PRESTypePluginKeyKind +ConnextStaticRawDataPlugin_get_key_kind(void); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void *endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData ** sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder *key, + const ConnextStaticRawData *instance); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *instance, + const ConnextStaticRawDataKeyHolder *key); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t *keyhash, + const ConnextStaticRawData *instance); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + DDS_KeyHash_t *keyhash, + RTIBool deserialize_encapsulation, + void *endpoint_plugin_qos); + +/* Plugin Functions */ +NDDSUSERDllExport extern struct PRESTypePlugin* +ConnextStaticRawDataPlugin_new(void); + +NDDSUSERDllExport extern struct PRESTypePlugin* +ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * type_code); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* ConnextStaticRawDataPlugin_1419555957_h */ + diff --git a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_support.h b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_support.h new file mode 100644 index 00000000..bcd112d6 --- /dev/null +++ b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_support.h @@ -0,0 +1,61 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef ConnextStaticRawDataSupport_1419555957_h +#define ConnextStaticRawDataSupport_1419555957_h + +/* Uses */ +#include "rosidl_typesupport_connext_c/connext_static_raw_data.h" + +#ifndef ndds_c_h +#include "rosidl_typesupport_connext_c/ndds_include.h" +#endif + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) + +#endif + +/* ========================================================================= */ +/** +Uses: T + +Defines: TTypeSupport, TDataWriter, TDataReader + +Organized using the well-documented "Generics Pattern" for +implementing generics in C and C++. +*/ + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) + +#endif + +DDS_TYPESUPPORT_C(ConnextStaticRawDataTypeSupport, ConnextStaticRawData); +DDS_DATAWRITER_C(ConnextStaticRawDataDataWriter, ConnextStaticRawData); +DDS_DATAREADER_C(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); + +NDDSUSERDllExport DDSCDllExport +DDS_ReturnCode_t ConnextStaticRawDataTypeSupport_register_external_type( + DDS_DomainParticipant* participant, + const char* type_name, + struct DDS_TypeCode *type_code); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* ConnextStaticRawDataSupport_1419555957_h */ + diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/ndds_include.hpp b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/ndds_include.h similarity index 68% rename from rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/ndds_include.hpp rename to rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/ndds_include.h index a02d0e93..e6892070 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/ndds_include.hpp +++ b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/ndds_include.h @@ -12,21 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_HPP_ -#define ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_HPP_ +#ifndef ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_H_ +#define ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_H_ #ifndef _WIN32 # pragma GCC diagnostic push # pragma GCC diagnostic ignored "-Wunused-parameter" # ifdef __clang__ -# pragma clang diagnostic ignored "-Wdeprecated-register" -# pragma clang diagnostic ignored "-Wreturn-type-c-linkage" +# pragma clang diagnostic ignored "-Wpedantic" # endif #endif -#include "ndds/ndds_cpp.h" -#include "ndds/ndds_requestreply_cpp.h" +#include "ndds/ndds_c.h" +//#include "ndds/ndds_requestreply_cpp.h" #ifndef _WIN32 # pragma GCC diagnostic pop #endif -#endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_HPP_ +#endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_H_ diff --git a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em index 5dcc22d8..533a7e8c 100644 --- a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em +++ b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em @@ -107,44 +107,10 @@ __dds_msg_type_prefix = "{0}::{1}::dds_::{2}_".format( using __dds_msg_type = @(__dds_msg_type_prefix); using __ros_msg_type = @(pkg)__@(subfolder)__@(type); -static bool -register_type(void * untyped_participant, const char * type_name) +static DDS_TypeCode * +get_type_code() { - if (!untyped_participant) { - fprintf(stderr, "untyped participant handle is null\n"); - return false; - } - if (!type_name) { - fprintf(stderr, "type name handle is null\n"); - return false; - } - DDSDomainParticipant * participant = static_cast(untyped_participant); - - DDS_ReturnCode_t status = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::register_type(participant, type_name); - switch (status) { - case DDS_RETCODE_ERROR: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::register_type: " - "an internal error has occurred\n"); - return false; - case DDS_RETCODE_BAD_PARAMETER: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::register_type: " - "bad domain participant or type name parameter\n"); - return false; - case DDS_RETCODE_OUT_OF_RESOURCES: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::register_type: " - "out of resources\n"); - return false; - case DDS_RETCODE_PRECONDITION_NOT_MET: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::register_type: " - "already registered with a different TypeSupport class\n"); - return false; - case DDS_RETCODE_OK: - return true; - default: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::register_type: unknown return code\n"); - } - return false; + return @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::get_typecode(); } static bool @@ -256,73 +222,6 @@ convert_ros_to_dds(const void * untyped_ros_message, void * untyped_dds_message) return true; } -static bool -publish(void * dds_data_writer, ConnextStaticCDRStream * untyped_ros_messageFIX) -{ - void *untyped_ros_message = (void *) untyped_ros_messageFIX; - if (!dds_data_writer) { - fprintf(stderr, "data writer handle is null\n"); - return false; - } - const __ros_msg_type * ros_message = static_cast(untyped_ros_message); - if (!ros_message) { - fprintf(stderr, "ros message handle is null\n"); - return false; - } - - DDSDataWriter * topic_writer = static_cast(dds_data_writer); - - __dds_msg_type dds_message; - if (!convert_ros_to_dds(ros_message, &dds_message)) { - return false; - } - @(pkg)::@(subfolder)::dds_::@(type)_DataWriter * data_writer = - @(pkg)::@(subfolder)::dds_::@(type)_DataWriter::narrow(topic_writer); - if (!data_writer) { - fprintf(stderr, "failed to narrow data writer\n"); - return false; - } - DDS_ReturnCode_t status = data_writer->write(dds_message, DDS_HANDLE_NIL); - - switch (status) { - case DDS_RETCODE_ERROR: - fprintf(stderr, "@(pkg)::@(subfolder)::dds_::@(type)_DataWriter.write: " - "an internal error has occurred\n"); - return false; - case DDS_RETCODE_BAD_PARAMETER: - fprintf(stderr, "@(pkg)::@(subfolder)::dds_::@(type)_DataWriter.write: " - "bad handle or instance_data parameter\n"); - return false; - case DDS_RETCODE_ALREADY_DELETED: - fprintf(stderr, "@(pkg)::@(subfolder)::dds_::@(type)_DataWriter.write: " - "this @(__dds_msg_type_prefix)DataWriter has already been deleted\n"); - return false; - case DDS_RETCODE_OUT_OF_RESOURCES: - fprintf(stderr, "@(pkg)::@(subfolder)::dds_::@(type)_DataWriter.write: " - "out of resources\n"); - return false; - case DDS_RETCODE_NOT_ENABLED: - fprintf(stderr, "@(pkg)::@(subfolder)::dds_::@(type)_DataWriter.write: " - "this @(__dds_msg_type_prefix)DataWriter is not enabled\n"); - return false; - case DDS_RETCODE_PRECONDITION_NOT_MET: - fprintf(stderr, "@(pkg)::@(subfolder)::dds_::@(type)_DataWriter.write: " - "the handle has not been registered with this @(__dds_msg_type_prefix)DataWriter\n"); - return false; - case DDS_RETCODE_TIMEOUT: - fprintf(stderr, "@(pkg)::@(subfolder)::dds_::@(type)_DataWriter.write: " - "writing resulted in blocking and then exceeded the timeout set by the " - "max_blocking_time of the ReliabilityQosPolicy\n"); - return false; - case DDS_RETCODE_OK: - return true; - default: - fprintf(stderr, "@(pkg)::@(subfolder)::dds_::@(type)_DataWriter.write: " - "unknown return code\n"); - } - return false; -} - static bool convert_dds_to_ros(const void * untyped_dds_message, void * untyped_ros_message) { @@ -422,148 +321,6 @@ else: return true; } -static bool -take( - void * dds_data_reader, - bool ignore_local_publications, - ConnextStaticCDRStream * cdr_streamFIX, - bool * taken, - void * sending_publication_handle) -{ - void * untyped_ros_message = reinterpret_cast(cdr_streamFIX); - if (untyped_ros_message == 0) { - fprintf(stderr, "invalid ros message pointer\n"); - return false; - } - - DDSDataReader * topic_reader = static_cast(dds_data_reader); - - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader * data_reader = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader::narrow(topic_reader); - if (!data_reader) { - fprintf(stderr, "failed to narrow data reader\n"); - return false; - } - - @(__dds_msg_type_prefix)Seq dds_messages; - DDS_SampleInfoSeq sample_infos; - DDS_ReturnCode_t status = data_reader->take( - dds_messages, - sample_infos, - 1, - DDS_ANY_SAMPLE_STATE, - DDS_ANY_VIEW_STATE, - DDS_ANY_INSTANCE_STATE); - - bool ignore_sample = false; - - switch (status) { - case DDS_RETCODE_ERROR: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.take: " - "an internal error has occurred\n"); - goto finally; - case DDS_RETCODE_ALREADY_DELETED: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.take: " - "this @(__dds_msg_type_prefix)DataReader has already been deleted\n"); - goto finally; - case DDS_RETCODE_OUT_OF_RESOURCES: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.take: " - "out of resources\n"); - goto finally; - case DDS_RETCODE_NOT_ENABLED: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.take: " - "this @(__dds_msg_type_prefix)DataReader is not enabled\n"); - goto finally; - case DDS_RETCODE_PRECONDITION_NOT_MET: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.take: " - "a precondition is not met, one of: " - "max_samples > maximum and max_samples != LENGTH_UNLIMITED, or " - "the two sequences do not have matching parameters (length, maximum, release), or " - "maximum > 0 and release is false.\n"); - goto finally; - case DDS_RETCODE_NO_DATA: - *taken = false; - goto finally; - case DDS_RETCODE_OK: - break; - default: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.take: " - "unknown return code\n"); - goto finally; - } - - { - DDS_SampleInfo & sample_info = sample_infos[0]; - if (!sample_info.valid_data) { - // skip sample without data - ignore_sample = true; - } else if (ignore_local_publications) { - // compare the lower 12 octets of the guids from the sender and this receiver - // if they are equal the sample has been sent from this process and should be ignored - DDS_GUID_t sender_guid = sample_info.original_publication_virtual_guid; - DDS_InstanceHandle_t receiver_instance_handle = topic_reader->get_instance_handle(); - ignore_sample = true; - for (size_t i = 0; i < 12; ++i) { - DDS_Octet * sender_element = &(sender_guid.value[i]); - DDS_Octet * receiver_element = &(reinterpret_cast(&receiver_instance_handle)[i]); - if (*sender_element != *receiver_element) { - ignore_sample = false; - break; - } - } - // This is nullptr when being used with plain rmw_take, so check first. - if (sending_publication_handle) { - *static_cast(sending_publication_handle) = - sample_info.publication_handle; - } - } - } - - if (!ignore_sample) { - if (!convert_dds_to_ros(&dds_messages[0], untyped_ros_message)) { - goto finally; - } - *taken = true; - } else { - *taken = false; - } - -finally: - // Ensure the loan is returned. - status = data_reader->return_loan(dds_messages, sample_infos); - switch (status) { - case DDS_RETCODE_ERROR: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.return_loan: " - "an internal error has occurred\n"); - return false; - case DDS_RETCODE_ALREADY_DELETED: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.return_loan: " - "this @(__dds_msg_type_prefix)DataReader has already been deleted\n"); - return false; - case DDS_RETCODE_OUT_OF_RESOURCES: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.return_loan: " - "out of resources\n"); - return false; - case DDS_RETCODE_NOT_ENABLED: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.return_loan: " - "this @(__dds_msg_type_prefix)DataReader is not enabled\n"); - return false; - case DDS_RETCODE_PRECONDITION_NOT_MET: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.return_loan: " - "a precondition is not met, one of: " - "the data_values and info_seq do not belong to a single related pair, or " - "the data_values and info_seq were not obtained from this " - "@(__dds_msg_type_prefix)DataReader\n"); - return false; - case DDS_RETCODE_OK: - return true; - default: - fprintf(stderr, "@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_DataReader.return_loan: " - "unknown return code\n"); - } - - return false; -} static bool to_cdr_stream( @@ -576,6 +333,25 @@ to_cdr_stream( if (!cdr_stream) { return false; } + const __ros_msg_type * ros_message = static_cast(untyped_ros_message); + __dds_msg_type dds_message; + if (!convert_ros_to_dds(ros_message, &dds_message)) { + return false; + } + + // call the serialize function for the first time to get the expected length of the message + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(NULL, &cdr_stream->message_length, &dds_message) != RTI_TRUE) { + return false; + } + // allocate enough memory for the CDR stream + // TODO(karsten1987): This allocation has to be preallocated + // or at least bring in a custom allocator + cdr_stream->raw_message = (char *)malloc(sizeof(char) * cdr_stream->message_length); + // call the function again and fill the buffer this time + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->raw_message, &cdr_stream->message_length, &dds_message) != RTI_TRUE) { + return false; + } + return true; } @@ -590,17 +366,26 @@ to_message( if (!untyped_ros_message) { return false; } - return true; + + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); + if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer(dds_message, cdr_stream->raw_message, cdr_stream->message_length) != RTI_TRUE) { + fprintf(stderr, "deserialize from cdr buffer failed\n"); + return false; + } + bool success = convert_dds_to_ros(dds_message, untyped_ros_message); + if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK){ + return false; + } + return success; } + @ @# // Collect the callback functions and provide a function to get the type support struct. static message_type_support_callbacks_t __callbacks = { "@(pkg)", // package_name "@(msg)", // message_name - register_type, // register_type - publish, // publish - take, // take + get_type_code, // get_type_code convert_ros_to_dds, // convert_ros_to_dds convert_dds_to_ros, // convert_dds_to_ros to_cdr_stream, // to_cdr_stream diff --git a/rosidl_typesupport_connext_c/src/connext_static_raw_data.c b/rosidl_typesupport_connext_c/src/connext_static_raw_data.c new file mode 100644 index 00000000..b3b1109e --- /dev/null +++ b/rosidl_typesupport_connext_c/src/connext_static_raw_data.c @@ -0,0 +1,302 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef NDDS_STANDALONE_TYPE +#ifndef ndds_c_h +#include "rosidl_typesupport_connext_c/ndds_include.h" +#endif + +#ifndef cdr_type_h +#include "cdr/cdr_type.h" +#endif + +#ifndef osapi_heap_h +#include "osapi/osapi_heap.h" +#endif +#else +#include "ndds_standalone_type.h" +#endif + +#include "rosidl_typesupport_connext_c/connext_static_raw_data.h" + +/* ========================================================================= */ +const char *ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; + +DDS_TypeCode* ConnextStaticRawData_get_typecode() +{ + static RTIBool is_initialized = RTI_FALSE; + + static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array =DDS_INITIALIZE_ARRAY_TYPECODE(1,(KEY_HASH_LENGTH_16), NULL,NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); + static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3]= + { + + { + (char *)"key_hash",/* Member name */ + { + 0,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_KEY_MEMBER , /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + }, + { + (char *)"serialized_key",/* Member name */ + { + 1,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + }, + { + (char *)"serialized_data",/* Member name */ + { + 2,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + } + }; + + static DDS_TypeCode ConnextStaticRawData_g_tc = + {{ + DDS_TK_STRUCT,/* Kind */ + DDS_BOOLEAN_FALSE, /* Ignored */ + -1, /*Ignored*/ + (char *)"ConnextStaticRawData", /* Name */ + NULL, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + 3, /* Number of members */ + ConnextStaticRawData_g_tc_members, /* Members */ + DDS_VM_NONE /* Ignored */ + }}; /* Type code for ConnextStaticRawData*/ + + if (is_initialized) { + return &ConnextStaticRawData_g_tc; + } + + ConnextStaticRawData_g_tc_key_hash_array._data._typeCode =(RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_members[0]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_key_hash_array; + ConnextStaticRawData_g_tc_members[1]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_key_sequence; + ConnextStaticRawData_g_tc_members[2]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_data_sequence; + + is_initialized = RTI_TRUE; + + return &ConnextStaticRawData_g_tc; +} + +RTIBool ConnextStaticRawData_initialize( + ConnextStaticRawData* sample) { + return ConnextStaticRawData_initialize_ex(sample,RTI_TRUE,RTI_TRUE); +} + +RTIBool ConnextStaticRawData_initialize_ex( + ConnextStaticRawData* sample,RTIBool allocatePointers, RTIBool allocateMemory) +{ + + struct DDS_TypeAllocationParams_t allocParams = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; + + allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; + allocParams.allocate_memory = (DDS_Boolean)allocateMemory; + + return ConnextStaticRawData_initialize_w_params( + sample,&allocParams); + +} + +RTIBool ConnextStaticRawData_initialize_w_params( + ConnextStaticRawData* sample, const struct DDS_TypeAllocationParams_t * allocParams) +{ + + void* buffer = NULL; + if (buffer) {} /* To avoid warnings */ + + if (sample == NULL) { + return RTI_FALSE; + } + if (allocParams == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_initArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_key ); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key , RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { + return RTI_FALSE; + } + } else { + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_data ); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data , RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { + return RTI_FALSE; + } + } else { + DDS_OctetSeq_set_length(&sample->serialized_data, 0); + } + return RTI_TRUE; +} + +void ConnextStaticRawData_finalize( + ConnextStaticRawData* sample) +{ + + ConnextStaticRawData_finalize_ex(sample,RTI_TRUE); +} + +void ConnextStaticRawData_finalize_ex( + ConnextStaticRawData* sample,RTIBool deletePointers) +{ + struct DDS_TypeDeallocationParams_t deallocParams = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; + + if (sample==NULL) { + return; + } + + deallocParams.delete_pointers = (DDS_Boolean)deletePointers; + + ConnextStaticRawData_finalize_w_params( + sample,&deallocParams); +} + +void ConnextStaticRawData_finalize_w_params( + ConnextStaticRawData* sample,const struct DDS_TypeDeallocationParams_t * deallocParams) +{ + + if (sample==NULL) { + return; + } + + if (deallocParams == NULL) { + return; + } + + DDS_OctetSeq_finalize(&sample->serialized_key); + + DDS_OctetSeq_finalize(&sample->serialized_data); + +} + +void ConnextStaticRawData_finalize_optional_members( + ConnextStaticRawData* sample, RTIBool deletePointers) +{ + struct DDS_TypeDeallocationParams_t deallocParamsTmp = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; + struct DDS_TypeDeallocationParams_t * deallocParams = + &deallocParamsTmp; + + if (sample==NULL) { + return; + } + if (deallocParams) {} /* To avoid warnings */ + + deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; + deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; + +} + +RTIBool ConnextStaticRawData_copy( + ConnextStaticRawData* dst, + const ConnextStaticRawData* src) +{ + + if (dst == NULL || src == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_key , + &src->serialized_key )) { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_data , + &src->serialized_data )) { + return RTI_FALSE; + } + + return RTI_TRUE; + +} + +/** +* <> +* +* Defines: TSeq, T +* +* Configure and implement 'ConnextStaticRawData' sequence class. +*/ +#define T ConnextStaticRawData +#define TSeq ConnextStaticRawDataSeq + +#define T_initialize_w_params ConnextStaticRawData_initialize_w_params + +#define T_finalize_w_params ConnextStaticRawData_finalize_w_params +#define T_copy ConnextStaticRawData_copy + +#ifndef NDDS_STANDALONE_TYPE +#include "dds_c/generic/dds_c_sequence_TSeq.gen" +#else +#include "dds_c_sequence_TSeq.gen" +#endif + +#undef T_copy +#undef T_finalize_w_params + +#undef T_initialize_w_params + +#undef TSeq +#undef T + diff --git a/rosidl_typesupport_connext_c/src/connext_static_raw_data_plugin.c b/rosidl_typesupport_connext_c/src/connext_static_raw_data_plugin.c new file mode 100644 index 00000000..ec1985ac --- /dev/null +++ b/rosidl_typesupport_connext_c/src/connext_static_raw_data_plugin.c @@ -0,0 +1,1527 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#include + +#ifndef ndds_c_h +#include "rosidl_typesupport_connext_c/ndds_include.h" +#endif + +#ifndef osapi_type_h +#include "osapi/osapi_type.h" +#endif +#ifndef osapi_heap_h +#include "osapi/osapi_heap.h" +#endif + +#ifndef osapi_utility_h +#include "osapi/osapi_utility.h" +#endif + +#ifndef cdr_type_h +#include "cdr/cdr_type.h" +#endif + +#ifndef cdr_type_object_h +#include "cdr/cdr_typeObject.h" +#endif + +#ifndef cdr_encapsulation_h +#include "cdr/cdr_encapsulation.h" +#endif + +#ifndef cdr_stream_h +#include "cdr/cdr_stream.h" +#endif + +#ifndef cdr_log_h +#include "cdr/cdr_log.h" +#endif + +#ifndef pres_typePlugin_h +#include "pres/pres_typePlugin.h" +#endif + +#define RTI_CDR_CURRENT_SUBMODULE RTI_CDR_SUBMODULE_MASK_STREAM + +#include "rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h" + +/* ---------------------------------------------------------------------------- +* Type ConnextStaticRawData +* -------------------------------------------------------------------------- */ + +/* ----------------------------------------------------------------------------- +Support functions: +* -------------------------------------------------------------------------- */ + +ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params) +{ + ConnextStaticRawData *sample = NULL; + + RTIOsapiHeap_allocateStructure(&(sample),ConnextStaticRawData); + if (sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_w_params(sample,alloc_params)) { + RTIOsapiHeap_freeStructure(sample); + sample=NULL; + } + return sample; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) +{ + ConnextStaticRawData *sample = NULL; + + RTIOsapiHeap_allocateStructure(&(sample),ConnextStaticRawData); + + if(sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_ex(sample,allocate_pointers, RTI_TRUE)) { + RTIOsapiHeap_freeStructure(sample); + sample=NULL; + } + + return sample; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data(void) +{ + return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); +} + +void +ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData *sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params) { + + ConnextStaticRawData_finalize_w_params(sample,dealloc_params); + + RTIOsapiHeap_freeStructure(sample); + sample=NULL; +} + +void +ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData *sample,RTIBool deallocate_pointers) { + + ConnextStaticRawData_finalize_ex(sample,deallocate_pointers); + + RTIOsapiHeap_freeStructure(sample); + sample=NULL; +} + +void +ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData *sample) { + + ConnextStaticRawDataPluginSupport_destroy_data_ex(sample,RTI_TRUE); + +} + +RTIBool +ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData *dst, + const ConnextStaticRawData *src) +{ + return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); +} + +void +ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData *sample, + const char *desc, + unsigned int indent_level) +{ + + RTICdrType_printIndent(indent_level); + + if (desc != NULL) { + RTILog_debug("%s:\n", desc); + } else { + RTILog_debug("\n"); + } + + if (sample == NULL) { + RTILog_debug("NULL\n"); + return; + } + + RTICdrType_printArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "key_hash", indent_level + 1); + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key ), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data ), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } + +} +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers){ + ConnextStaticRawData *key = NULL; + + RTIOsapiHeap_allocateStructure(&(key),ConnextStaticRawDataKeyHolder); + + ConnextStaticRawData_initialize_ex(key,allocate_pointers, RTI_TRUE); + + return key; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key(void) +{ + return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); +} + +void +ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) +{ + ConnextStaticRawData_finalize_ex(key,deallocate_pointers); + + RTIOsapiHeap_freeStructure(key); + key=NULL; + +} + +void +ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder *key) { + + ConnextStaticRawDataPluginSupport_destroy_key_ex(key,RTI_TRUE); + +} + +/* ---------------------------------------------------------------------------- +Callback functions: +* ---------------------------------------------------------------------------- */ + +PRESTypePluginParticipantData +ConnextStaticRawDataPlugin_on_participant_attached( + void *registration_data, + const struct PRESTypePluginParticipantInfo *participant_info, + RTIBool top_level_registration, + void *container_plugin_context, + RTICdrTypeCode *type_code) +{ + if (registration_data) {} /* To avoid warnings */ + if (participant_info) {} /* To avoid warnings */ + if (top_level_registration) {} /* To avoid warnings */ + if (container_plugin_context) {} /* To avoid warnings */ + if (type_code) {} /* To avoid warnings */ + + return PRESTypePluginDefaultParticipantData_new(participant_info); + +} + +void +ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data) +{ + + PRESTypePluginDefaultParticipantData_delete(participant_data); +} + +PRESTypePluginEndpointData +ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo *endpoint_info, + RTIBool top_level_registration, + void *containerPluginContext) +{ + PRESTypePluginEndpointData epd = NULL; + + unsigned int serializedSampleMaxSize; + + unsigned int serializedKeyMaxSize; + + if (top_level_registration) {} /* To avoid warnings */ + if (containerPluginContext) {} /* To avoid warnings */ + + epd = PRESTypePluginDefaultEndpointData_new( + participant_data, + endpoint_info, + (PRESTypePluginDefaultEndpointDataCreateSampleFunction) + ConnextStaticRawDataPluginSupport_create_data, + (PRESTypePluginDefaultEndpointDataDestroySampleFunction) + ConnextStaticRawDataPluginSupport_destroy_data, + (PRESTypePluginDefaultEndpointDataCreateKeyFunction) + ConnextStaticRawDataPluginSupport_create_key , + (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) + ConnextStaticRawDataPluginSupport_destroy_key); + + if (epd == NULL) { + return NULL; + } + serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( + epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); + + if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( + epd,endpoint_info,serializedKeyMaxSize)) + { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + + if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { + serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); + + PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); + + if (PRESTypePluginDefaultEndpointData_createWriterPool( + epd, + endpoint_info, + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size, + epd) == RTI_FALSE) { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + } + + return epd; +} + +void +ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data) +{ + + PRESTypePluginDefaultEndpointData_delete(endpoint_data); +} + +void +ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + void *handle) +{ + + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + + PRESTypePluginDefaultEndpointData_returnSample( + endpoint_data, sample, handle); +} + +RTIBool +ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *dst, + const ConnextStaticRawData *src) +{ + if (endpoint_data) {} /* To avoid warnings */ + return ConnextStaticRawDataPluginSupport_copy_data(dst,src); +} + +/* ---------------------------------------------------------------------------- +(De)Serialize functions: +* ------------------------------------------------------------------------- */ +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +/** + * This plugin can only be used as a top-level type. + * It expects the ConnextStaticRawData to contain data serialized with its + * encapsulation header already there. + * + * @param serialize_encapsulation normally it indicates whether encapsulaton should be serialized + * it also indicates whether we are serializing the data as a a top-level type (in the + * RTPSSerializedPayload or we are serializing as part of a nested type). + For ConnextStaticRawDataPlugin_serialize() it must be set to TRUE + * + * @param encapsulation_id the encapsulation identifier in case serialize_encapsulation==TRUE + * the implementation of ConnextStaticRawDataPlugin_serialize checks that the value of + * encapsulation_id matches the first two bytes in sample->key_hash + */ +RTIBool +ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + RTIBool retval = RTI_TRUE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + /* This plugin can only be used to publish the top-level DDS Topic-Type + * in which case serialize_encapsulation==TRUE. If that is not + * the case then it is an error. + */ + if (!serialize_encapsulation) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + + if(serialize_sample) { + /* The sample->serialized_data contains the serialized encapsulation followed by the serialized + * data, so we only need to copy that into + * the CDR stream. Not the key_hash, not the length of the data itself + * The ConnextStaticRawData sample->serialized_data is always a contiguous buffer + */ + DDS_Octet *buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); + if ( buffer == NULL ) { + return RTI_FALSE; + } + + /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian + * byte order + */ + if ( encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { + return RTI_FALSE; + } + + /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ + if (!RTICdrStream_serializePrimitiveArray( + stream, (void*)buffer, + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } + + RTICdrStream_restoreAlignment(stream ,position); + + return retval; +} + + +/** + TODO. The code-block below does not belong here. + It should be pushed to the CDR module, perhaps inside + RTICdrStream_deserializeAndSetCdrEncapsulation so that the + stream size is alredy correct when ConnextStaticRawDataPlugin_deserialize_sample + is called. + + Adjust the size of the CDR stream to not include the alignment + padding. See http://issues.omg.org/browse/DDSXTY12-10 + + @precondition The RTICdrStream *stream has alreadt processed + the encapsulation header and therefore has set the + encapsulation options returned by + RTICdrStream_getEncapsulationOptions() +*/ +void +ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream *stream) +{ + /* See http://issues.omg.org/browse/DDSXTY12-10 */ + DDS_UnsignedShort padding_size_mask = 0x0003; + DDS_UnsignedShort padding_size; + int adjustedBufferLength; + + padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; + adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; + RTICdrStream_setBufferLength(stream, adjustedBufferLength); +} + +/** +* This plugin can only be used as a top-level type. +* It expects the RTICdrStream to contain the encapsulatio header +* followed by the serialized data itself +*/ +RTIBool +ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + RTIBool done = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + /* This plugin can only be used to publish the top-level DDS Topic-Type + * in which case deserialize_encapsulation==TRUE. If that is not + * the case then it is an error. + */ + if (!deserialize_encapsulation) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + + /* TODO. The call does not belong here. It should be pushed + * inside RTICdrStream_deserializeAndSetCdrEncapsulation + */ + ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); + + if (deserialize_sample) { + /* Note that sample->key_hash was already set by ConnextStaticRawDataPlugin_deserialize() + it is done there because ConnextStaticRawDataPlugin_deserialize_sample does not + have access to the SampleInfo where that information is + */ + + /* We do not set the serialized_key on deserialization */ + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + + /* We copy everything that remains in the CDR stream */ + int bytesLeftInStream = RTICdrStream_getRemainder(stream); + DDS_Octet *cdrBufferPtr = (DDS_Octet *)RTICdrStream_getCurrentPosition(stream); + if (cdrBufferPtr == NULL) { + goto fin; + } + + /* Do not call ConnextStaticRawData_initialize_ex initialize here + because it would override the key_hash field + ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); + */ + if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { + goto fin; + } + RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); + } + + done = RTI_TRUE; + + fin: + if ( (done != RTI_TRUE) && + (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) { + return RTI_FALSE; + } + + RTICdrStream_restoreAlignment(stream,position); + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData *sample) +{ + struct RTICdrStream stream; + struct PRESTypePluginDefaultEndpointData epd; + RTIBool result; + + if (length == NULL) { + return RTI_FALSE; + } + + epd._maxSizeSerializedSample = + ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); + + if (buffer == NULL) { + *length = + ConnextStaticRawDataPlugin_get_serialized_sample_size( + (PRESTypePluginEndpointData)&epd, + RTI_TRUE, + RTICdrEncapsulation_getNativeCdrEncapsulationId(), + 0, + sample); + + if (*length == 0) { + return RTI_FALSE; + } + + return RTI_TRUE; + } + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, *length); + + result = ConnextStaticRawDataPlugin_serialize( + (PRESTypePluginEndpointData)&epd, sample, &stream, + RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), + RTI_TRUE, NULL); + + *length = RTICdrStream_getCurrentPositionOffset(&stream); + return result; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData *sample, + const char * buffer, + unsigned int length) +{ + struct RTICdrStream stream; + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, length); + + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + return ConnextStaticRawDataPlugin_deserialize_sample( + NULL, sample, + &stream, RTI_TRUE, RTI_TRUE, + NULL); +} + +DDS_ReturnCode_t +ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property) +{ + DDS_DynamicData *data = NULL; + char *buffer = NULL; + unsigned int length = 0; + struct DDS_PrintFormat printFormat; + DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; + + if (sample == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (str_size == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (property == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + NULL, + &length, + sample)) { + return DDS_RETCODE_ERROR; + } + + RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); + if (buffer == NULL) { + return DDS_RETCODE_ERROR; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + buffer, + &length, + sample)) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } + + data = DDS_DynamicData_new( + ConnextStaticRawData_get_typecode(), + &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); + if (data == NULL) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } + + retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_PrintFormatProperty_to_print_format( + property, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_DynamicDataFormatter_to_string_w_format( + data, + str, + str_size, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return DDS_RETCODE_OK; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos) +{ + + RTIBool result; + const char *METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; + if (drop_sample) {} /* To avoid warnings */ + + stream->_xTypesState.unassignable = RTI_FALSE; + result= ConnextStaticRawDataPlugin_deserialize_sample( + endpoint_data, (sample != NULL)?*sample:NULL, + stream, deserialize_encapsulation, deserialize_sample, + endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; + } + } + if (!result && stream->_xTypesState.unassignable ) { + + RTICdrLog_exception( + METHOD_NAME, + &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, + "ConnextStaticRawData"); + + } + + return result; + +} + +RTIBool ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(skip_encapsulation) { + if (!RTICdrStream_skipEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if (skip_sample) { + + if (!RTICdrStream_skipPrimitiveArray( + stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + goto fin; + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + } + + done = RTI_TRUE; + fin: + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + if(skip_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + if (endpoint_data) {} /* To avoid warnings */ + if (include_encapsulation) {} + if (encapsulation_id) {} + if (current_alignment) {} + + if (overflow != NULL) { + *overflow = RTI_TRUE; + } + + return RTI_CDR_MAX_SERIALIZED_SIZE; + +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + unsigned int size; + RTIBool overflow = RTI_FALSE; + + size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } + + return size; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + + if (endpoint_data) {} /* To avoid warnings */ + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } + + current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment,0, RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment,0, RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +/* Returns the size of the sample in its serialized form (in bytes). +* It can also be an estimation in excess of the real buffer needed +* during a call to the serialize() function. +* The value reported does not have to include the space for the +* encapsulation flags. +*/ +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + struct PRESTypePluginDefaultEndpointData epd; + + if (sample==NULL) { + return 0; + } + if (endpoint_data == NULL) { + endpoint_data = (PRESTypePluginEndpointData) &epd; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +/* -------------------------------------------------------------------------------------- +Key Management functions: +* -------------------------------------------------------------------------------------- */ + +PRESTypePluginKeyKind +ConnextStaticRawDataPlugin_get_key_kind_USER_KEY(void) +{ + return PRES_TYPEPLUGIN_USER_KEY; +} + +PRESTypePluginKeyKind +ConnextStaticRawDataPlugin_get_key_kind_NO_KEY(void) +{ + return PRES_TYPEPLUGIN_NO_KEY; +} + + +PRESTypePluginKeyKind +ConnextStaticRawDataPlugin_get_key_kind(void) +{ + return PRES_TYPEPLUGIN_USER_KEY; +} + +RTIBool +ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(serialize_encapsulation) { + if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if(serialize_key) { + + if (!RTICdrStream_serializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + } + + if(serialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + + char * position = NULL; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(deserialize_encapsulation) { + + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + if (deserialize_key) { + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; + +} + +RTIBool ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + RTIBool result; + if (drop_sample) {} /* To avoid warnings */ + stream->_xTypesState.unassignable = RTI_FALSE; + result= ConnextStaticRawDataPlugin_deserialize_key_sample( + endpoint_data, (sample != NULL)?*sample:NULL, stream, + deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; + } + } + + return result; + +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + + if (endpoint_data) {} /* To avoid warnings */ + if (overflow) {} /* To avoid warnings */ + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } + + current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + unsigned int size; + RTIBool overflow = RTI_FALSE; + + size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } + + return size; +} + +RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + if(deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + position = RTICdrStream_resetAlignment(stream); + } + + if (deserialize_key) { + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + + } + + done = RTI_TRUE; + fin: + if(!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + } else { + return RTI_FALSE; + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder *dst, + const ConnextStaticRawData *src) +{ + + if (endpoint_data) {} /* To avoid warnings */ + + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *dst, const + ConnextStaticRawDataKeyHolder *src) +{ + + if (endpoint_data) {} /* To avoid warnings */ + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t *keyhash, + const ConnextStaticRawData *instance) +{ + struct RTICdrStream * md5Stream = NULL; + struct RTICdrStreamState cdrState; + char * buffer = NULL; + + RTICdrStreamState_init(&cdrState); + md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); + + if (md5Stream == NULL) { + return RTI_FALSE; + } + + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + int size; + + RTICdrStream_pushState(md5Stream, &cdrState, -1); + + size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( + endpoint_data, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + 0, + instance); + + if (size <= RTICdrStream_getBufferLength(md5Stream)) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTIOsapiHeap_allocateBuffer(&buffer,size,0); + + if (buffer == NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTICdrStream_set(md5Stream, buffer, size); + RTIOsapiMemory_zero( + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getBufferLength(md5Stream)); + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + return RTI_FALSE; + } + } + + if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > + (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || + PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { + RTICdrStream_computeMD5(md5Stream, keyhash->value); + } else { + RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); + RTIOsapiMemory_copy( + keyhash->value, + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getCurrentPositionOffset(md5Stream)); + } + + keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; + + if (buffer != NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + } + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + DDS_KeyHash_t *keyhash, + RTIBool deserialize_encapsulation, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + ConnextStaticRawData * sample=NULL; + + if (endpoint_plugin_qos) {} /* To avoid warnings */ + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + + if(deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + sample = (ConnextStaticRawData *) + PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); + + if (sample == NULL) { + return RTI_FALSE; + } + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + done = RTI_TRUE; + fin: + if(!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + } else { + return RTI_FALSE; + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + if (!ConnextStaticRawDataPlugin_instance_to_keyhash( + endpoint_data, keyhash, sample)) { + return RTI_FALSE; + } + + return RTI_TRUE; +} + +/* ------------------------------------------------------------------------ +* Plug-in Installation Methods +* ------------------------------------------------------------------------ */ +struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) +{ + return NULL; +} + +/* The following need to made public. They are defined in TypeCodeSupport.c */ +extern DDS_Boolean DDS_TypeCode_is_type_keyed(const DDS_TypeCode *type); + +struct PRESTypePlugin * ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * type_code) +{ + struct PRESTypePlugin *plugin = NULL; + const struct PRESTypePluginVersion PLUGIN_VERSION = + PRES_TYPE_PLUGIN_VERSION_2_0; + + RTIOsapiHeap_allocateStructure( + &plugin, struct PRESTypePlugin); + + if (plugin == NULL) { + return NULL; + } + + plugin->version = PLUGIN_VERSION; + + /* set up parent's function pointers */ + plugin->onParticipantAttached = + (PRESTypePluginOnParticipantAttachedCallback) + ConnextStaticRawDataPlugin_on_participant_attached; + plugin->onParticipantDetached = + (PRESTypePluginOnParticipantDetachedCallback) + ConnextStaticRawDataPlugin_on_participant_detached; + plugin->onEndpointAttached = + (PRESTypePluginOnEndpointAttachedCallback) + ConnextStaticRawDataPlugin_on_endpoint_attached; + plugin->onEndpointDetached = + (PRESTypePluginOnEndpointDetachedCallback) + ConnextStaticRawDataPlugin_on_endpoint_detached; + + plugin->copySampleFnc = + (PRESTypePluginCopySampleFunction) + ConnextStaticRawDataPlugin_copy_sample; + plugin->createSampleFnc = + (PRESTypePluginCreateSampleFunction) + ConnextStaticRawDataPlugin_create_sample; + plugin->destroySampleFnc = + (PRESTypePluginDestroySampleFunction) + ConnextStaticRawDataPlugin_destroy_sample; + + plugin->serializeFnc = + (PRESTypePluginSerializeFunction) + ConnextStaticRawDataPlugin_serialize; + plugin->deserializeFnc = + (PRESTypePluginDeserializeFunction) + ConnextStaticRawDataPlugin_deserialize; + plugin->getSerializedSampleMaxSizeFnc = + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size; + plugin->getSerializedSampleMinSizeFnc = + (PRESTypePluginGetSerializedSampleMinSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_min_size; + + plugin->getSampleFnc = + (PRESTypePluginGetSampleFunction) + ConnextStaticRawDataPlugin_get_sample; + plugin->returnSampleFnc = + (PRESTypePluginReturnSampleFunction) + ConnextStaticRawDataPlugin_return_sample; + + if ( (type_code != NULL) && DDS_TypeCode_is_type_keyed(type_code) ) { + plugin->getKeyKindFnc = (PRESTypePluginGetKeyKindFunction) + ConnextStaticRawDataPlugin_get_key_kind_USER_KEY; + } + else { + plugin->getKeyKindFnc = (PRESTypePluginGetKeyKindFunction) + ConnextStaticRawDataPlugin_get_key_kind_NO_KEY; + } + + plugin->getSerializedKeyMaxSizeFnc = + (PRESTypePluginGetSerializedKeyMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_key_max_size; + plugin->serializeKeyFnc = + (PRESTypePluginSerializeKeyFunction) + ConnextStaticRawDataPlugin_serialize_key; + plugin->deserializeKeyFnc = + (PRESTypePluginDeserializeKeyFunction) + ConnextStaticRawDataPlugin_deserialize_key; + plugin->deserializeKeySampleFnc = + (PRESTypePluginDeserializeKeySampleFunction) + ConnextStaticRawDataPlugin_deserialize_key_sample; + + plugin-> instanceToKeyHashFnc = + (PRESTypePluginInstanceToKeyHashFunction) + ConnextStaticRawDataPlugin_instance_to_keyhash; + plugin->serializedSampleToKeyHashFnc = + (PRESTypePluginSerializedSampleToKeyHashFunction) + ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; + + plugin->getKeyFnc = + (PRESTypePluginGetKeyFunction) + ConnextStaticRawDataPlugin_get_key; + plugin->returnKeyFnc = + (PRESTypePluginReturnKeyFunction) + ConnextStaticRawDataPlugin_return_key; + + plugin->instanceToKeyFnc = + (PRESTypePluginInstanceToKeyFunction) + ConnextStaticRawDataPlugin_instance_to_key; + plugin->keyToInstanceFnc = + (PRESTypePluginKeyToInstanceFunction) + ConnextStaticRawDataPlugin_key_to_instance; + plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ + plugin->typeCode = (struct RTICdrTypeCode *)type_code; + + plugin->languageKind = PRES_TYPEPLUGIN_DDS_TYPE; + + /* Serialized buffer */ + plugin->getBuffer = + (PRESTypePluginGetBufferFunction) + ConnextStaticRawDataPlugin_get_buffer; + plugin->returnBuffer = + (PRESTypePluginReturnBufferFunction) + ConnextStaticRawDataPlugin_return_buffer; + plugin->getSerializedSampleSizeFnc = + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size; + + plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; + + return plugin; +} + +void +ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) +{ + RTIOsapiHeap_freeStructure(plugin); +} +#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rosidl_typesupport_connext_c/src/connext_static_raw_data_support.c b/rosidl_typesupport_connext_c/src/connext_static_raw_data_support.c new file mode 100644 index 00000000..6bacc179 --- /dev/null +++ b/rosidl_typesupport_connext_c/src/connext_static_raw_data_support.c @@ -0,0 +1,167 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from ConnextStaticRawData.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#include "rosidl_typesupport_connext_c/connext_static_raw_data_support.h" +#include "rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h" + +/* ========================================================================= */ +/** +<> + +Defines: TData, +TDataWriter, +TDataReader, +TTypeSupport + +Configure and implement 'ConnextStaticRawData' support classes. + +Note: Only the #defined classes get defined +*/ + +/* ----------------------------------------------------------------- */ +/* DDSDataWriter +*/ + +/** +<> + +Defines: TDataWriter, TData +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME + +/* Defines */ +#define TDataWriter ConnextStaticRawDataDataWriter +#define TData ConnextStaticRawData + +#include "dds_c/generic/dds_c_data_TDataWriter.gen" + +#undef TDataWriter +#undef TData + +#undef TTYPENAME + +/* ----------------------------------------------------------------- */ +/* DDSDataReader +*/ + +/** +<> + +Defines: TDataReader, TDataSeq, TData +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME + +/* Defines */ +#define TDataReader ConnextStaticRawDataDataReader +#define TDataSeq ConnextStaticRawDataSeq +#define TData ConnextStaticRawData + +#include "dds_c/generic/dds_c_data_TDataReader.gen" + +#undef TDataReader +#undef TDataSeq +#undef TData + +#undef TTYPENAME + +/* ----------------------------------------------------------------- */ +/* TypeSupport + +<> + +Requires: TTYPENAME, +TPlugin_new +TPlugin_delete +Defines: TTypeSupport, TData, TDataReader, TDataWriter +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME +#define TPlugin_new ConnextStaticRawDataPlugin_new +#define TPlugin_delete ConnextStaticRawDataPlugin_delete + +/* Defines */ +#define TTypeSupport ConnextStaticRawDataTypeSupport +#define TData ConnextStaticRawData +#define TDataReader ConnextStaticRawDataDataReader +#define TDataWriter ConnextStaticRawDataDataWriter +#define TGENERATE_SER_CODE +#define TGENERATE_TYPECODE + +#include "dds_c/generic/dds_c_data_TTypeSupport.gen" + +#undef TTypeSupport +#undef TData +#undef TDataReader +#undef TDataWriter +#undef TGENERATE_TYPECODE +#undef TGENERATE_SER_CODE +#undef TTYPENAME +#undef TPlugin_new +#undef TPlugin_delete + +/* + The ConnextStaticRawData needs to be registered passing the + TypeCode of the underlying type. That will ensure the right + type information is propagated on the wire. + + Note that ConnextStaticRawDataTypeSupport_register_type() is defined by + the macros above. We can't avoid it the way the code is generated. + That function should not be called. Instead the application should + call TTypeSupport_register_external_type() + + serialized_key_max_size should be set to 0 if the type is Unkeyed. Otherwise + it shoud be set to the maximum size of the serialized key of the + underlying type defined by the type_code paramameter. + + TODO: Would be nice to redefine ConnextStaticRawDataTypeSupport_register_type() + to have the implementation below instead of having to add a new register_external_type() + function. At least we could find a way to prevent register_type() from being + called... +*/ +DDS_ReturnCode_t ConnextStaticRawDataTypeSupport_register_external_type( + DDS_DomainParticipant* participant, + const char* type_name, + struct DDS_TypeCode *type_code) +{ + struct PRESTypePlugin *presTypePlugin = NULL; + DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; + + if (participant == NULL) { + goto finError; + } + + /* TODO pass the type_code */ + presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); + if (presTypePlugin == NULL) { + goto finError; + } + + retcode = DDS_DomainParticipant_register_type( + participant, + type_name, + presTypePlugin, + NULL /* registration_data */); + if (retcode != DDS_RETCODE_OK) { + goto finError; + } + return DDS_RETCODE_OK; + +finError: + if (presTypePlugin != NULL) { + ConnextStaticRawDataPlugin_delete(presTypePlugin); + } + + return retcode; +} diff --git a/rosidl_typesupport_connext_cpp/CMakeLists.txt b/rosidl_typesupport_connext_cpp/CMakeLists.txt index c25a3ad5..d2c420de 100644 --- a/rosidl_typesupport_connext_cpp/CMakeLists.txt +++ b/rosidl_typesupport_connext_cpp/CMakeLists.txt @@ -41,13 +41,7 @@ ament_export_include_directories(include) ament_python_install_package(${PROJECT_NAME}) -add_library( - ${PROJECT_NAME} - SHARED - src/connext_static_raw_data.cxx - src/connext_static_raw_data_plugin.cxx - src/connext_static_raw_data_support.cxx - src/identifier.cpp) +add_library(${PROJECT_NAME} SHARED src/identifier.cpp) if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSIDL_TYPESUPPORT_CONNEXT_CPP_BUILDING_DLL") @@ -56,10 +50,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC include ) -ament_target_dependencies( - ${PROJECT_NAME} - "Connext" -) ament_export_libraries(${PROJECT_NAME}) ament_index_register_resource("rosidl_typesupport_cpp") diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h index a52b400c..5e1ebf62 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h @@ -19,18 +19,21 @@ #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" +// forward declare DDS_TypeCode +struct DDS_TypeCode; + typedef struct message_type_support_callbacks_t { const char * package_name; const char * message_name; // Function to register type with given dds_participant - bool (* register_type)(void * dds_participant, const char * type_name); + DDS_TypeCode * (* get_type_code)(void); // Function to publish a ROS message with a given DDS data_writer - bool (* publish)(void * dds_data_writer, ConnextStaticCDRStream * cdr_stream); + //bool (* publish)(void * dds_data_writer, ConnextStaticCDRStream * cdr_stream); // Function to take a ROS message from a dds data reader - bool (* take)( - void * dds_data_reader, bool ignore_local_publications, ConnextStaticCDRStream * cdr_stream, bool * taken, - void * sending_publication_handle); + //bool (* take)( + // void * dds_data_reader, bool ignore_local_publications, ConnextStaticCDRStream * cdr_stream, bool * taken, + // void * sending_publication_handle); bool (* convert_ros_to_dds)( const void * untyped_ros_message, void * untyped_data_message); diff --git a/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em b/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em index 935b31ad..aac16340 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em @@ -63,10 +63,8 @@ namespace @(subfolder) namespace typesupport_connext_cpp { -bool -register_type__@(spec.base_type.type)( - DDSDomainParticipant * participant, - const char * type_name); +DDS_TypeCode * +get_type_code__@(spec.base_type.type)(); bool ROSIDL_TYPESUPPORT_CONNEXT_CPP_PUBLIC_@(spec.base_type.pkg_name) @@ -74,10 +72,10 @@ convert_ros_message_to_dds( const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message, @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ & dds_message); -bool -publish__@(spec.base_type.type)( - DDSDataWriter * topic_writer, - const void * untyped_ros_message); +//bool +//publish__@(spec.base_type.type)( +// DDSDataWriter * topic_writer, +// const void * untyped_ros_message); bool ROSIDL_TYPESUPPORT_CONNEXT_CPP_PUBLIC_@(spec.base_type.pkg_name) @@ -85,12 +83,12 @@ convert_dds_message_to_ros( const @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ & dds_message, @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message); -bool -take__@(spec.base_type.type)( - DDSDataReader * topic_reader, - bool ignore_local_publications, - ConnextStaticCDRStream * untyped_ros_message, - bool * taken); +//bool +//take__@(spec.base_type.type)( +// DDSDataReader * topic_reader, +// bool ignore_local_publications, +// ConnextStaticCDRStream * untyped_ros_message, +// bool * taken); bool to_cdr_stream__@(spec.base_type.type)( diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index be784486..26bbdc21 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -21,7 +21,6 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" -#include "rosidl_typesupport_connext_cpp/connext_static_raw_data_support.h" #include "rosidl_typesupport_connext_cpp/identifier.hpp" #include "rosidl_typesupport_connext_cpp/message_type_support.h" #include "rosidl_typesupport_connext_cpp/message_type_support_decl.hpp" @@ -61,28 +60,10 @@ namespace @(subfolder) namespace typesupport_connext_cpp { -bool -register_type__@(spec.base_type.type)( - void * untyped_participant, - const char * type_name) +DDS_TypeCode * +get_type_code__@(spec.base_type.type)() { - DDSDomainParticipant * participant = static_cast(untyped_participant); - DDS_TypeCode * type_code = - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::get_typecode(); - // This is a non-standard RTI Connext function - // It allows to register an external type to a static data writer - // In this case, we register the custom message type to a data writer, - // which only publishes DDS_Octets - // The purpose of this is to send only raw data DDS_Octets over the wire, - // advertise the topic however with a type of the message, e.g. std_msgs::msg::dds_::String - DDS_ReturnCode_t status = ConnextStaticRawDataSupport_register_external_type( - participant, - type_name, - type_code); - if (status != DDS_RETCODE_OK) { - fprintf(stderr, "Failed to register external type\n"); - } - return status == DDS_RETCODE_OK; + return @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::get_typecode(); } bool @@ -160,53 +141,6 @@ convert_ros_message_to_dds( return true; } -bool -publish__@(spec.base_type.type)( - void * untyped_topic_writer, - ConnextStaticCDRStream * cdr_stream) -{ - DDSDataWriter * topic_writer = static_cast(untyped_topic_writer); - - ConnextStaticRawDataDataWriter * data_writer = - ConnextStaticRawDataDataWriter::narrow(topic_writer); - if (!data_writer) { - fprintf(stderr, "failed to narrow data writer\n"); - return false; - } - - ConnextStaticRawData * instance = ConnextStaticRawDataTypeSupport::create_data(); - if (!instance) { - fprintf(stderr, "failed to create dds message instance\n"); - return false; - } - - DDS_ReturnCode_t status = DDS_RETCODE_ERROR; - - instance->serialized_data.maximum(0); - if (!instance->serialized_data.loan_contiguous( - reinterpret_cast(cdr_stream->raw_message), - cdr_stream->message_length, cdr_stream->message_length)) { - fprintf(stderr, "failed to loan memory for message\n"); - return false; - } - - fprintf(stderr, "raw data size: %d\n", instance->serialized_data.length()); - for (DDS_Long i = 0; i < instance->serialized_data.length(); ++i) { - fprintf(stderr, "%02x ", instance->serialized_data[i]); - } - fprintf(stderr, "\n"); - - status = data_writer->write(*instance, DDS_HANDLE_NIL); - - if (!instance->serialized_data.unloan()) { - fprintf(stderr, "failed to return loaned memory\n"); - return false; - } - status = ConnextStaticRawDataTypeSupport::delete_data(instance); - - return status == DDS_RETCODE_OK; -} - bool convert_dds_message_to_ros( const @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ & dds_message, @@ -258,91 +192,6 @@ convert_dds_message_to_ros( return true; } -bool -take__@(spec.base_type.type)( - void * untyped_topic_reader, - bool ignore_local_publications, - ConnextStaticCDRStream * cdr_stream, - bool * taken, - void * sending_publication_handle) -{ - if (!untyped_topic_reader) { - throw std::runtime_error("topic reader handle is null"); - } - if (!cdr_stream) { - throw std::runtime_error("cdr stream handle is null"); - } - if (!taken) { - throw std::runtime_error("taken handle is null"); - } - - DDSDataReader * topic_reader = static_cast(untyped_topic_reader); - - ConnextStaticRawDataDataReader * data_reader = - ConnextStaticRawDataDataReader::narrow(topic_reader); - if (!data_reader) { - fprintf(stderr, "failed to narrow data reader\n"); - return false; - } - - ConnextStaticRawDataSeq dds_messages; - DDS_SampleInfoSeq sample_infos; - DDS_ReturnCode_t status = data_reader->take( - dds_messages, - sample_infos, - 1, - DDS_ANY_SAMPLE_STATE, - DDS_ANY_VIEW_STATE, - DDS_ANY_INSTANCE_STATE); - if (status == DDS_RETCODE_NO_DATA) { - *taken = false; - return true; - } - if (status != DDS_RETCODE_OK) { - fprintf(stderr, "take failed with status = %d\n", status); - return false; - } - - bool ignore_sample = false; - DDS_SampleInfo & sample_info = sample_infos[0]; - if (!sample_info.valid_data) { - // skip sample without data - ignore_sample = true; - } else if (ignore_local_publications) { - // compare the lower 12 octets of the guids from the sender and this receiver - // if they are equal the sample has been sent from this process and should be ignored - DDS_GUID_t sender_guid = sample_info.original_publication_virtual_guid; - DDS_InstanceHandle_t receiver_instance_handle = topic_reader->get_instance_handle(); - ignore_sample = true; - for (size_t i = 0; i < 12; ++i) { - DDS_Octet * sender_element = &(sender_guid.value[i]); - DDS_Octet * receiver_element = &(reinterpret_cast(&receiver_instance_handle)[i]); - if (*sender_element != *receiver_element) { - ignore_sample = false; - break; - } - } - } - if (sample_info.valid_data && sending_publication_handle) { - *static_cast(sending_publication_handle) = - sample_info.publication_handle; - } - - if (!ignore_sample) { - cdr_stream->message_length = dds_messages[0].serialized_data.length(); - cdr_stream->raw_message = (char *) malloc(cdr_stream->message_length * sizeof(char)); - for (unsigned int i = 0; i < cdr_stream->message_length; ++i) { - cdr_stream->raw_message[i] = dds_messages[0].serialized_data[i]; - } - *taken = true; - } else { - *taken = false; - } - data_reader->return_loan(dds_messages, sample_infos); - - return true; -} - bool to_cdr_stream__@(spec.base_type.type)( const void * untyped_ros_message, @@ -421,9 +270,7 @@ to_message__@(spec.base_type.type)( static message_type_support_callbacks_t callbacks = { "@(spec.base_type.pkg_name)", "@(spec.base_type.type)", - ®ister_type__@(spec.base_type.type), - &publish__@(spec.base_type.type), - &take__@(spec.base_type.type), + &get_type_code__@(spec.base_type.type), nullptr, nullptr, &to_cdr_stream__@(spec.base_type.type), From 7654bbddb49651f52e1e6e06862d37a8bace41d0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 27 Feb 2018 15:22:07 -0800 Subject: [PATCH 11/34] linters --- .../SerializedType.idl | 7 - .../SerializedType_publisher.c | 274 -------------- .../SerializedType_subscriber.c | 263 -------------- .../connext_static_raw_data.h | 88 ----- .../connext_static_raw_data_plugin.h | 339 ------------------ .../connext_static_raw_data_support.h | 61 ---- .../resource/msg__type_support_c.cpp.em | 20 +- ...msg__rosidl_typesupport_connext_cpp.hpp.em | 12 - .../resource/msg__type_support.cpp.em | 4 +- 9 files changed, 16 insertions(+), 1052 deletions(-) delete mode 100644 rosidl_typesupport_connext_c/SerializedType.idl delete mode 100644 rosidl_typesupport_connext_c/SerializedType_publisher.c delete mode 100644 rosidl_typesupport_connext_c/SerializedType_subscriber.c delete mode 100644 rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data.h delete mode 100644 rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h delete mode 100644 rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_support.h diff --git a/rosidl_typesupport_connext_c/SerializedType.idl b/rosidl_typesupport_connext_c/SerializedType.idl deleted file mode 100644 index 89ea3650..00000000 --- a/rosidl_typesupport_connext_c/SerializedType.idl +++ /dev/null @@ -1,7 +0,0 @@ -const long KEY_HASH_LENGTH_16 = 16; - -struct SerializedType { - @key octet key_hash[KEY_HASH_LENGTH_16]; - sequence serialized_key; - sequence serialized_data; -}; \ No newline at end of file diff --git a/rosidl_typesupport_connext_c/SerializedType_publisher.c b/rosidl_typesupport_connext_c/SerializedType_publisher.c deleted file mode 100644 index 2aec32ca..00000000 --- a/rosidl_typesupport_connext_c/SerializedType_publisher.c +++ /dev/null @@ -1,274 +0,0 @@ - -/**/ - -#include -#include -#include "ndds/ndds_c.h" -#include "SerializedType.h" -#include "SerializedTypeSupport.h" - -//#include "ShapeType.h" -//#include "ShapeTypePlugin.h" -#include "DynamicArrayPrimitives_.h" -#include "DynamicArrayPrimitives_Plugin.h" -#include "DynamicArrayPrimitives_Support.h" - -/* Delete all entities */ -static int publisher_shutdown( - DDS_DomainParticipant *participant) -{ - DDS_ReturnCode_t retcode; - int status = 0; - - if (participant != NULL) { - retcode = DDS_DomainParticipant_delete_contained_entities(participant); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "delete_contained_entities error %d\n", retcode); - status = -1; - } - - retcode = DDS_DomainParticipantFactory_delete_participant( - DDS_TheParticipantFactory, participant); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "delete_participant error %d\n", retcode); - status = -1; - } - } - - /* RTI Data Distribution Service provides finalize_instance() method on - domain participant factory for people who want to release memory used - by the participant factory. Uncomment the following block of code for - clean destruction of the singleton. */ - /* - retcode = DDS_DomainParticipantFactory_finalize_instance(); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "finalize_instance error %d\n", retcode); - status = -1; - } - */ - - return status; -} - -void strcpy_s(char * str, int lenMax, const char * strToCopy) { - if (!str || !strToCopy) { - return; - } - int strToCopyLen = strlen(strToCopy) + 1; - if(strToCopyLen < lenMax) { - lenMax = strToCopyLen; - } - strncpy(str, strToCopy, lenMax); - str[lenMax - 1] = '\0'; -} - -static int publisher_main(int domainId, int sample_count) -{ - DDS_DomainParticipant *participant = NULL; - DDS_Publisher *publisher = NULL; - DDS_Topic *topic = NULL; - DDS_DataWriter *writer = NULL; - SerializedTypeDataWriter *SerializedType_writer = NULL; - SerializedType *instance = NULL; - DDS_ReturnCode_t retcode; - DDS_InstanceHandle_t instance_handle = DDS_HANDLE_NIL; - int count = 0, i = 0; - struct DDS_Duration_t send_period = {1,0}; - - /* To customize participant QoS, use - the configuration file USER_QOS_PROFILES.xml */ - participant = DDS_DomainParticipantFactory_create_participant( - DDS_TheParticipantFactory, domainId, &DDS_PARTICIPANT_QOS_DEFAULT, - NULL /* listener */, DDS_STATUS_MASK_NONE); - if (participant == NULL) { - fprintf(stderr, "create_participant error\n"); - publisher_shutdown(participant); - return -1; - } - - /* To customize publisher QoS, use - the configuration file USER_QOS_PROFILES.xml */ - publisher = DDS_DomainParticipant_create_publisher( - participant, &DDS_PUBLISHER_QOS_DEFAULT, NULL /* listener */, - DDS_STATUS_MASK_NONE); - if (publisher == NULL) { - fprintf(stderr, "create_publisher error\n"); - publisher_shutdown(participant); - return -1; - } - - /* When regiatering the Data-Type with DDS it is important to use - the correct type code which corresponds to the serilized data-format - otherwise the type would be seen as incompatible by others and tools - like visualization will not work. - - Here since the serialized data corresponds to a ShapeType we - use the ShapeType_get_typecode() - */ - const char * type_name = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport_get_type_name(); - struct DDS_TypeCode * type_code = test_msgs_msg_dds__DynamicArrayPrimitives__get_typecode(); - retcode = SerializedTypeTypeSupport_register_external_type( - participant, type_name, type_code); - - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "register_type error %d\n", retcode); - publisher_shutdown(participant); - return -1; - } - - /* To customize topic QoS, use - the configuration file USER_QOS_PROFILES.xml */ - topic = DDS_DomainParticipant_create_topic( - participant, "my_generic_type", - type_name, &DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, - DDS_STATUS_MASK_NONE); - if (topic == NULL) { - fprintf(stderr, "create_topic error\n"); - publisher_shutdown(participant); - return -1; - } - - /* To customize data writer QoS, use - the configuration file USER_QOS_PROFILES.xml */ - writer = DDS_Publisher_create_datawriter( - publisher, topic, - &DDS_DATAWRITER_QOS_DEFAULT, NULL /* listener */, DDS_STATUS_MASK_NONE); - if (writer == NULL) { - fprintf(stderr, "create_datawriter error\n"); - publisher_shutdown(participant); - return -1; - } - SerializedType_writer = SerializedTypeDataWriter_narrow(writer); - if (SerializedType_writer == NULL) { - fprintf(stderr, "DataWriter narrow error\n"); - publisher_shutdown(participant); - return -1; - } - - /* Create data sample for writing */ - instance = SerializedTypeTypeSupport_create_data_ex(DDS_BOOLEAN_TRUE); - if (instance == NULL) { - fprintf(stderr, "SerializedTypeTypeSupport_create_data error\n"); - publisher_shutdown(participant); - return -1; - } -// /* We take advantage of the ShapeTypePlugin to serialize the data -// * At the application layer -// */ -// ShapeType shapeType; -// ShapeType_initialize(&shapeType); -// -//#define NUMBER_OF_COLORS (4) -// char *colors[NUMBER_OF_COLORS] = { "GREEN", "RED", "BLUE", "YELLOW" }; -// int xbase[NUMBER_OF_COLORS] = { 10, 50, 100, 150 }; -// int ybase[NUMBER_OF_COLORS] = { 0, 0, 0, 0 }; -// -// /* Memory area where to put the serialized (ShapeType) data */ -// unsigned int serializationLength; -// unsigned int serializationBufferSize = ShapeTypePlugin_get_serialized_sample_max_size(NULL, RTI_TRUE, 0, 0); -// -// /* RTI_CDR_MAX_SERIALIZED_SIZE indites the type is unbounded normally the application -// would have some knwledge of the size. Here we print an error in this situation */ -// if (serializationBufferSize == RTI_CDR_MAX_SERIALIZED_SIZE) { -// fprintf(stderr, "Type is unbounded. Please enter buffer size manually here\n"); -// return publisher_shutdown(participant); -// } - - test_msgs_msg_dds__DynamicArrayPrimitives_ dynamic_array_instance; - test_msgs_msg_dds__DynamicArrayPrimitives__initialize(&dynamic_array_instance); - DDS_LongSeq_initialize(&dynamic_array_instance.int32_values_); - size_t length = 100; - DDS_LongSeq_set_length(&dynamic_array_instance.int32_values_, length); - for (size_t i = 0; i < length; ++i) { - DDS_Long * element = DDS_LongSeq_get_reference(&dynamic_array_instance.int32_values_, i); - *element = i; - } - - unsigned int serializationLength; - unsigned int serializationBufferSize = test_msgs_msg_dds__DynamicArrayPrimitives_Plugin_get_serialized_sample_max_size(NULL, RTI_TRUE, 0, 0); - - DDS_Octet *serializationBuffer = (DDS_Octet *)malloc(serializationBufferSize); - - for (count = 0; (sample_count == 0) || (count < sample_count); ++count) { - - printf("Writing DynamicAraryPrimitives, count %d\n", count); - - /* Modify the data to be written here */ - - /* Use ShapeTypePlugin to serialize into an application buffer. - Serialization includes the 4-byte SerializationHeader. - - Note: serializationLength on input it is the maximum size. - On successful output it is the number of bytes used for - the serialization - */ - serializationLength = serializationBufferSize; - if (!test_msgs_msg_dds__DynamicArrayPrimitives_Plugin_serialize_to_cdr_buffer((char *)serializationBuffer, &serializationLength, &dynamic_array_instance)) { - fprintf(stderr, "Serialization of ShapeType failed\n"); - } - else { - /* At this point: - serializationBuffer - contains the serialized shapeType - serializationLength - contains the number of bytes in serializationBuffer used by the serialization - */ - /* Use DDS_OctetSeq_loan_contiguous() instead of DDS_OctetSeq_copy() to save one copy */ - DDS_OctetSeq_loan_contiguous(&(instance->serialized_data), serializationBuffer, - serializationLength, serializationBufferSize); - - /* TODO: Use ShapeType_serialize_key */ -// for (i = 0; i < 16; ++i) { -// instance->key_hash[i] = (char)(count % NUMBER_OF_COLORS); -// } - - retcode = SerializedTypeDataWriter_write( - SerializedType_writer, instance, &instance_handle); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "write error %d\n", retcode); - } - DDS_OctetSeq_unloan(&(instance->serialized_data)); - } - - NDDS_Utility_sleep(&send_period); - } - - /* - retcode = SerializedTypeDataWriter_unregister_instance( - SerializedType_writer, instance, &instance_handle); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "unregister instance error %d\n", retcode); - } - */ - - /* Delete data sample */ - retcode = SerializedTypeTypeSupport_delete_data_ex(instance, DDS_BOOLEAN_TRUE); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "SerializedTypeTypeSupport_delete_data error %d\n", retcode); - } - - /* Cleanup and delete delete all entities */ - free(serializationBuffer); - return publisher_shutdown(participant); -} - -int main(int argc, char *argv[]) -{ - int domainId = 0; - int sample_count = 0; /* infinite loop */ - - if (argc >= 2) { - domainId = atoi(argv[1]); - } - if (argc >= 3) { - sample_count = atoi(argv[2]); - } - - /* Uncomment this to turn on additional logging - NDDS_Config_Logger_set_verbosity_by_category( - NDDS_Config_Logger_get_instance(), - NDDS_CONFIG_LOG_CATEGORY_API, - NDDS_CONFIG_LOG_VERBOSITY_STATUS_ALL); - */ - - return publisher_main(domainId, sample_count); -} - diff --git a/rosidl_typesupport_connext_c/SerializedType_subscriber.c b/rosidl_typesupport_connext_c/SerializedType_subscriber.c deleted file mode 100644 index ca550092..00000000 --- a/rosidl_typesupport_connext_c/SerializedType_subscriber.c +++ /dev/null @@ -1,263 +0,0 @@ - -/* -*/ - -#include -#include -#include "ndds/ndds_c.h" -#include "SerializedType.h" -#include "SerializedTypeSupport.h" - -#include "DynamicArrayPrimitives_.h" -#include "DynamicArrayPrimitives_Support.h" - -void SerializedTypeListener_on_requested_deadline_missed( - void* listener_data, - DDS_DataReader* reader, - const struct DDS_RequestedDeadlineMissedStatus *status) -{ -} - -void SerializedTypeListener_on_requested_incompatible_qos( - void* listener_data, - DDS_DataReader* reader, - const struct DDS_RequestedIncompatibleQosStatus *status) -{ -} - -void SerializedTypeListener_on_sample_rejected( - void* listener_data, - DDS_DataReader* reader, - const struct DDS_SampleRejectedStatus *status) -{ -} - -void SerializedTypeListener_on_liveliness_changed( - void* listener_data, - DDS_DataReader* reader, - const struct DDS_LivelinessChangedStatus *status) -{ -} - -void SerializedTypeListener_on_sample_lost( - void* listener_data, - DDS_DataReader* reader, - const struct DDS_SampleLostStatus *status) -{ -} - -void SerializedTypeListener_on_subscription_matched( - void* listener_data, - DDS_DataReader* reader, - const struct DDS_SubscriptionMatchedStatus *status) -{ -} - -void SerializedTypeListener_on_data_available( - void* listener_data, - DDS_DataReader* reader) -{ - SerializedTypeDataReader *SerializedType_reader = NULL; - struct SerializedTypeSeq data_seq = DDS_SEQUENCE_INITIALIZER; - struct DDS_SampleInfoSeq info_seq = DDS_SEQUENCE_INITIALIZER; - struct DDS_SampleInfo *sample_info; - - DDS_ReturnCode_t retcode; - int i; - - SerializedType_reader = SerializedTypeDataReader_narrow(reader); - if (SerializedType_reader == NULL) { - fprintf(stderr, "DataReader narrow error\n"); - return; - } - - retcode = SerializedTypeDataReader_take( - SerializedType_reader, - &data_seq, &info_seq, DDS_LENGTH_UNLIMITED, - DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE); - if (retcode == DDS_RETCODE_NO_DATA) { - return; - } else if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "take error %d\n", retcode); - return; - } - - for (i = 0; i < SerializedTypeSeq_get_length(&data_seq); ++i) { - sample_info = DDS_SampleInfoSeq_get_reference(&info_seq, i); - if (sample_info->valid_data) { - SerializedType *serializedType = SerializedTypeSeq_get_reference(&data_seq, i); - - /* Print information as a serialized buffer */ - printf("\nSerializedTypeSeq (as bytes):"); - SerializedTypeTypeSupport_print_data(serializedType); - - /* Pretty print it using the TypeCode if available*/ - //SerializedTypeTypeSupport_print_data2(serializedType, stdout, "SerializedTypeSeq (as ShapeType)", 0, ShapeType_get_typecode()); - } - } - - retcode = SerializedTypeDataReader_return_loan( - SerializedType_reader, - &data_seq, &info_seq); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "return loan error %d\n", retcode); - } -} - -/* Delete all entities */ -static int subscriber_shutdown( - DDS_DomainParticipant *participant) -{ - DDS_ReturnCode_t retcode; - int status = 0; - - if (participant != NULL) { - retcode = DDS_DomainParticipant_delete_contained_entities(participant); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "delete_contained_entities error %d\n", retcode); - status = -1; - } - - retcode = DDS_DomainParticipantFactory_delete_participant( - DDS_TheParticipantFactory, participant); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "delete_participant error %d\n", retcode); - status = -1; - } - } - - /* RTI Data Distribution Service provides the finalize_instance() method on - domain participant factory for users who want to release memory used - by the participant factory. Uncomment the following block of code for - clean destruction of the singleton. */ - /* - retcode = DDS_DomainParticipantFactory_finalize_instance(); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "finalize_instance error %d\n", retcode); - status = -1; - } - */ - - return status; -} - -static int subscriber_main(int domainId, int sample_count) -{ - DDS_DomainParticipant *participant = NULL; - DDS_Subscriber *subscriber = NULL; - DDS_Topic *topic = NULL; - struct DDS_DataReaderListener reader_listener = - DDS_DataReaderListener_INITIALIZER; - DDS_DataReader *reader = NULL; - DDS_ReturnCode_t retcode; - int count = 0; - struct DDS_Duration_t poll_period = {4,0}; - - /* To customize participant QoS, use - the configuration file USER_QOS_PROFILES.xml */ - participant = DDS_DomainParticipantFactory_create_participant( - DDS_TheParticipantFactory, domainId, &DDS_PARTICIPANT_QOS_DEFAULT, - NULL /* listener */, DDS_STATUS_MASK_NONE); - if (participant == NULL) { - fprintf(stderr, "create_participant error\n"); - subscriber_shutdown(participant); - return -1; - } - - /* To customize subscriber QoS, use - the configuration file USER_QOS_PROFILES.xml */ - subscriber = DDS_DomainParticipant_create_subscriber( - participant, &DDS_SUBSCRIBER_QOS_DEFAULT, NULL /* listener */, - DDS_STATUS_MASK_NONE); - if (subscriber == NULL) { - fprintf(stderr, "create_subscriber error\n"); - subscriber_shutdown(participant); - return -1; - } - - /* Register the type before creating the topic */ - retcode = SerializedTypeTypeSupport_register_external_type( - participant, - test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport_get_type_name(), - test_msgs_msg_dds__DynamicArrayPrimitives__get_typecode()); - - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "register_type error %d\n", retcode); - subscriber_shutdown(participant); - return -1; - } - - /* To customize topic QoS, use - the configuration file USER_QOS_PROFILES.xml */ - topic = DDS_DomainParticipant_create_topic( - participant, "my_generic_type", - test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport_get_type_name(), - &DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, - DDS_STATUS_MASK_NONE); - if (topic == NULL) { - fprintf(stderr, "create_topic error\n"); - subscriber_shutdown(participant); - return -1; - } - - /* Set up a data reader listener */ - reader_listener.on_requested_deadline_missed = - SerializedTypeListener_on_requested_deadline_missed; - reader_listener.on_requested_incompatible_qos = - SerializedTypeListener_on_requested_incompatible_qos; - reader_listener.on_sample_rejected = - SerializedTypeListener_on_sample_rejected; - reader_listener.on_liveliness_changed = - SerializedTypeListener_on_liveliness_changed; - reader_listener.on_sample_lost = - SerializedTypeListener_on_sample_lost; - reader_listener.on_subscription_matched = - SerializedTypeListener_on_subscription_matched; - reader_listener.on_data_available = - SerializedTypeListener_on_data_available; - - /* To customize data reader QoS, use - the configuration file USER_QOS_PROFILES.xml */ - reader = DDS_Subscriber_create_datareader( - subscriber, DDS_Topic_as_topicdescription(topic), - &DDS_DATAREADER_QOS_DEFAULT, &reader_listener, DDS_STATUS_MASK_ALL); - if (reader == NULL) { - fprintf(stderr, "create_datareader error\n"); - subscriber_shutdown(participant); - return -1; - } - - /* Main loop */ - for (count=0; (sample_count == 0) || (count < sample_count); ++count) { - printf("SerializedType subscriber sleeping for %d sec...\n", - poll_period.sec); - - NDDS_Utility_sleep(&poll_period); - } - - /* Cleanup and delete all entities */ - return subscriber_shutdown(participant); -} - -int main(int argc, char *argv[]) -{ - int domainId = 0; - int sample_count = 0; /* infinite loop */ - - if (argc >= 2) { - domainId = atoi(argv[1]); - } - if (argc >= 3) { - sample_count = atoi(argv[2]); - } - - /* Uncomment this to turn on additional logging - NDDS_Config_Logger_set_verbosity_by_category( - NDDS_Config_Logger_get_instance(), - NDDS_CONFIG_LOG_CATEGORY_API, - NDDS_CONFIG_LOG_VERBOSITY_STATUS_ALL); - */ - - return subscriber_main(domainId, sample_count); -} - diff --git a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data.h b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data.h deleted file mode 100644 index 5ae09454..00000000 --- a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data.h +++ /dev/null @@ -1,88 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef ConnextStaticRawData_1419555957_h -#define ConnextStaticRawData_1419555957_h - -#ifndef NDDS_STANDALONE_TYPE -#ifndef ndds_c_h -#include "rosidl_typesupport_connext_c/ndds_include.h" -#endif -#else -#include "ndds_standalone_type.h" -#endif - -#define KEY_HASH_LENGTH_16 (16) - -extern const char *ConnextStaticRawDataTYPENAME; - -typedef struct ConnextStaticRawData { - - DDS_Octet key_hash [(KEY_HASH_LENGTH_16)]; - struct DDS_OctetSeq serialized_key ; - struct DDS_OctetSeq serialized_data ; - -} ConnextStaticRawData ; -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) -#endif - -NDDSUSERDllExport DDS_TypeCode* ConnextStaticRawData_get_typecode(void); /* Type code */ - -DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData* self); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData* self,RTIBool allocatePointers,RTIBool allocateMemory); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData* self, - const struct DDS_TypeAllocationParams_t * allocParams); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize( - ConnextStaticRawData* self); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData* self,RTIBool deletePointers); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData* self, - const struct DDS_TypeDeallocationParams_t * deallocParams); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* self, RTIBool deletePointers); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData* dst, - const ConnextStaticRawData* src); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* ConnextStaticRawData */ - diff --git a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h deleted file mode 100644 index db05cbfa..00000000 --- a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h +++ /dev/null @@ -1,339 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef ConnextStaticRawDataPlugin_1419555957_h -#define ConnextStaticRawDataPlugin_1419555957_h - -#include "rosidl_typesupport_connext_c/connext_static_raw_data.h" - -struct RTICdrStream; - -#ifndef pres_typePlugin_h -#include "pres/pres_typePlugin.h" -#endif - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) -#endif - -/* The type used to store keys for instances of type struct -* AnotherSimple. -* -* By default, this type is struct ConnextStaticRawData -* itself. However, if for some reason this choice is not practical for your -* system (e.g. if sizeof(struct ConnextStaticRawData) -* is very large), you may redefine this typedef in terms of another type of -* your choosing. HOWEVER, if you define the KeyHolder type to be something -* other than struct AnotherSimple, the -* following restriction applies: the key of struct -* ConnextStaticRawData must consist of a -* single field of your redefined KeyHolder type and that field must be the -* first field in struct ConnextStaticRawData. -*/ -typedef struct ConnextStaticRawData ConnextStaticRawDataKeyHolder; - -#define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample -#define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer -#define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer - -#define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey -#define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey - -#define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample -#define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample - -/* -------------------------------------------------------------------------------------- -Support functions: -* -------------------------------------------------------------------------------------- */ - -NDDSUSERDllExport extern ConnextStaticRawData* -ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params); - -NDDSUSERDllExport extern ConnextStaticRawData* -ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); - -NDDSUSERDllExport extern ConnextStaticRawData* -ConnextStaticRawDataPluginSupport_create_data(void); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData *out, - const ConnextStaticRawData *in); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData *sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData *sample,RTIBool deallocate_pointers); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData *sample); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData *sample, - const char *desc, - unsigned int indent); - -NDDSUSERDllExport extern ConnextStaticRawData* -ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); - -NDDSUSERDllExport extern ConnextStaticRawData* -ConnextStaticRawDataPluginSupport_create_key(void); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder *key); - -/* ---------------------------------------------------------------------------- -Callback functions: -* ---------------------------------------------------------------------------- */ - -NDDSUSERDllExport extern PRESTypePluginParticipantData -ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, - const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, - void *container_plugin_context, - RTICdrTypeCode *typeCode); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data); - -NDDSUSERDllExport extern PRESTypePluginEndpointData -ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, - void *container_plugin_context); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - void *handle); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *out, - const ConnextStaticRawData *in); - -/* ---------------------------------------------------------------------------- -(De)Serialize functions: -* ------------------------------------------------------------------------- */ - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void *endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData *sample); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData *sample, - const char * buffer, - unsigned int length); -NDDSUSERDllExport extern DDS_ReturnCode_t -ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void *endpoint_plugin_qos); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample); - -/* -------------------------------------------------------------------------------------- -Key Management functions: -* -------------------------------------------------------------------------------------- */ -NDDSUSERDllExport extern PRESTypePluginKeyKind -ConnextStaticRawDataPlugin_get_key_kind(void); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void *endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData ** sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *key, - const ConnextStaticRawData *instance); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *instance, - const ConnextStaticRawDataKeyHolder *key); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t *keyhash, - const ConnextStaticRawData *instance); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - DDS_KeyHash_t *keyhash, - RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos); - -/* Plugin Functions */ -NDDSUSERDllExport extern struct PRESTypePlugin* -ConnextStaticRawDataPlugin_new(void); - -NDDSUSERDllExport extern struct PRESTypePlugin* -ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * type_code); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* ConnextStaticRawDataPlugin_1419555957_h */ - diff --git a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_support.h b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_support.h deleted file mode 100644 index bcd112d6..00000000 --- a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/connext_static_raw_data_support.h +++ /dev/null @@ -1,61 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef ConnextStaticRawDataSupport_1419555957_h -#define ConnextStaticRawDataSupport_1419555957_h - -/* Uses */ -#include "rosidl_typesupport_connext_c/connext_static_raw_data.h" - -#ifndef ndds_c_h -#include "rosidl_typesupport_connext_c/ndds_include.h" -#endif - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) - -#endif - -/* ========================================================================= */ -/** -Uses: T - -Defines: TTypeSupport, TDataWriter, TDataReader - -Organized using the well-documented "Generics Pattern" for -implementing generics in C and C++. -*/ - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) - -#endif - -DDS_TYPESUPPORT_C(ConnextStaticRawDataTypeSupport, ConnextStaticRawData); -DDS_DATAWRITER_C(ConnextStaticRawDataDataWriter, ConnextStaticRawData); -DDS_DATAREADER_C(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); - -NDDSUSERDllExport DDSCDllExport -DDS_ReturnCode_t ConnextStaticRawDataTypeSupport_register_external_type( - DDS_DomainParticipant* participant, - const char* type_name, - struct DDS_TypeCode *type_code); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* ConnextStaticRawDataSupport_1419555957_h */ - diff --git a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em index 533a7e8c..1d9c07f9 100644 --- a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em +++ b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em @@ -340,15 +340,20 @@ to_cdr_stream( } // call the serialize function for the first time to get the expected length of the message - if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(NULL, &cdr_stream->message_length, &dds_message) != RTI_TRUE) { + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( + NULL, &cdr_stream->message_length, &dds_message) != RTI_TRUE) + { return false; } // allocate enough memory for the CDR stream // TODO(karsten1987): This allocation has to be preallocated // or at least bring in a custom allocator - cdr_stream->raw_message = (char *)malloc(sizeof(char) * cdr_stream->message_length); + cdr_stream->raw_message = + reinterpret_cast(malloc(sizeof(char) * cdr_stream->message_length)); // call the function again and fill the buffer this time - if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->raw_message, &cdr_stream->message_length, &dds_message) != RTI_TRUE) { + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( + cdr_stream->raw_message, &cdr_stream->message_length, &dds_message) != RTI_TRUE) + { return false; } @@ -367,13 +372,16 @@ to_message( return false; } - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); - if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer(dds_message, cdr_stream->raw_message, cdr_stream->message_length) != RTI_TRUE) { + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); + if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer( + dds_message, cdr_stream->raw_message, cdr_stream->message_length) != RTI_TRUE) + { fprintf(stderr, "deserialize from cdr buffer failed\n"); return false; } bool success = convert_dds_to_ros(dds_message, untyped_ros_message); - if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK){ + if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK) { return false; } return success; diff --git a/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em b/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em index aac16340..c5047635 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__rosidl_typesupport_connext_cpp.hpp.em @@ -72,24 +72,12 @@ convert_ros_message_to_dds( const @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message, @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ & dds_message); -//bool -//publish__@(spec.base_type.type)( -// DDSDataWriter * topic_writer, -// const void * untyped_ros_message); - bool ROSIDL_TYPESUPPORT_CONNEXT_CPP_PUBLIC_@(spec.base_type.pkg_name) convert_dds_message_to_ros( const @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ & dds_message, @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message); -//bool -//take__@(spec.base_type.type)( -// DDSDataReader * topic_reader, -// bool ignore_local_publications, -// ConnextStaticCDRStream * untyped_ros_message, -// bool * taken); - bool to_cdr_stream__@(spec.base_type.type)( const void * untyped_ros_message, diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index 26bbdc21..b3ec7a21 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -231,7 +231,7 @@ to_cdr_stream__@(spec.base_type.type)( if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->raw_message, &cdr_stream->message_length, dds_message) != RTI_TRUE) { return false; } - if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK){ + if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK) { return false; } return true; @@ -261,7 +261,7 @@ to_message__@(spec.base_type.type)( @(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) & ros_message = *(@(spec.base_type.pkg_name)::@(subfolder)::@(spec.base_type.type) *)untyped_ros_message; bool success = convert_dds_message_to_ros(*dds_message, ros_message); - if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK){ + if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK) { return false; } return success; From b2a02ff382641dc308dbaa00889f6ceaeb8911b9 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 27 Feb 2018 16:50:10 -0800 Subject: [PATCH 12/34] remove unused files --- rosidl_typesupport_connext_c/CMakeLists.txt | 8 +- .../src/connext_static_raw_data.c | 302 ---- .../src/connext_static_raw_data_plugin.c | 1527 ----------------- .../src/connext_static_raw_data_support.c | 167 -- .../src/RawData_publisher.cxx | 280 --- .../src/RawData_subscriber.cxx | 270 --- 6 files changed, 1 insertion(+), 2553 deletions(-) delete mode 100644 rosidl_typesupport_connext_c/src/connext_static_raw_data.c delete mode 100644 rosidl_typesupport_connext_c/src/connext_static_raw_data_plugin.c delete mode 100644 rosidl_typesupport_connext_c/src/connext_static_raw_data_support.c delete mode 100644 rosidl_typesupport_connext_cpp/src/RawData_publisher.cxx delete mode 100644 rosidl_typesupport_connext_cpp/src/RawData_subscriber.cxx diff --git a/rosidl_typesupport_connext_c/CMakeLists.txt b/rosidl_typesupport_connext_c/CMakeLists.txt index c728fdc7..94914d9d 100644 --- a/rosidl_typesupport_connext_c/CMakeLists.txt +++ b/rosidl_typesupport_connext_c/CMakeLists.txt @@ -45,13 +45,7 @@ ament_export_include_directories(include) ament_python_install_package(${PROJECT_NAME}) -add_library( - ${PROJECT_NAME} - SHARED - src/connext_static_raw_data.c - src/connext_static_raw_data_plugin.c - src/connext_static_raw_data_support.c - src/identifier.cpp) +add_library(${PROJECT_NAME} SHARED src/identifier.cpp) if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSIDL_TYPESUPPORT_CONNEXT_C_BUILDING_DLL") diff --git a/rosidl_typesupport_connext_c/src/connext_static_raw_data.c b/rosidl_typesupport_connext_c/src/connext_static_raw_data.c deleted file mode 100644 index b3b1109e..00000000 --- a/rosidl_typesupport_connext_c/src/connext_static_raw_data.c +++ /dev/null @@ -1,302 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef NDDS_STANDALONE_TYPE -#ifndef ndds_c_h -#include "rosidl_typesupport_connext_c/ndds_include.h" -#endif - -#ifndef cdr_type_h -#include "cdr/cdr_type.h" -#endif - -#ifndef osapi_heap_h -#include "osapi/osapi_heap.h" -#endif -#else -#include "ndds_standalone_type.h" -#endif - -#include "rosidl_typesupport_connext_c/connext_static_raw_data.h" - -/* ========================================================================= */ -const char *ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; - -DDS_TypeCode* ConnextStaticRawData_get_typecode() -{ - static RTIBool is_initialized = RTI_FALSE; - - static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array =DDS_INITIALIZE_ARRAY_TYPECODE(1,(KEY_HASH_LENGTH_16), NULL,NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); - static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3]= - { - - { - (char *)"key_hash",/* Member name */ - { - 0,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_KEY_MEMBER , /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - }, - { - (char *)"serialized_key",/* Member name */ - { - 1,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - }, - { - (char *)"serialized_data",/* Member name */ - { - 2,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - } - }; - - static DDS_TypeCode ConnextStaticRawData_g_tc = - {{ - DDS_TK_STRUCT,/* Kind */ - DDS_BOOLEAN_FALSE, /* Ignored */ - -1, /*Ignored*/ - (char *)"ConnextStaticRawData", /* Name */ - NULL, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - 3, /* Number of members */ - ConnextStaticRawData_g_tc_members, /* Members */ - DDS_VM_NONE /* Ignored */ - }}; /* Type code for ConnextStaticRawData*/ - - if (is_initialized) { - return &ConnextStaticRawData_g_tc; - } - - ConnextStaticRawData_g_tc_key_hash_array._data._typeCode =(RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_members[0]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_key_hash_array; - ConnextStaticRawData_g_tc_members[1]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_key_sequence; - ConnextStaticRawData_g_tc_members[2]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_data_sequence; - - is_initialized = RTI_TRUE; - - return &ConnextStaticRawData_g_tc; -} - -RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData* sample) { - return ConnextStaticRawData_initialize_ex(sample,RTI_TRUE,RTI_TRUE); -} - -RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData* sample,RTIBool allocatePointers, RTIBool allocateMemory) -{ - - struct DDS_TypeAllocationParams_t allocParams = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; - - allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; - allocParams.allocate_memory = (DDS_Boolean)allocateMemory; - - return ConnextStaticRawData_initialize_w_params( - sample,&allocParams); - -} - -RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData* sample, const struct DDS_TypeAllocationParams_t * allocParams) -{ - - void* buffer = NULL; - if (buffer) {} /* To avoid warnings */ - - if (sample == NULL) { - return RTI_FALSE; - } - if (allocParams == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_initArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_key ); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key , RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_data ); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data , RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_data, 0); - } - return RTI_TRUE; -} - -void ConnextStaticRawData_finalize( - ConnextStaticRawData* sample) -{ - - ConnextStaticRawData_finalize_ex(sample,RTI_TRUE); -} - -void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData* sample,RTIBool deletePointers) -{ - struct DDS_TypeDeallocationParams_t deallocParams = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - - if (sample==NULL) { - return; - } - - deallocParams.delete_pointers = (DDS_Boolean)deletePointers; - - ConnextStaticRawData_finalize_w_params( - sample,&deallocParams); -} - -void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData* sample,const struct DDS_TypeDeallocationParams_t * deallocParams) -{ - - if (sample==NULL) { - return; - } - - if (deallocParams == NULL) { - return; - } - - DDS_OctetSeq_finalize(&sample->serialized_key); - - DDS_OctetSeq_finalize(&sample->serialized_data); - -} - -void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* sample, RTIBool deletePointers) -{ - struct DDS_TypeDeallocationParams_t deallocParamsTmp = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - struct DDS_TypeDeallocationParams_t * deallocParams = - &deallocParamsTmp; - - if (sample==NULL) { - return; - } - if (deallocParams) {} /* To avoid warnings */ - - deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; - deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; - -} - -RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData* dst, - const ConnextStaticRawData* src) -{ - - if (dst == NULL || src == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_key , - &src->serialized_key )) { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_data , - &src->serialized_data )) { - return RTI_FALSE; - } - - return RTI_TRUE; - -} - -/** -* <> -* -* Defines: TSeq, T -* -* Configure and implement 'ConnextStaticRawData' sequence class. -*/ -#define T ConnextStaticRawData -#define TSeq ConnextStaticRawDataSeq - -#define T_initialize_w_params ConnextStaticRawData_initialize_w_params - -#define T_finalize_w_params ConnextStaticRawData_finalize_w_params -#define T_copy ConnextStaticRawData_copy - -#ifndef NDDS_STANDALONE_TYPE -#include "dds_c/generic/dds_c_sequence_TSeq.gen" -#else -#include "dds_c_sequence_TSeq.gen" -#endif - -#undef T_copy -#undef T_finalize_w_params - -#undef T_initialize_w_params - -#undef TSeq -#undef T - diff --git a/rosidl_typesupport_connext_c/src/connext_static_raw_data_plugin.c b/rosidl_typesupport_connext_c/src/connext_static_raw_data_plugin.c deleted file mode 100644 index ec1985ac..00000000 --- a/rosidl_typesupport_connext_c/src/connext_static_raw_data_plugin.c +++ /dev/null @@ -1,1527 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#include - -#ifndef ndds_c_h -#include "rosidl_typesupport_connext_c/ndds_include.h" -#endif - -#ifndef osapi_type_h -#include "osapi/osapi_type.h" -#endif -#ifndef osapi_heap_h -#include "osapi/osapi_heap.h" -#endif - -#ifndef osapi_utility_h -#include "osapi/osapi_utility.h" -#endif - -#ifndef cdr_type_h -#include "cdr/cdr_type.h" -#endif - -#ifndef cdr_type_object_h -#include "cdr/cdr_typeObject.h" -#endif - -#ifndef cdr_encapsulation_h -#include "cdr/cdr_encapsulation.h" -#endif - -#ifndef cdr_stream_h -#include "cdr/cdr_stream.h" -#endif - -#ifndef cdr_log_h -#include "cdr/cdr_log.h" -#endif - -#ifndef pres_typePlugin_h -#include "pres/pres_typePlugin.h" -#endif - -#define RTI_CDR_CURRENT_SUBMODULE RTI_CDR_SUBMODULE_MASK_STREAM - -#include "rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h" - -/* ---------------------------------------------------------------------------- -* Type ConnextStaticRawData -* -------------------------------------------------------------------------- */ - -/* ----------------------------------------------------------------------------- -Support functions: -* -------------------------------------------------------------------------- */ - -ConnextStaticRawData* -ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params) -{ - ConnextStaticRawData *sample = NULL; - - RTIOsapiHeap_allocateStructure(&(sample),ConnextStaticRawData); - if (sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_w_params(sample,alloc_params)) { - RTIOsapiHeap_freeStructure(sample); - sample=NULL; - } - return sample; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) -{ - ConnextStaticRawData *sample = NULL; - - RTIOsapiHeap_allocateStructure(&(sample),ConnextStaticRawData); - - if(sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_ex(sample,allocate_pointers, RTI_TRUE)) { - RTIOsapiHeap_freeStructure(sample); - sample=NULL; - } - - return sample; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data(void) -{ - return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); -} - -void -ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData *sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params) { - - ConnextStaticRawData_finalize_w_params(sample,dealloc_params); - - RTIOsapiHeap_freeStructure(sample); - sample=NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData *sample,RTIBool deallocate_pointers) { - - ConnextStaticRawData_finalize_ex(sample,deallocate_pointers); - - RTIOsapiHeap_freeStructure(sample); - sample=NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData *sample) { - - ConnextStaticRawDataPluginSupport_destroy_data_ex(sample,RTI_TRUE); - -} - -RTIBool -ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData *dst, - const ConnextStaticRawData *src) -{ - return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); -} - -void -ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData *sample, - const char *desc, - unsigned int indent_level) -{ - - RTICdrType_printIndent(indent_level); - - if (desc != NULL) { - RTILog_debug("%s:\n", desc); - } else { - RTILog_debug("\n"); - } - - if (sample == NULL) { - RTILog_debug("NULL\n"); - return; - } - - RTICdrType_printArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "key_hash", indent_level + 1); - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key ), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data ), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } - -} -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers){ - ConnextStaticRawData *key = NULL; - - RTIOsapiHeap_allocateStructure(&(key),ConnextStaticRawDataKeyHolder); - - ConnextStaticRawData_initialize_ex(key,allocate_pointers, RTI_TRUE); - - return key; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key(void) -{ - return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); -} - -void -ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) -{ - ConnextStaticRawData_finalize_ex(key,deallocate_pointers); - - RTIOsapiHeap_freeStructure(key); - key=NULL; - -} - -void -ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder *key) { - - ConnextStaticRawDataPluginSupport_destroy_key_ex(key,RTI_TRUE); - -} - -/* ---------------------------------------------------------------------------- -Callback functions: -* ---------------------------------------------------------------------------- */ - -PRESTypePluginParticipantData -ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, - const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, - void *container_plugin_context, - RTICdrTypeCode *type_code) -{ - if (registration_data) {} /* To avoid warnings */ - if (participant_info) {} /* To avoid warnings */ - if (top_level_registration) {} /* To avoid warnings */ - if (container_plugin_context) {} /* To avoid warnings */ - if (type_code) {} /* To avoid warnings */ - - return PRESTypePluginDefaultParticipantData_new(participant_info); - -} - -void -ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data) -{ - - PRESTypePluginDefaultParticipantData_delete(participant_data); -} - -PRESTypePluginEndpointData -ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, - void *containerPluginContext) -{ - PRESTypePluginEndpointData epd = NULL; - - unsigned int serializedSampleMaxSize; - - unsigned int serializedKeyMaxSize; - - if (top_level_registration) {} /* To avoid warnings */ - if (containerPluginContext) {} /* To avoid warnings */ - - epd = PRESTypePluginDefaultEndpointData_new( - participant_data, - endpoint_info, - (PRESTypePluginDefaultEndpointDataCreateSampleFunction) - ConnextStaticRawDataPluginSupport_create_data, - (PRESTypePluginDefaultEndpointDataDestroySampleFunction) - ConnextStaticRawDataPluginSupport_destroy_data, - (PRESTypePluginDefaultEndpointDataCreateKeyFunction) - ConnextStaticRawDataPluginSupport_create_key , - (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) - ConnextStaticRawDataPluginSupport_destroy_key); - - if (epd == NULL) { - return NULL; - } - serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( - epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); - - if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( - epd,endpoint_info,serializedKeyMaxSize)) - { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - - if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { - serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); - - PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); - - if (PRESTypePluginDefaultEndpointData_createWriterPool( - epd, - endpoint_info, - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size, - epd) == RTI_FALSE) { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - } - - return epd; -} - -void -ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data) -{ - - PRESTypePluginDefaultEndpointData_delete(endpoint_data); -} - -void -ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - void *handle) -{ - - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - - PRESTypePluginDefaultEndpointData_returnSample( - endpoint_data, sample, handle); -} - -RTIBool -ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *dst, - const ConnextStaticRawData *src) -{ - if (endpoint_data) {} /* To avoid warnings */ - return ConnextStaticRawDataPluginSupport_copy_data(dst,src); -} - -/* ---------------------------------------------------------------------------- -(De)Serialize functions: -* ------------------------------------------------------------------------- */ -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -/** - * This plugin can only be used as a top-level type. - * It expects the ConnextStaticRawData to contain data serialized with its - * encapsulation header already there. - * - * @param serialize_encapsulation normally it indicates whether encapsulaton should be serialized - * it also indicates whether we are serializing the data as a a top-level type (in the - * RTPSSerializedPayload or we are serializing as part of a nested type). - For ConnextStaticRawDataPlugin_serialize() it must be set to TRUE - * - * @param encapsulation_id the encapsulation identifier in case serialize_encapsulation==TRUE - * the implementation of ConnextStaticRawDataPlugin_serialize checks that the value of - * encapsulation_id matches the first two bytes in sample->key_hash - */ -RTIBool -ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void *endpoint_plugin_qos) -{ - char * position = NULL; - RTIBool retval = RTI_TRUE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - /* This plugin can only be used to publish the top-level DDS Topic-Type - * in which case serialize_encapsulation==TRUE. If that is not - * the case then it is an error. - */ - if (!serialize_encapsulation) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - - if(serialize_sample) { - /* The sample->serialized_data contains the serialized encapsulation followed by the serialized - * data, so we only need to copy that into - * the CDR stream. Not the key_hash, not the length of the data itself - * The ConnextStaticRawData sample->serialized_data is always a contiguous buffer - */ - DDS_Octet *buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); - if ( buffer == NULL ) { - return RTI_FALSE; - } - - /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian - * byte order - */ - if ( encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { - return RTI_FALSE; - } - - /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ - if (!RTICdrStream_serializePrimitiveArray( - stream, (void*)buffer, - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } - - RTICdrStream_restoreAlignment(stream ,position); - - return retval; -} - - -/** - TODO. The code-block below does not belong here. - It should be pushed to the CDR module, perhaps inside - RTICdrStream_deserializeAndSetCdrEncapsulation so that the - stream size is alredy correct when ConnextStaticRawDataPlugin_deserialize_sample - is called. - - Adjust the size of the CDR stream to not include the alignment - padding. See http://issues.omg.org/browse/DDSXTY12-10 - - @precondition The RTICdrStream *stream has alreadt processed - the encapsulation header and therefore has set the - encapsulation options returned by - RTICdrStream_getEncapsulationOptions() -*/ -void -ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream *stream) -{ - /* See http://issues.omg.org/browse/DDSXTY12-10 */ - DDS_UnsignedShort padding_size_mask = 0x0003; - DDS_UnsignedShort padding_size; - int adjustedBufferLength; - - padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; - adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; - RTICdrStream_setBufferLength(stream, adjustedBufferLength); -} - -/** -* This plugin can only be used as a top-level type. -* It expects the RTICdrStream to contain the encapsulatio header -* followed by the serialized data itself -*/ -RTIBool -ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos) -{ - char * position = NULL; - RTIBool done = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - /* This plugin can only be used to publish the top-level DDS Topic-Type - * in which case deserialize_encapsulation==TRUE. If that is not - * the case then it is an error. - */ - if (!deserialize_encapsulation) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - - /* TODO. The call does not belong here. It should be pushed - * inside RTICdrStream_deserializeAndSetCdrEncapsulation - */ - ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); - - if (deserialize_sample) { - /* Note that sample->key_hash was already set by ConnextStaticRawDataPlugin_deserialize() - it is done there because ConnextStaticRawDataPlugin_deserialize_sample does not - have access to the SampleInfo where that information is - */ - - /* We do not set the serialized_key on deserialization */ - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - - /* We copy everything that remains in the CDR stream */ - int bytesLeftInStream = RTICdrStream_getRemainder(stream); - DDS_Octet *cdrBufferPtr = (DDS_Octet *)RTICdrStream_getCurrentPosition(stream); - if (cdrBufferPtr == NULL) { - goto fin; - } - - /* Do not call ConnextStaticRawData_initialize_ex initialize here - because it would override the key_hash field - ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); - */ - if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { - goto fin; - } - RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); - } - - done = RTI_TRUE; - - fin: - if ( (done != RTI_TRUE) && - (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) { - return RTI_FALSE; - } - - RTICdrStream_restoreAlignment(stream,position); - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData *sample) -{ - struct RTICdrStream stream; - struct PRESTypePluginDefaultEndpointData epd; - RTIBool result; - - if (length == NULL) { - return RTI_FALSE; - } - - epd._maxSizeSerializedSample = - ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); - - if (buffer == NULL) { - *length = - ConnextStaticRawDataPlugin_get_serialized_sample_size( - (PRESTypePluginEndpointData)&epd, - RTI_TRUE, - RTICdrEncapsulation_getNativeCdrEncapsulationId(), - 0, - sample); - - if (*length == 0) { - return RTI_FALSE; - } - - return RTI_TRUE; - } - - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, *length); - - result = ConnextStaticRawDataPlugin_serialize( - (PRESTypePluginEndpointData)&epd, sample, &stream, - RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), - RTI_TRUE, NULL); - - *length = RTICdrStream_getCurrentPositionOffset(&stream); - return result; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData *sample, - const char * buffer, - unsigned int length) -{ - struct RTICdrStream stream; - - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, length); - - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - return ConnextStaticRawDataPlugin_deserialize_sample( - NULL, sample, - &stream, RTI_TRUE, RTI_TRUE, - NULL); -} - -DDS_ReturnCode_t -ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property) -{ - DDS_DynamicData *data = NULL; - char *buffer = NULL; - unsigned int length = 0; - struct DDS_PrintFormat printFormat; - DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; - - if (sample == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (str_size == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (property == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - NULL, - &length, - sample)) { - return DDS_RETCODE_ERROR; - } - - RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); - if (buffer == NULL) { - return DDS_RETCODE_ERROR; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - buffer, - &length, - sample)) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - data = DDS_DynamicData_new( - ConnextStaticRawData_get_typecode(), - &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); - if (data == NULL) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - retCode = DDS_PrintFormatProperty_to_print_format( - property, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - retCode = DDS_DynamicDataFormatter_to_string_w_format( - data, - str, - str_size, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return DDS_RETCODE_OK; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos) -{ - - RTIBool result; - const char *METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; - if (drop_sample) {} /* To avoid warnings */ - - stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_sample( - endpoint_data, (sample != NULL)?*sample:NULL, - stream, deserialize_encapsulation, deserialize_sample, - endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } - } - if (!result && stream->_xTypesState.unassignable ) { - - RTICdrLog_exception( - METHOD_NAME, - &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, - "ConnextStaticRawData"); - - } - - return result; - -} - -RTIBool ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(skip_encapsulation) { - if (!RTICdrStream_skipEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if (skip_sample) { - - if (!RTICdrStream_skipPrimitiveArray( - stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - goto fin; - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - } - - done = RTI_TRUE; - fin: - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - if(skip_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - if (endpoint_data) {} /* To avoid warnings */ - if (include_encapsulation) {} - if (encapsulation_id) {} - if (current_alignment) {} - - if (overflow != NULL) { - *overflow = RTI_TRUE; - } - - return RTI_CDR_MAX_SERIALIZED_SIZE; - -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int size; - RTIBool overflow = RTI_FALSE; - - size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); - - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } - - return size; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - - if (endpoint_data) {} /* To avoid warnings */ - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - } - - current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment,0, RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment,0, RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -/* Returns the size of the sample in its serialized form (in bytes). -* It can also be an estimation in excess of the real buffer needed -* during a call to the serialize() function. -* The value reported does not have to include the space for the -* encapsulation flags. -*/ -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - struct PRESTypePluginDefaultEndpointData epd; - - if (sample==NULL) { - return 0; - } - if (endpoint_data == NULL) { - endpoint_data = (PRESTypePluginEndpointData) &epd; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -/* -------------------------------------------------------------------------------------- -Key Management functions: -* -------------------------------------------------------------------------------------- */ - -PRESTypePluginKeyKind -ConnextStaticRawDataPlugin_get_key_kind_USER_KEY(void) -{ - return PRES_TYPEPLUGIN_USER_KEY; -} - -PRESTypePluginKeyKind -ConnextStaticRawDataPlugin_get_key_kind_NO_KEY(void) -{ - return PRES_TYPEPLUGIN_NO_KEY; -} - - -PRESTypePluginKeyKind -ConnextStaticRawDataPlugin_get_key_kind(void) -{ - return PRES_TYPEPLUGIN_USER_KEY; -} - -RTIBool -ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(serialize_encapsulation) { - if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if(serialize_key) { - - if (!RTICdrStream_serializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - } - - if(serialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - - char * position = NULL; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(deserialize_encapsulation) { - - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - if (deserialize_key) { - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; - -} - -RTIBool ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - RTIBool result; - if (drop_sample) {} /* To avoid warnings */ - stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_key_sample( - endpoint_data, (sample != NULL)?*sample:NULL, stream, - deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } - } - - return result; - -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - - if (endpoint_data) {} /* To avoid warnings */ - if (overflow) {} /* To avoid warnings */ - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - } - - current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int size; - RTIBool overflow = RTI_FALSE; - - size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); - - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } - - return size; -} - -RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - if(deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - position = RTICdrStream_resetAlignment(stream); - } - - if (deserialize_key) { - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - - } - - done = RTI_TRUE; - fin: - if(!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - } else { - return RTI_FALSE; - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *dst, - const ConnextStaticRawData *src) -{ - - if (endpoint_data) {} /* To avoid warnings */ - - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *dst, const - ConnextStaticRawDataKeyHolder *src) -{ - - if (endpoint_data) {} /* To avoid warnings */ - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t *keyhash, - const ConnextStaticRawData *instance) -{ - struct RTICdrStream * md5Stream = NULL; - struct RTICdrStreamState cdrState; - char * buffer = NULL; - - RTICdrStreamState_init(&cdrState); - md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); - - if (md5Stream == NULL) { - return RTI_FALSE; - } - - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - int size; - - RTICdrStream_pushState(md5Stream, &cdrState, -1); - - size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( - endpoint_data, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - 0, - instance); - - if (size <= RTICdrStream_getBufferLength(md5Stream)) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTIOsapiHeap_allocateBuffer(&buffer,size,0); - - if (buffer == NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTICdrStream_set(md5Stream, buffer, size); - RTIOsapiMemory_zero( - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getBufferLength(md5Stream)); - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - return RTI_FALSE; - } - } - - if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > - (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || - PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { - RTICdrStream_computeMD5(md5Stream, keyhash->value); - } else { - RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); - RTIOsapiMemory_copy( - keyhash->value, - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getCurrentPositionOffset(md5Stream)); - } - - keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; - - if (buffer != NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - } - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - DDS_KeyHash_t *keyhash, - RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - ConnextStaticRawData * sample=NULL; - - if (endpoint_plugin_qos) {} /* To avoid warnings */ - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - - if(deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - sample = (ConnextStaticRawData *) - PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); - - if (sample == NULL) { - return RTI_FALSE; - } - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - done = RTI_TRUE; - fin: - if(!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - } else { - return RTI_FALSE; - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - if (!ConnextStaticRawDataPlugin_instance_to_keyhash( - endpoint_data, keyhash, sample)) { - return RTI_FALSE; - } - - return RTI_TRUE; -} - -/* ------------------------------------------------------------------------ -* Plug-in Installation Methods -* ------------------------------------------------------------------------ */ -struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) -{ - return NULL; -} - -/* The following need to made public. They are defined in TypeCodeSupport.c */ -extern DDS_Boolean DDS_TypeCode_is_type_keyed(const DDS_TypeCode *type); - -struct PRESTypePlugin * ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * type_code) -{ - struct PRESTypePlugin *plugin = NULL; - const struct PRESTypePluginVersion PLUGIN_VERSION = - PRES_TYPE_PLUGIN_VERSION_2_0; - - RTIOsapiHeap_allocateStructure( - &plugin, struct PRESTypePlugin); - - if (plugin == NULL) { - return NULL; - } - - plugin->version = PLUGIN_VERSION; - - /* set up parent's function pointers */ - plugin->onParticipantAttached = - (PRESTypePluginOnParticipantAttachedCallback) - ConnextStaticRawDataPlugin_on_participant_attached; - plugin->onParticipantDetached = - (PRESTypePluginOnParticipantDetachedCallback) - ConnextStaticRawDataPlugin_on_participant_detached; - plugin->onEndpointAttached = - (PRESTypePluginOnEndpointAttachedCallback) - ConnextStaticRawDataPlugin_on_endpoint_attached; - plugin->onEndpointDetached = - (PRESTypePluginOnEndpointDetachedCallback) - ConnextStaticRawDataPlugin_on_endpoint_detached; - - plugin->copySampleFnc = - (PRESTypePluginCopySampleFunction) - ConnextStaticRawDataPlugin_copy_sample; - plugin->createSampleFnc = - (PRESTypePluginCreateSampleFunction) - ConnextStaticRawDataPlugin_create_sample; - plugin->destroySampleFnc = - (PRESTypePluginDestroySampleFunction) - ConnextStaticRawDataPlugin_destroy_sample; - - plugin->serializeFnc = - (PRESTypePluginSerializeFunction) - ConnextStaticRawDataPlugin_serialize; - plugin->deserializeFnc = - (PRESTypePluginDeserializeFunction) - ConnextStaticRawDataPlugin_deserialize; - plugin->getSerializedSampleMaxSizeFnc = - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size; - plugin->getSerializedSampleMinSizeFnc = - (PRESTypePluginGetSerializedSampleMinSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_min_size; - - plugin->getSampleFnc = - (PRESTypePluginGetSampleFunction) - ConnextStaticRawDataPlugin_get_sample; - plugin->returnSampleFnc = - (PRESTypePluginReturnSampleFunction) - ConnextStaticRawDataPlugin_return_sample; - - if ( (type_code != NULL) && DDS_TypeCode_is_type_keyed(type_code) ) { - plugin->getKeyKindFnc = (PRESTypePluginGetKeyKindFunction) - ConnextStaticRawDataPlugin_get_key_kind_USER_KEY; - } - else { - plugin->getKeyKindFnc = (PRESTypePluginGetKeyKindFunction) - ConnextStaticRawDataPlugin_get_key_kind_NO_KEY; - } - - plugin->getSerializedKeyMaxSizeFnc = - (PRESTypePluginGetSerializedKeyMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_key_max_size; - plugin->serializeKeyFnc = - (PRESTypePluginSerializeKeyFunction) - ConnextStaticRawDataPlugin_serialize_key; - plugin->deserializeKeyFnc = - (PRESTypePluginDeserializeKeyFunction) - ConnextStaticRawDataPlugin_deserialize_key; - plugin->deserializeKeySampleFnc = - (PRESTypePluginDeserializeKeySampleFunction) - ConnextStaticRawDataPlugin_deserialize_key_sample; - - plugin-> instanceToKeyHashFnc = - (PRESTypePluginInstanceToKeyHashFunction) - ConnextStaticRawDataPlugin_instance_to_keyhash; - plugin->serializedSampleToKeyHashFnc = - (PRESTypePluginSerializedSampleToKeyHashFunction) - ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; - - plugin->getKeyFnc = - (PRESTypePluginGetKeyFunction) - ConnextStaticRawDataPlugin_get_key; - plugin->returnKeyFnc = - (PRESTypePluginReturnKeyFunction) - ConnextStaticRawDataPlugin_return_key; - - plugin->instanceToKeyFnc = - (PRESTypePluginInstanceToKeyFunction) - ConnextStaticRawDataPlugin_instance_to_key; - plugin->keyToInstanceFnc = - (PRESTypePluginKeyToInstanceFunction) - ConnextStaticRawDataPlugin_key_to_instance; - plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ - plugin->typeCode = (struct RTICdrTypeCode *)type_code; - - plugin->languageKind = PRES_TYPEPLUGIN_DDS_TYPE; - - /* Serialized buffer */ - plugin->getBuffer = - (PRESTypePluginGetBufferFunction) - ConnextStaticRawDataPlugin_get_buffer; - plugin->returnBuffer = - (PRESTypePluginReturnBufferFunction) - ConnextStaticRawDataPlugin_return_buffer; - plugin->getSerializedSampleSizeFnc = - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size; - - plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; - - return plugin; -} - -void -ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) -{ - RTIOsapiHeap_freeStructure(plugin); -} -#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rosidl_typesupport_connext_c/src/connext_static_raw_data_support.c b/rosidl_typesupport_connext_c/src/connext_static_raw_data_support.c deleted file mode 100644 index 6bacc179..00000000 --- a/rosidl_typesupport_connext_c/src/connext_static_raw_data_support.c +++ /dev/null @@ -1,167 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#include "rosidl_typesupport_connext_c/connext_static_raw_data_support.h" -#include "rosidl_typesupport_connext_c/connext_static_raw_data_plugin.h" - -/* ========================================================================= */ -/** -<> - -Defines: TData, -TDataWriter, -TDataReader, -TTypeSupport - -Configure and implement 'ConnextStaticRawData' support classes. - -Note: Only the #defined classes get defined -*/ - -/* ----------------------------------------------------------------- */ -/* DDSDataWriter -*/ - -/** -<> - -Defines: TDataWriter, TData -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME - -/* Defines */ -#define TDataWriter ConnextStaticRawDataDataWriter -#define TData ConnextStaticRawData - -#include "dds_c/generic/dds_c_data_TDataWriter.gen" - -#undef TDataWriter -#undef TData - -#undef TTYPENAME - -/* ----------------------------------------------------------------- */ -/* DDSDataReader -*/ - -/** -<> - -Defines: TDataReader, TDataSeq, TData -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME - -/* Defines */ -#define TDataReader ConnextStaticRawDataDataReader -#define TDataSeq ConnextStaticRawDataSeq -#define TData ConnextStaticRawData - -#include "dds_c/generic/dds_c_data_TDataReader.gen" - -#undef TDataReader -#undef TDataSeq -#undef TData - -#undef TTYPENAME - -/* ----------------------------------------------------------------- */ -/* TypeSupport - -<> - -Requires: TTYPENAME, -TPlugin_new -TPlugin_delete -Defines: TTypeSupport, TData, TDataReader, TDataWriter -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME -#define TPlugin_new ConnextStaticRawDataPlugin_new -#define TPlugin_delete ConnextStaticRawDataPlugin_delete - -/* Defines */ -#define TTypeSupport ConnextStaticRawDataTypeSupport -#define TData ConnextStaticRawData -#define TDataReader ConnextStaticRawDataDataReader -#define TDataWriter ConnextStaticRawDataDataWriter -#define TGENERATE_SER_CODE -#define TGENERATE_TYPECODE - -#include "dds_c/generic/dds_c_data_TTypeSupport.gen" - -#undef TTypeSupport -#undef TData -#undef TDataReader -#undef TDataWriter -#undef TGENERATE_TYPECODE -#undef TGENERATE_SER_CODE -#undef TTYPENAME -#undef TPlugin_new -#undef TPlugin_delete - -/* - The ConnextStaticRawData needs to be registered passing the - TypeCode of the underlying type. That will ensure the right - type information is propagated on the wire. - - Note that ConnextStaticRawDataTypeSupport_register_type() is defined by - the macros above. We can't avoid it the way the code is generated. - That function should not be called. Instead the application should - call TTypeSupport_register_external_type() - - serialized_key_max_size should be set to 0 if the type is Unkeyed. Otherwise - it shoud be set to the maximum size of the serialized key of the - underlying type defined by the type_code paramameter. - - TODO: Would be nice to redefine ConnextStaticRawDataTypeSupport_register_type() - to have the implementation below instead of having to add a new register_external_type() - function. At least we could find a way to prevent register_type() from being - called... -*/ -DDS_ReturnCode_t ConnextStaticRawDataTypeSupport_register_external_type( - DDS_DomainParticipant* participant, - const char* type_name, - struct DDS_TypeCode *type_code) -{ - struct PRESTypePlugin *presTypePlugin = NULL; - DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; - - if (participant == NULL) { - goto finError; - } - - /* TODO pass the type_code */ - presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); - if (presTypePlugin == NULL) { - goto finError; - } - - retcode = DDS_DomainParticipant_register_type( - participant, - type_name, - presTypePlugin, - NULL /* registration_data */); - if (retcode != DDS_RETCODE_OK) { - goto finError; - } - return DDS_RETCODE_OK; - -finError: - if (presTypePlugin != NULL) { - ConnextStaticRawDataPlugin_delete(presTypePlugin); - } - - return retcode; -} diff --git a/rosidl_typesupport_connext_cpp/src/RawData_publisher.cxx b/rosidl_typesupport_connext_cpp/src/RawData_publisher.cxx deleted file mode 100644 index 31dd0f7b..00000000 --- a/rosidl_typesupport_connext_cpp/src/RawData_publisher.cxx +++ /dev/null @@ -1,280 +0,0 @@ - -/* RawData_publisher.cxx - -A publication of data of type RawData - -This file is derived from code automatically generated by the rtiddsgen -command: - -rtiddsgen -language C++ -example RawData.idl - -Example publication of type RawData automatically generated by -'rtiddsgen'. To test them follow these steps: - -(1) Compile this file and the example subscription. - -(2) Start the subscription with the command -objs//RawData_subscriber - -(3) Start the publication with the command -objs//RawData_publisher - -(4) [Optional] Specify the list of discovery initial peers and -multicast receive addresses via an environment variable or a file -(in the current working directory) called NDDS_DISCOVERY_PEERS. - -You can run any number of publishers and subscribers programs, and can -add and remove them dynamically from the domain. - -Example: - -To run the example application on domain : - -On Unix: - -objs//RawData_publisher o -objs//RawData_subscriber - -On Windows: - -objs\\RawData_publisher -objs\\RawData_subscriber - -*/ - -#include -#include - -#include "RawData.h" -#include "RawDataSupport.h" -#include "ndds/ndds_cpp.h" - -#include "DynamicArrayPrimitives_.h" -#include "DynamicArrayPrimitives_Plugin.h" -#include "DynamicArrayPrimitives_Support.h" - -/* Delete all entities */ -static int publisher_shutdown( - DDSDomainParticipant *participant) -{ - DDS_ReturnCode_t retcode; - int status = 0; - - if (participant != NULL) { - retcode = participant->delete_contained_entities(); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "delete_contained_entities error %d\n", retcode); - status = -1; - } - - retcode = DDSTheParticipantFactory->delete_participant(participant); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "delete_participant error %d\n", retcode); - status = -1; - } - } - - /* RTI Connext provides finalize_instance() method on - domain participant factory for people who want to release memory used - by the participant factory. Uncomment the following block of code for - clean destruction of the singleton. */ - /* - - retcode = DDSDomainParticipantFactory::finalize_instance(); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "finalize_instance error %d\n", retcode); - status = -1; - } - */ - - return status; -} - -extern "C" int publisher_main(int domainId, int sample_count) -{ - DDSDomainParticipant *participant = NULL; - DDSPublisher *publisher = NULL; - DDSTopic *topic = NULL; - DDSDataWriter *writer = NULL; - RawDataDataWriter * RawData_writer = NULL; - RawData *instance = NULL; - DDS_ReturnCode_t retcode; - DDS_InstanceHandle_t instance_handle = DDS_HANDLE_NIL; - const char *type_name = NULL; - struct DDS_TypeCode *type_code = NULL; - int count = 0; - DDS_Duration_t send_period = {4,0}; - - /* To customize participant QoS, use - the configuration file USER_QOS_PROFILES.xml */ - participant = DDSTheParticipantFactory->create_participant( - domainId, DDS_PARTICIPANT_QOS_DEFAULT, - NULL /* listener */, DDS_STATUS_MASK_NONE); - if (participant == NULL) { - fprintf(stderr, "create_participant error\n"); - publisher_shutdown(participant); - return -1; - } - - /* To customize publisher QoS, use - the configuration file USER_QOS_PROFILES.xml */ - publisher = participant->create_publisher( - DDS_PUBLISHER_QOS_DEFAULT, NULL /* listener */, DDS_STATUS_MASK_NONE); - if (publisher == NULL) { - fprintf(stderr, "create_publisher error\n"); - publisher_shutdown(participant); - return -1; - } - - /* Register type before creating topic */ - type_name = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::get_type_name(); - type_code = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::get_typecode(); - retcode = RawDataSupport_register_external_type( - participant, type_name, type_code); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "register_type error %d\n", retcode); - publisher_shutdown(participant); - return -1; - } - - fprintf(stderr, "%s\n", type_name); - - /* To customize topic QoS, use - the configuration file USER_QOS_PROFILES.xml */ - topic = participant->create_topic( - "my_generic_type", - type_name, - DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, - DDS_STATUS_MASK_NONE); - if (topic == NULL) { - fprintf(stderr, "create_topic error\n"); - publisher_shutdown(participant); - return -1; - } - - /* To customize data writer QoS, use - the configuration file USER_QOS_PROFILES.xml */ - writer = publisher->create_datawriter( - topic, DDS_DATAWRITER_QOS_DEFAULT, NULL /* listener */, - DDS_STATUS_MASK_NONE); - if (writer == NULL) { - fprintf(stderr, "create_datawriter error\n"); - publisher_shutdown(participant); - return -1; - } - RawData_writer = RawDataDataWriter::narrow(writer); - if (RawData_writer == NULL) { - fprintf(stderr, "DataWriter narrow error\n"); - publisher_shutdown(participant); - return -1; - } - - /* Create data sample for writing */ - instance = RawDataTypeSupport::create_data(); - instance->serialized_data.maximum(0); - if (instance == NULL) { - fprintf(stderr, "RawDataTypeSupport::create_data error\n"); - publisher_shutdown(participant); - return -1; - } - - /* For a data type that has a key, if the same instance is going to be - written multiple times, initialize the key here - and register the keyed instance prior to writing */ - instance_handle = RawData_writer->register_instance(*instance); - - - test_msgs_msg_dds__DynamicArrayPrimitives_ * dynamic_array_instance = - test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::create_data(); - size_t length = 4; - dynamic_array_instance->int32_values_.ensure_length(length, length); - for (size_t i = 0; i < length; ++i) { - dynamic_array_instance->int32_values_[i] = i; - } - strcpy(dynamic_array_instance->my_key, "abc"); - - unsigned int serialization_buffer_size = test_msgs_msg_dds__DynamicArrayPrimitives_Plugin_get_serialized_sample_max_size(NULL, RTI_TRUE, 0, 0); - fprintf(stderr, "serialization buffer size %u\n", serialization_buffer_size); - if (serialization_buffer_size == RTI_CDR_MAX_SERIALIZED_SIZE) { - fprintf(stderr, "max size ohje ohje\n"); - serialization_buffer_size = 1024; - } - DDS_Octet * serialization_buffer = (DDS_Octet *)malloc(serialization_buffer_size); - for (unsigned int i = 0; i < serialization_buffer_size; ++i) { - serialization_buffer[i] = 0; - } - - if (!test_msgs_msg_dds__DynamicArrayPrimitives_Plugin_serialize_to_cdr_buffer( - (char *)serialization_buffer, &serialization_buffer_size, dynamic_array_instance)) { - fprintf(stderr, "Serialization of DyanmicArrayType failed\n"); - return false; - } - - for (unsigned int i = 0; i < serialization_buffer_size; ++i) { - fprintf(stderr, "%02x ", serialization_buffer[i]); - } - fprintf(stderr, "\n"); - instance->serialized_data.loan_contiguous( - serialization_buffer, serialization_buffer_size, serialization_buffer_size); - - for (unsigned int i = 0; i < serialization_buffer_size; ++i) { - fprintf(stderr, "%02x ", instance->serialized_data[i]); - } - fprintf(stderr, "\n"); - - /* Main loop */ - for (count=0; (sample_count == 0) || (count < sample_count); ++count) { - - printf("Writing RawData, count %d\n", count); - - /* Modify the data to be sent here */ - - retcode = RawData_writer->write(*instance, instance_handle); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "write error %d\n", retcode); - } - - NDDSUtility::sleep(send_period); - } - - instance->serialized_data.unloan(); - free(serialization_buffer); - /* - retcode = RawData_writer->unregister_instance( - *instance, instance_handle); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "unregister instance error %d\n", retcode); - } - */ - - /* Delete data sample */ - retcode = RawDataTypeSupport::delete_data(instance); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "RawDataTypeSupport::delete_data error %d\n", retcode); - } - - /* Delete all entities */ - return publisher_shutdown(participant); -} - -int main(int argc, char *argv[]) -{ - int domainId = 0; - int sample_count = 0; /* infinite loop */ - - if (argc >= 2) { - domainId = atoi(argv[1]); - } - if (argc >= 3) { - sample_count = atoi(argv[2]); - } - - /* Uncomment this to turn on additional logging - NDDSConfigLogger::get_instance()-> - set_verbosity_by_category(NDDS_CONFIG_LOG_CATEGORY_API, - NDDS_CONFIG_LOG_VERBOSITY_STATUS_ALL); - */ - - return publisher_main(domainId, sample_count); -} - diff --git a/rosidl_typesupport_connext_cpp/src/RawData_subscriber.cxx b/rosidl_typesupport_connext_cpp/src/RawData_subscriber.cxx deleted file mode 100644 index 429ce0b9..00000000 --- a/rosidl_typesupport_connext_cpp/src/RawData_subscriber.cxx +++ /dev/null @@ -1,270 +0,0 @@ - -/* RawData_subscriber.cxx - -A subscription example - -This file is derived from code automatically generated by the rtiddsgen -command: - -rtiddsgen -language C++ -example RawData.idl - -Example subscription of type RawData automatically generated by -'rtiddsgen'. To test them follow these steps: - -(1) Compile this file and the example publication. - -(2) Start the subscription with the command -objs//RawData_subscriber - -(3) Start the publication with the command -objs//RawData_publisher - -(4) [Optional] Specify the list of discovery initial peers and -multicast receive addresses via an environment variable or a file -(in the current working directory) called NDDS_DISCOVERY_PEERS. - -You can run any number of publishers and subscribers programs, and can -add and remove them dynamically from the domain. - -Example: - -To run the example application on domain : - -On Unix: - -objs//RawData_publisher -objs//RawData_subscriber - -On Windows: - -objs\\RawData_publisher -objs\\RawData_subscriber - -*/ - -#include -#include - -#include "RawData.h" -#include "RawDataSupport.h" -#include "ndds/ndds_cpp.h" - -#include "DynamicArrayPrimitives_.h" -#include "DynamicArrayPrimitives_Plugin.h" -#include "DynamicArrayPrimitives_Support.h" - -class RawDataListener : public DDSDataReaderListener { - public: - virtual void on_requested_deadline_missed( - DDSDataReader* /*reader*/, - const DDS_RequestedDeadlineMissedStatus& /*status*/) {} - - virtual void on_requested_incompatible_qos( - DDSDataReader* /*reader*/, - const DDS_RequestedIncompatibleQosStatus& /*status*/) {} - - virtual void on_sample_rejected( - DDSDataReader* /*reader*/, - const DDS_SampleRejectedStatus& /*status*/) {} - - virtual void on_liveliness_changed( - DDSDataReader* /*reader*/, - const DDS_LivelinessChangedStatus& /*status*/) {} - - virtual void on_sample_lost( - DDSDataReader* /*reader*/, - const DDS_SampleLostStatus& /*status*/) {} - - virtual void on_subscription_matched( - DDSDataReader* /*reader*/, - const DDS_SubscriptionMatchedStatus& /*status*/) {} - - virtual void on_data_available(DDSDataReader* reader); -}; - -void RawDataListener::on_data_available(DDSDataReader* reader) -{ - RawDataDataReader *RawData_reader = NULL; - RawDataSeq data_seq; - DDS_SampleInfoSeq info_seq; - DDS_ReturnCode_t retcode; - int i; - - RawData_reader = RawDataDataReader::narrow(reader); - if (RawData_reader == NULL) { - fprintf(stderr, "DataReader narrow error\n"); - return; - } - - retcode = RawData_reader->take( - data_seq, info_seq, DDS_LENGTH_UNLIMITED, - DDS_ANY_SAMPLE_STATE, DDS_ANY_VIEW_STATE, DDS_ANY_INSTANCE_STATE); - - if (retcode == DDS_RETCODE_NO_DATA) { - return; - } else if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "take error %d\n", retcode); - return; - } - - for (i = 0; i < data_seq.length(); ++i) { - if (info_seq[i].valid_data) { - for (size_t j = 0; j < data_seq[i].serialized_data.length(); ++j) { - printf("%02x ", data_seq[i].serialized_data[j]); - //RawDataTypeSupport::print_data(&data_seq[i]); - } - } - } - - retcode = RawData_reader->return_loan(data_seq, info_seq); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "return loan error %d\n", retcode); - } -} - -/* Delete all entities */ -static int subscriber_shutdown( - DDSDomainParticipant *participant) -{ - DDS_ReturnCode_t retcode; - int status = 0; - - if (participant != NULL) { - retcode = participant->delete_contained_entities(); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "delete_contained_entities error %d\n", retcode); - status = -1; - } - - retcode = DDSTheParticipantFactory->delete_participant(participant); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "delete_participant error %d\n", retcode); - status = -1; - } - } - - /* RTI Connext provides the finalize_instance() method on - domain participant factory for people who want to release memory used - by the participant factory. Uncomment the following block of code for - clean destruction of the singleton. */ - /* - - retcode = DDSDomainParticipantFactory::finalize_instance(); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "finalize_instance error %d\n", retcode); - status = -1; - } - */ - return status; -} - -extern "C" int subscriber_main(int domainId, int sample_count) -{ - DDSDomainParticipant *participant = NULL; - DDSSubscriber *subscriber = NULL; - DDSTopic *topic = NULL; - RawDataListener *reader_listener = NULL; - DDSDataReader *reader = NULL; - DDS_ReturnCode_t retcode; - const char *type_name = NULL; - DDS_TypeCode *type_code = NULL; - int count = 0; - DDS_Duration_t receive_period = {4,0}; - int status = 0; - - /* To customize the participant QoS, use - the configuration file USER_QOS_PROFILES.xml */ - participant = DDSTheParticipantFactory->create_participant( - domainId, DDS_PARTICIPANT_QOS_DEFAULT, - NULL /* listener */, DDS_STATUS_MASK_NONE); - if (participant == NULL) { - fprintf(stderr, "create_participant error\n"); - subscriber_shutdown(participant); - return -1; - } - - /* To customize the subscriber QoS, use - the configuration file USER_QOS_PROFILES.xml */ - subscriber = participant->create_subscriber( - DDS_SUBSCRIBER_QOS_DEFAULT, NULL /* listener */, DDS_STATUS_MASK_NONE); - if (subscriber == NULL) { - fprintf(stderr, "create_subscriber error\n"); - subscriber_shutdown(participant); - return -1; - } - - /* Register the type before creating the topic */ - type_name = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::get_type_name(); - type_code = test_msgs_msg_dds__DynamicArrayPrimitives_TypeSupport::get_typecode(); - retcode = RawDataSupport_register_external_type( - participant, type_name, type_code); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "register_type error %d\n", retcode); - subscriber_shutdown(participant); - return -1; - } - - /* To customize the topic QoS, use - the configuration file USER_QOS_PROFILES.xml */ - topic = participant->create_topic( - "my_generic_type", - type_name, DDS_TOPIC_QOS_DEFAULT, NULL /* listener */, - DDS_STATUS_MASK_NONE); - if (topic == NULL) { - fprintf(stderr, "create_topic error\n"); - subscriber_shutdown(participant); - return -1; - } - - /* Create a data reader listener */ - reader_listener = new RawDataListener(); - - /* To customize the data reader QoS, use - the configuration file USER_QOS_PROFILES.xml */ - reader = subscriber->create_datareader( - topic, DDS_DATAREADER_QOS_DEFAULT, reader_listener, - DDS_STATUS_MASK_ALL); - if (reader == NULL) { - fprintf(stderr, "create_datareader error\n"); - subscriber_shutdown(participant); - delete reader_listener; - return -1; - } - - /* Main loop */ - for (count=0; (sample_count == 0) || (count < sample_count); ++count) { - - printf("RawData subscriber sleeping for %d sec...\n", - receive_period.sec); - - NDDSUtility::sleep(receive_period); - } - - /* Delete all entities */ - status = subscriber_shutdown(participant); - delete reader_listener; - - return status; -} - -int main(int argc, char *argv[]) -{ - int domainId = 0; - int sample_count = 0; /* infinite loop */ - - if (argc >= 2) { - domainId = atoi(argv[1]); - } - if (argc >= 3) { - sample_count = atoi(argv[2]); - } - - /* Uncomment this to turn on additional logging - NDDSConfigLogger::get_instance()-> - set_verbosity_by_category(NDDS_CONFIG_LOG_CATEGORY_API, - NDDS_CONFIG_LOG_VERBOSITY_STATUS_ALL); - */ - - return subscriber_main(domainId, sample_count); -} - From 62ceb14613efbd9c83baf47e73a79e9fe6e3e982 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 28 Feb 2018 09:59:22 -0800 Subject: [PATCH 13/34] linters --- .../connext_static_raw_data.hpp | 9 +- .../connext_static_raw_data_plugin.hpp | 123 +++++---- .../connext_static_raw_data_support.hpp | 7 - .../src/connext_static_raw_data.cpp | 35 ++- .../src/connext_static_raw_data_plugin.cpp | 251 +++++++++--------- .../src/connext_static_raw_data_support.cpp | 12 +- rmw_connext_cpp/src/rmw_publish.cpp | 8 +- .../resource/msg__type_support.cpp.em | 3 +- 8 files changed, 211 insertions(+), 237 deletions(-) diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp index d9b155c5..0c9d355d 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp @@ -1,5 +1,3 @@ - - /* WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. @@ -34,7 +32,7 @@ class ConnextStaticRawDataDataWriter; class ConnextStaticRawDataDataReader; #endif -class ConnextStaticRawData +class ConnextStaticRawData { public: typedef struct ConnextStaticRawDataSeq Seq; @@ -71,7 +69,7 @@ RTIBool ConnextStaticRawData_initialize_ex( NDDSUSERDllExport RTIBool ConnextStaticRawData_initialize_w_params( ConnextStaticRawData* self, - const struct DDS_TypeAllocationParams_t * allocParams); + const struct DDS_TypeAllocationParams_t * allocParams); NDDSUSERDllExport void ConnextStaticRawData_finalize( @@ -88,7 +86,7 @@ void ConnextStaticRawData_finalize_w_params( NDDSUSERDllExport void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* self, RTIBool deletePointers); + ConnextStaticRawData* self, RTIBool deletePointers); NDDSUSERDllExport RTIBool ConnextStaticRawData_copy( @@ -103,4 +101,3 @@ RTIBool ConnextStaticRawData_copy( #endif #endif /* ConnextStaticRawData */ - diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp index 855667ef..00321bc4 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp @@ -1,5 +1,3 @@ - - /* WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. @@ -45,15 +43,15 @@ extern "C" { */ typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; - #define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample - #define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer - #define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer + #define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample + #define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer + #define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer - #define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey + #define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey #define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey - #define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample - #define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample + #define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample + #define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample /* -------------------------------------------------------------------------------------- Support functions: @@ -69,25 +67,25 @@ extern "C" { NDDSUSERDllExport extern ConnextStaticRawData* ConnextStaticRawDataPluginSupport_create_data(void); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPluginSupport_copy_data( ConnextStaticRawData *out, const ConnextStaticRawData *in); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPluginSupport_destroy_data_w_params( ConnextStaticRawData *sample, const struct DDS_TypeDeallocationParams_t * dealloc_params); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPluginSupport_destroy_data_ex( ConnextStaticRawData *sample,RTIBool deallocate_pointers); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPluginSupport_destroy_data( ConnextStaticRawData *sample); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPluginSupport_print_data( const ConnextStaticRawData *sample, const char *desc, @@ -99,11 +97,11 @@ extern "C" { NDDSUSERDllExport extern ConnextStaticRawData* ConnextStaticRawDataPluginSupport_create_key(void); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPluginSupport_destroy_key_ex( ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPluginSupport_destroy_key( ConnextStaticRawDataKeyHolder *key); @@ -111,36 +109,36 @@ extern "C" { Callback functions: * ---------------------------------------------------------------------------- */ - NDDSUSERDllExport extern PRESTypePluginParticipantData + NDDSUSERDllExport extern PRESTypePluginParticipantData ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, + void *registration_data, const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, + RTIBool top_level_registration, void *container_plugin_context, RTICdrTypeCode *typeCode); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPlugin_on_participant_detached( PRESTypePluginParticipantData participant_data); - NDDSUSERDllExport extern PRESTypePluginEndpointData + NDDSUSERDllExport extern PRESTypePluginEndpointData ConnextStaticRawDataPlugin_on_endpoint_attached( PRESTypePluginParticipantData participant_data, const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, + RTIBool top_level_registration, void *container_plugin_context); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPlugin_on_endpoint_detached( PRESTypePluginEndpointData endpoint_data); - NDDSUSERDllExport extern void + NDDSUSERDllExport extern void ConnextStaticRawDataPlugin_return_sample( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData *sample, - void *handle); + void *handle); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_copy_sample( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData *out, @@ -150,77 +148,77 @@ extern "C" { (De)Serialize functions: * ------------------------------------------------------------------------- */ - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_serialize( PRESTypePluginEndpointData endpoint_data, const ConnextStaticRawData *sample, - struct RTICdrStream *stream, + struct RTICdrStream *stream, RTIBool serialize_encapsulation, RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, + RTIBool serialize_sample, void *endpoint_plugin_qos); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_deserialize_sample( PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, + ConnextStaticRawData *sample, struct RTICdrStream *stream, RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, + RTIBool deserialize_sample, void *endpoint_plugin_qos); NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( char * buffer, unsigned int * length, - const ConnextStaticRawData *sample); + const ConnextStaticRawData *sample); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_deserialize( PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, + ConnextStaticRawData **sample, RTIBool * drop_sample, struct RTICdrStream *stream, RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, + RTIBool deserialize_sample, void *endpoint_plugin_qos); NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( ConnextStaticRawData *sample, const char * buffer, - unsigned int length); + unsigned int length); NDDSUSERDllExport extern DDS_ReturnCode_t ConnextStaticRawDataPlugin_data_to_string( const ConnextStaticRawData *sample, char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property); + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property); NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_skip( PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, void *endpoint_plugin_qos); - NDDSUSERDllExport extern unsigned int + NDDSUSERDllExport extern unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( PRESTypePluginEndpointData endpoint_data, RTIBool * overflow, RTIBool include_encapsulation, RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); + unsigned int current_alignment); - NDDSUSERDllExport extern unsigned int + NDDSUSERDllExport extern unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_max_size( PRESTypePluginEndpointData endpoint_data, RTIBool include_encapsulation, RTIEncapsulationId encapsulation_id, unsigned int current_alignment); - NDDSUSERDllExport extern unsigned int + NDDSUSERDllExport extern unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_min_size( PRESTypePluginEndpointData endpoint_data, RTIBool include_encapsulation, @@ -238,10 +236,10 @@ extern "C" { /* -------------------------------------------------------------------------------------- Key Management functions: * -------------------------------------------------------------------------------------- */ - NDDSUSERDllExport extern PRESTypePluginKeyKind + NDDSUSERDllExport extern PRESTypePluginKeyKind ConnextStaticRawDataPlugin_get_key_kind(void); - NDDSUSERDllExport extern unsigned int + NDDSUSERDllExport extern unsigned int ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( PRESTypePluginEndpointData endpoint_data, RTIBool * overflow, @@ -249,14 +247,14 @@ extern "C" { RTIEncapsulationId encapsulation_id, unsigned int current_alignment); - NDDSUSERDllExport extern unsigned int + NDDSUSERDllExport extern unsigned int ConnextStaticRawDataPlugin_get_serialized_key_max_size( PRESTypePluginEndpointData endpoint_data, RTIBool include_encapsulation, RTIEncapsulationId encapsulation_id, unsigned int current_alignment); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_serialize_key( PRESTypePluginEndpointData endpoint_data, const ConnextStaticRawData *sample, @@ -266,7 +264,7 @@ extern "C" { RTIBool serialize_key, void *endpoint_plugin_qos); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData * sample, @@ -275,7 +273,7 @@ extern "C" { RTIBool deserialize_key, void *endpoint_plugin_qos); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_deserialize_key( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData ** sample, @@ -289,36 +287,36 @@ extern "C" { ConnextStaticRawDataPlugin_serialized_sample_to_key( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, void *endpoint_plugin_qos); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_instance_to_key( PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *key, + ConnextStaticRawDataKeyHolder *key, const ConnextStaticRawData *instance); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_key_to_instance( PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *instance, + ConnextStaticRawData *instance, const ConnextStaticRawDataKeyHolder *key); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_instance_to_keyhash( PRESTypePluginEndpointData endpoint_data, DDS_KeyHash_t *keyhash, const ConnextStaticRawData *instance); - NDDSUSERDllExport extern RTIBool + NDDSUSERDllExport extern RTIBool ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, + struct RTICdrStream *stream, DDS_KeyHash_t *keyhash, RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos); + void *endpoint_plugin_qos); /* Plugin Functions */ NDDSUSERDllExport extern struct PRESTypePlugin* @@ -341,4 +339,3 @@ extern "C" { #endif #endif /* ConnextStaticRawDataPlugin_1689213465_h */ - diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp index 74f223b9..1b6570b9 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp @@ -1,4 +1,3 @@ - /* WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. @@ -44,10 +43,6 @@ implementing generics in C and C++. #endif -// DDS_TYPESUPPORT_CPP( -// ConnextStaticRawDataTypeSupport, -// ConnextStaticRawData); - class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport { public: @@ -130,7 +125,6 @@ class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ConnextStaticRawDataTypeSupport(); }; - DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); @@ -149,4 +143,3 @@ ConnextStaticRawDataSupport_register_external_type( #endif #endif /* ConnextStaticRawDataSupport_1689213465_h */ - diff --git a/rmw_connext_cpp/src/connext_static_raw_data.cpp b/rmw_connext_cpp/src/connext_static_raw_data.cpp index b8ba2fc1..307ec34c 100644 --- a/rmw_connext_cpp/src/connext_static_raw_data.cpp +++ b/rmw_connext_cpp/src/connext_static_raw_data.cpp @@ -1,5 +1,3 @@ - - /* WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. @@ -13,16 +11,16 @@ or consult the RTI Connext manual. #ifndef ndds_cpp_h #include "rmw_connext_shared_cpp/ndds_include.hpp" #endif -#ifndef dds_c_log_impl_h -#include "dds_c/dds_c_log_impl.h" -#endif +#ifndef dds_c_log_impl_h +#include "dds_c/dds_c_log_impl.h" +#endif #ifndef cdr_type_h #include "cdr/cdr_type.h" -#endif +#endif #ifndef osapi_heap_h -#include "osapi/osapi_heap.h" +#include "osapi/osapi_heap.h" #endif #else #include "ndds_standalone_type.h" @@ -48,7 +46,7 @@ DDS_TypeCode* ConnextStaticRawData_get_typecode() { (char *)"key_hash",/* Member name */ { - 0,/* Representation ID */ + 0,/* Representation ID */ DDS_BOOLEAN_FALSE,/* Is a pointer? */ -1, /* Bitfield bits */ NULL/* Member type code is assigned later */ @@ -61,11 +59,11 @@ DDS_TypeCode* ConnextStaticRawData_get_typecode() DDS_PUBLIC_MEMBER,/* Member visibility */ 1, NULL/* Ignored */ - }, + }, { (char *)"serialized_key",/* Member name */ { - 1,/* Representation ID */ + 1,/* Representation ID */ DDS_BOOLEAN_FALSE,/* Is a pointer? */ -1, /* Bitfield bits */ NULL/* Member type code is assigned later */ @@ -78,11 +76,11 @@ DDS_TypeCode* ConnextStaticRawData_get_typecode() DDS_PUBLIC_MEMBER,/* Member visibility */ 1, NULL/* Ignored */ - }, + }, { (char *)"serialized_data",/* Member name */ { - 2,/* Representation ID */ + 2,/* Representation ID */ DDS_BOOLEAN_FALSE,/* Is a pointer? */ -1, /* Bitfield bits */ NULL/* Member type code is assigned later */ @@ -104,13 +102,13 @@ DDS_TypeCode* ConnextStaticRawData_get_typecode() DDS_BOOLEAN_FALSE, /* Ignored */ -1, /*Ignored*/ (char *)"ConnextStaticRawData", /* Name */ - NULL, /* Ignored */ + NULL, /* Ignored */ 0, /* Ignored */ 0, /* Ignored */ NULL, /* Ignored */ 3, /* Number of members */ ConnextStaticRawData_g_tc_members, /* Members */ - DDS_VM_NONE /* Ignored */ + DDS_VM_NONE /* Ignored */ }}; /* Type code for ConnextStaticRawData*/ if (is_initialized) { @@ -176,7 +174,7 @@ RTIBool ConnextStaticRawData_initialize_w_params( if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { return RTI_FALSE; } - } else { + } else { DDS_OctetSeq_set_length(&sample->serialized_key, 0); } if (allocParams->allocate_memory) { @@ -185,7 +183,7 @@ RTIBool ConnextStaticRawData_initialize_w_params( if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { return RTI_FALSE; } - } else { + } else { DDS_OctetSeq_set_length(&sample->serialized_data, 0); } return RTI_TRUE; @@ -206,7 +204,7 @@ void ConnextStaticRawData_finalize_ex( if (sample==NULL) { return; - } + } deallocParams.delete_pointers = (DDS_Boolean)deletePointers; @@ -242,7 +240,7 @@ void ConnextStaticRawData_finalize_optional_members( if (sample==NULL) { return; - } + } if (deallocParams) {} /* To avoid warnings */ deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; @@ -310,4 +308,3 @@ RTIBool ConnextStaticRawData_copy( #undef TSeq #undef T - diff --git a/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp index 84b37e5c..75861dbb 100644 --- a/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp +++ b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp @@ -1,4 +1,3 @@ - /* WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. @@ -65,7 +64,7 @@ Support functions: ConnextStaticRawData* ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params) + const struct DDS_TypeAllocationParams_t * alloc_params) { ConnextStaticRawData *sample = NULL; @@ -78,11 +77,11 @@ ConnextStaticRawDataPluginSupport_create_data_w_params( delete sample; sample=NULL; } - return sample; -} + return sample; +} ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) +ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) { ConnextStaticRawData *sample = NULL; @@ -97,7 +96,7 @@ ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) sample=NULL; } - return sample; + return sample; } ConnextStaticRawData * @@ -106,7 +105,7 @@ ConnextStaticRawDataPluginSupport_create_data(void) return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); } -void +void ConnextStaticRawDataPluginSupport_destroy_data_w_params( ConnextStaticRawData *sample, const struct DDS_TypeDeallocationParams_t * dealloc_params) { @@ -117,7 +116,7 @@ ConnextStaticRawDataPluginSupport_destroy_data_w_params( sample=NULL; } -void +void ConnextStaticRawDataPluginSupport_destroy_data_ex( ConnextStaticRawData *sample,RTIBool deallocate_pointers) { @@ -127,7 +126,7 @@ ConnextStaticRawDataPluginSupport_destroy_data_ex( sample=NULL; } -void +void ConnextStaticRawDataPluginSupport_destroy_data( ConnextStaticRawData *sample) { @@ -135,7 +134,7 @@ ConnextStaticRawDataPluginSupport_destroy_data( } -RTIBool +RTIBool ConnextStaticRawDataPluginSupport_copy_data( ConnextStaticRawData *dst, const ConnextStaticRawData *src) @@ -143,7 +142,7 @@ ConnextStaticRawDataPluginSupport_copy_data( return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); } -void +void ConnextStaticRawDataPluginSupport_print_data( const ConnextStaticRawData *sample, const char *desc, @@ -165,8 +164,8 @@ ConnextStaticRawDataPluginSupport_print_data( RTICdrType_printArray( sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "key_hash", indent_level + 1); + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "key_hash", indent_level + 1); if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { RTICdrType_printArray( @@ -216,7 +215,7 @@ ConnextStaticRawDataPluginSupport_create_key(void) return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); } -void +void ConnextStaticRawDataPluginSupport_destroy_key_ex( ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) { @@ -227,7 +226,7 @@ ConnextStaticRawDataPluginSupport_destroy_key_ex( } -void +void ConnextStaticRawDataPluginSupport_destroy_key( ConnextStaticRawDataKeyHolder *key) { @@ -239,7 +238,7 @@ ConnextStaticRawDataPluginSupport_destroy_key( Callback functions: * ---------------------------------------------------------------------------- */ -PRESTypePluginParticipantData +PRESTypePluginParticipantData ConnextStaticRawDataPlugin_on_participant_attached( void *registration_data, const struct PRESTypePluginParticipantInfo *participant_info, @@ -257,7 +256,7 @@ ConnextStaticRawDataPlugin_on_participant_attached( } -void +void ConnextStaticRawDataPlugin_on_participant_detached( PRESTypePluginParticipantData participant_data) { @@ -269,7 +268,7 @@ PRESTypePluginEndpointData ConnextStaticRawDataPlugin_on_endpoint_attached( PRESTypePluginParticipantData participant_data, const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, + RTIBool top_level_registration, void *containerPluginContext) { PRESTypePluginEndpointData epd = NULL; @@ -289,18 +288,18 @@ ConnextStaticRawDataPlugin_on_endpoint_attached( (PRESTypePluginDefaultEndpointDataDestroySampleFunction) ConnextStaticRawDataPluginSupport_destroy_data, (PRESTypePluginDefaultEndpointDataCreateKeyFunction) - ConnextStaticRawDataPluginSupport_create_key , + ConnextStaticRawDataPluginSupport_create_key , (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) ConnextStaticRawDataPluginSupport_destroy_key); if (epd == NULL) { return NULL; - } + } serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( - epd,endpoint_info,serializedKeyMaxSize)) + epd,endpoint_info,serializedKeyMaxSize)) { PRESTypePluginDefaultEndpointData_delete(epd); return NULL; @@ -325,18 +324,18 @@ ConnextStaticRawDataPlugin_on_endpoint_attached( } } - return epd; + return epd; } -void +void ConnextStaticRawDataPlugin_on_endpoint_detached( PRESTypePluginEndpointData endpoint_data) -{ +{ PRESTypePluginDefaultEndpointData_delete(endpoint_data); } -void +void ConnextStaticRawDataPlugin_return_sample( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData *sample, @@ -349,7 +348,7 @@ ConnextStaticRawDataPlugin_return_sample( endpoint_data, sample, handle); } -RTIBool +RTIBool ConnextStaticRawDataPlugin_copy_sample( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData *dst, @@ -362,21 +361,21 @@ ConnextStaticRawDataPlugin_copy_sample( /* ---------------------------------------------------------------------------- (De)Serialize functions: * ------------------------------------------------------------------------- */ -unsigned int +unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_max_size( PRESTypePluginEndpointData endpoint_data, RTIBool include_encapsulation, RTIEncapsulationId encapsulation_id, unsigned int current_alignment); -RTIBool +RTIBool ConnextStaticRawDataPlugin_serialize( PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, RTIBool serialize_encapsulation, RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, + RTIBool serialize_sample, void *endpoint_plugin_qos) { char * position = NULL; @@ -455,13 +454,13 @@ ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream *strea RTICdrStream_setBufferLength(stream, adjustedBufferLength); } -RTIBool +RTIBool ConnextStaticRawDataPlugin_deserialize_sample( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData *sample, - struct RTICdrStream *stream, + struct RTICdrStream *stream, RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, + RTIBool deserialize_sample, void *endpoint_plugin_qos) { char * position = NULL; @@ -543,7 +542,7 @@ ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); if (buffer == NULL) { - *length = + *length = ConnextStaticRawDataPlugin_get_serialized_sample_size( (PRESTypePluginEndpointData)&epd, RTI_TRUE, @@ -556,18 +555,18 @@ ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( } return RTI_TRUE; - } + } RTICdrStream_init(&stream); RTICdrStream_set(&stream, (char *)buffer, *length); result = ConnextStaticRawDataPlugin_serialize( - (PRESTypePluginEndpointData)&epd, sample, &stream, - RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), - RTI_TRUE, NULL); + (PRESTypePluginEndpointData)&epd, sample, &stream, + RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), + RTI_TRUE, NULL); *length = RTICdrStream_getCurrentPositionOffset(&stream); - return result; + return result; } RTIBool @@ -582,9 +581,9 @@ ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( RTICdrStream_set(&stream, (char *)buffer, length); ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - return ConnextStaticRawDataPlugin_deserialize_sample( + return ConnextStaticRawDataPlugin_deserialize_sample( NULL, sample, - &stream, RTI_TRUE, RTI_TRUE, + &stream, RTI_TRUE, RTI_TRUE, NULL); } @@ -592,7 +591,7 @@ DDS_ReturnCode_t ConnextStaticRawDataPlugin_data_to_string( const ConnextStaticRawData *sample, char *str, - DDS_UnsignedLong *str_size, + DDS_UnsignedLong *str_size, const struct DDS_PrintFormatProperty *property) { DDS_DynamicData *data = NULL; @@ -614,8 +613,8 @@ ConnextStaticRawDataPlugin_data_to_string( } if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - NULL, - &length, + NULL, + &length, sample)) { return DDS_RETCODE_ERROR; } @@ -626,15 +625,15 @@ ConnextStaticRawDataPlugin_data_to_string( } if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - buffer, - &length, + buffer, + &length, sample)) { RTIOsapiHeap_freeBuffer(buffer); return DDS_RETCODE_ERROR; } data = DDS_DynamicData_new( - ConnextStaticRawData_get_typecode(), + ConnextStaticRawData_get_typecode(), &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); if (data == NULL) { RTIOsapiHeap_freeBuffer(buffer); @@ -649,7 +648,7 @@ ConnextStaticRawDataPlugin_data_to_string( } retCode = DDS_PrintFormatProperty_to_print_format( - property, + property, &printFormat); if (retCode != DDS_RETCODE_OK) { RTIOsapiHeap_freeBuffer(buffer); @@ -658,9 +657,9 @@ ConnextStaticRawDataPlugin_data_to_string( } retCode = DDS_DynamicDataFormatter_to_string_w_format( - data, + data, str, - str_size, + str_size, &printFormat); if (retCode != DDS_RETCODE_OK) { RTIOsapiHeap_freeBuffer(buffer); @@ -673,14 +672,14 @@ ConnextStaticRawDataPlugin_data_to_string( return DDS_RETCODE_OK; } -RTIBool +RTIBool ConnextStaticRawDataPlugin_deserialize( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData **sample, RTIBool * drop_sample, - struct RTICdrStream *stream, + struct RTICdrStream *stream, RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, + RTIBool deserialize_sample, void *endpoint_plugin_qos) { @@ -689,9 +688,9 @@ ConnextStaticRawDataPlugin_deserialize( if (drop_sample) {} /* To avoid warnings */ stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_sample( + result= ConnextStaticRawDataPlugin_deserialize_sample( endpoint_data, (sample != NULL)?*sample:NULL, - stream, deserialize_encapsulation, deserialize_sample, + stream, deserialize_encapsulation, deserialize_sample, endpoint_plugin_qos); if (result) { if (stream->_xTypesState.unassignable) { @@ -701,8 +700,8 @@ ConnextStaticRawDataPlugin_deserialize( if (!result && stream->_xTypesState.unassignable ) { RTICdrLog_exception( - METHOD_NAME, - &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, + METHOD_NAME, + &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, "ConnextStaticRawData"); } @@ -712,9 +711,9 @@ ConnextStaticRawDataPlugin_deserialize( RTIBool ConnextStaticRawDataPlugin_skip( PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, + struct RTICdrStream *stream, RTIBool skip_encapsulation, - RTIBool skip_sample, + RTIBool skip_sample, void *endpoint_plugin_qos) { char * position = NULL; @@ -736,15 +735,15 @@ RTIBool ConnextStaticRawDataPlugin_skip( if (!RTICdrStream_skipPrimitiveArray( stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - goto fin; - } + goto fin; + } { RTICdrUnsignedLong sequence_length; if (!RTICdrStream_skipPrimitiveSequence( stream, &sequence_length, RTI_CDR_OCTET_TYPE)){ - goto fin; + goto fin; } } { @@ -753,17 +752,17 @@ RTIBool ConnextStaticRawDataPlugin_skip( stream, &sequence_length, RTI_CDR_OCTET_TYPE)){ - goto fin; + goto fin; } } } done = RTI_TRUE; fin: - if (done != RTI_TRUE && + if (done != RTI_TRUE && RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; + return RTI_FALSE; } if(skip_encapsulation) { RTICdrStream_restoreAlignment(stream,position); @@ -772,7 +771,7 @@ RTIBool ConnextStaticRawDataPlugin_skip( return RTI_TRUE; } -unsigned int +unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( PRESTypePluginEndpointData endpoint_data, RTIBool * overflow, @@ -794,7 +793,7 @@ ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( } -unsigned int +unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_max_size( PRESTypePluginEndpointData endpoint_data, RTIBool include_encapsulation, @@ -814,7 +813,7 @@ ConnextStaticRawDataPlugin_get_serialized_sample_max_size( return size; } -unsigned int +unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_min_size( PRESTypePluginEndpointData endpoint_data, RTIBool include_encapsulation, @@ -826,7 +825,7 @@ ConnextStaticRawDataPlugin_get_serialized_sample_min_size( unsigned int encapsulation_size = current_alignment; - if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ if (include_encapsulation) { @@ -853,7 +852,7 @@ ConnextStaticRawDataPlugin_get_serialized_sample_min_size( } /* Returns the size of the sample in its serialized form (in bytes). -* It can also be an estimation in excess of the real buffer needed +* It can also be an estimation in excess of the real buffer needed * during a call to the serialize() function. * The value reported does not have to include the space for the * encapsulation flags. @@ -864,13 +863,13 @@ ConnextStaticRawDataPlugin_get_serialized_sample_size( RTIBool include_encapsulation, RTIEncapsulationId encapsulation_id, unsigned int current_alignment, - const ConnextStaticRawData * sample) + const ConnextStaticRawData * sample) { unsigned int initial_alignment = current_alignment; unsigned int encapsulation_size = current_alignment; - struct PRESTypePluginDefaultEndpointData epd; + struct PRESTypePluginDefaultEndpointData epd; if (sample==NULL) { return 0; @@ -879,7 +878,7 @@ ConnextStaticRawDataPlugin_get_serialized_sample_size( endpoint_data = (PRESTypePluginEndpointData) &epd; PRESTypePluginDefaultEndpointData_setBaseAlignment( endpoint_data, - current_alignment); + current_alignment); } if (include_encapsulation) { @@ -898,18 +897,18 @@ ConnextStaticRawDataPlugin_get_serialized_sample_size( current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + endpoint_data, current_alignment), + ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), + endpoint_data, current_alignment), DDS_OctetSeq_get_length(&sample->serialized_key), RTI_CDR_OCTET_TYPE); current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), + endpoint_data, current_alignment), DDS_OctetSeq_get_length(&sample->serialized_data), RTI_CDR_OCTET_TYPE); @@ -923,17 +922,17 @@ ConnextStaticRawDataPlugin_get_serialized_sample_size( Key Management functions: * -------------------------------------------------------------------------------------- */ -PRESTypePluginKeyKind +PRESTypePluginKeyKind ConnextStaticRawDataPlugin_get_key_kind(void) { return PRES_TYPEPLUGIN_NO_KEY; } -RTIBool +RTIBool ConnextStaticRawDataPlugin_serialize_key( PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, RTIBool serialize_encapsulation, RTIEncapsulationId encapsulation_id, RTIBool serialize_key, @@ -970,7 +969,7 @@ ConnextStaticRawDataPlugin_serialize_key( RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, + ConnextStaticRawData *sample, struct RTICdrStream *stream, RTIBool deserialize_encapsulation, RTIBool deserialize_key, @@ -1013,7 +1012,7 @@ RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( RTIBool ConnextStaticRawDataPlugin_deserialize_key( PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, + ConnextStaticRawData **sample, RTIBool * drop_sample, struct RTICdrStream *stream, RTIBool deserialize_encapsulation, @@ -1032,7 +1031,7 @@ RTIBool ConnextStaticRawDataPlugin_deserialize_key( } } - return result; + return result; } @@ -1092,13 +1091,13 @@ ConnextStaticRawDataPlugin_get_serialized_key_max_size( return size; } -RTIBool +RTIBool ConnextStaticRawDataPlugin_serialized_sample_to_key( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, void *endpoint_plugin_qos) { char * position = NULL; @@ -1133,7 +1132,7 @@ ConnextStaticRawDataPlugin_serialized_sample_to_key( stream, &sequence_length, RTI_CDR_OCTET_TYPE)){ - goto fin; + goto fin; } } @@ -1143,7 +1142,7 @@ ConnextStaticRawDataPlugin_serialized_sample_to_key( stream, &sequence_length, RTI_CDR_OCTET_TYPE)){ - goto fin; + goto fin; } } @@ -1152,14 +1151,14 @@ ConnextStaticRawDataPlugin_serialized_sample_to_key( done = RTI_TRUE; fin: if(!error) { - if (done != RTI_TRUE && + if (done != RTI_TRUE && RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; + return RTI_FALSE; } } else { return RTI_FALSE; - } + } if(deserialize_encapsulation) { RTICdrStream_restoreAlignment(stream,position); @@ -1168,14 +1167,14 @@ ConnextStaticRawDataPlugin_serialized_sample_to_key( return RTI_TRUE; } -RTIBool +RTIBool ConnextStaticRawDataPlugin_instance_to_key( PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *dst, + ConnextStaticRawDataKeyHolder *dst, const ConnextStaticRawData *src) { - if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ if (!RTICdrType_copyArray( dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { @@ -1184,14 +1183,14 @@ ConnextStaticRawDataPlugin_instance_to_key( return RTI_TRUE; } -RTIBool +RTIBool ConnextStaticRawDataPlugin_key_to_instance( PRESTypePluginEndpointData endpoint_data, ConnextStaticRawData *dst, const ConnextStaticRawDataKeyHolder *src) { - if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ if (!RTICdrType_copyArray( dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { return RTI_FALSE; @@ -1199,7 +1198,7 @@ ConnextStaticRawDataPlugin_key_to_instance( return RTI_TRUE; } -RTIBool +RTIBool ConnextStaticRawDataPlugin_instance_to_keyhash( PRESTypePluginEndpointData endpoint_data, DDS_KeyHash_t *keyhash, @@ -1222,11 +1221,11 @@ ConnextStaticRawDataPlugin_instance_to_keyhash( if (!ConnextStaticRawDataPlugin_serialize_key( endpoint_data, instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, RTI_TRUE, - NULL)) + NULL)) { int size; @@ -1240,9 +1239,9 @@ ConnextStaticRawDataPlugin_instance_to_keyhash( instance); if (size <= RTICdrStream_getBufferLength(md5Stream)) { - RTICdrStream_popState(md5Stream, &cdrState); + RTICdrStream_popState(md5Stream, &cdrState); return RTI_FALSE; - } + } RTIOsapiHeap_allocateBuffer(&buffer,size,0); @@ -1260,27 +1259,27 @@ ConnextStaticRawDataPlugin_instance_to_keyhash( if (!ConnextStaticRawDataPlugin_serialize_key( endpoint_data, instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, RTI_TRUE, - NULL)) + NULL)) { RTICdrStream_popState(md5Stream, &cdrState); RTIOsapiHeap_freeBuffer(buffer); return RTI_FALSE; - } - } + } + } - if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > + if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { RTICdrStream_computeMD5(md5Stream, keyhash->value); } else { RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); RTIOsapiMemory_copy( - keyhash->value, - RTICdrStream_getBuffer(md5Stream), + keyhash->value, + RTICdrStream_getBuffer(md5Stream), RTICdrStream_getCurrentPositionOffset(md5Stream)); } @@ -1294,14 +1293,14 @@ ConnextStaticRawDataPlugin_instance_to_keyhash( return RTI_TRUE; } -RTIBool +RTIBool ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, + struct RTICdrStream *stream, DDS_KeyHash_t *keyhash, RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos) -{ + void *endpoint_plugin_qos) +{ char * position = NULL; RTIBool done = RTI_FALSE; @@ -1337,14 +1336,14 @@ ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( done = RTI_TRUE; fin: if(!error) { - if (done != RTI_TRUE && + if (done != RTI_TRUE && RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; + return RTI_FALSE; } } else { return RTI_FALSE; - } + } if(deserialize_encapsulation) { RTICdrStream_restoreAlignment(stream,position); @@ -1361,8 +1360,8 @@ ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( /* ------------------------------------------------------------------------ * Plug-in Installation Methods * ------------------------------------------------------------------------ */ -struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) -{ +struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) +{ return NULL; } @@ -1493,5 +1492,5 @@ void ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) { RTIOsapiHeap_freeStructure(plugin); -} -#undef RTI_CDR_CURRENT_SUBMODULE +} +#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rmw_connext_cpp/src/connext_static_raw_data_support.cpp b/rmw_connext_cpp/src/connext_static_raw_data_support.cpp index 2c49ebe9..35c1d585 100644 --- a/rmw_connext_cpp/src/connext_static_raw_data_support.cpp +++ b/rmw_connext_cpp/src/connext_static_raw_data_support.cpp @@ -1,4 +1,3 @@ - /* WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. @@ -11,9 +10,9 @@ or consult the RTI Connext manual. #include "rmw_connext_cpp/connext_static_raw_data_support.hpp" #include "rmw_connext_cpp/connext_static_raw_data_plugin.hpp" -#ifndef dds_c_log_impl_h -#include "dds_c/dds_c_log_impl.h" -#endif +#ifndef dds_c_log_impl_h +#include "dds_c/dds_c_log_impl.h" +#endif /* ========================================================================= */ /** @@ -152,11 +151,6 @@ ConnextStaticRawDataSupport_register_external_type( already_registered = participant->is_type_registered(type_name); retcode = participant->register_type(type_name, presTypePlugin, NULL, !already_registered); - //retcode = DDS_DomainParticipant_register_type( - // participant, - // type_name, - // presTypePlugin, - // NULL /* registration_data */); if (retcode != DDS_RETCODE_OK) { fprintf(stderr, "error while registering external type\n"); goto finError; diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index ea3ad584..6bad22d1 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -25,11 +25,6 @@ bool publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) { - // for (unsigned int i = 0; i < cdr_stream->message_length; ++i) { - // fprintf(stderr, "%x02 ", cdr_stream->raw_message[i]); - // } - // fprintf(stderr, "\n"); - ConnextStaticRawDataDataWriter * data_writer = ConnextStaticRawDataDataWriter::narrow(dds_data_writer); if (!data_writer) { @@ -48,7 +43,8 @@ publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) instance->serialized_data.maximum(0); if (!instance->serialized_data.loan_contiguous( reinterpret_cast(cdr_stream->raw_message), - cdr_stream->message_length, cdr_stream->message_length)) { + cdr_stream->message_length, cdr_stream->message_length)) + { RMW_SET_ERROR_MSG("failed to loan memory for message"); goto cleanup; } diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index b3ec7a21..9afa4b07 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -226,7 +226,8 @@ to_cdr_stream__@(spec.base_type.type)( // allocate enough memory for the CDR stream // TODO(karsten1987): This allocation has to be preallocated // or at least bring in a custom allocator - cdr_stream->raw_message = (char *)malloc(sizeof(char) * cdr_stream->message_length); + cdr_stream->raw_message = + reinterpret_cast(malloc(sizeof(char) * cdr_stream->message_length)); // call the function again and fill the buffer this time if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->raw_message, &cdr_stream->message_length, dds_message) != RTI_TRUE) { return false; From 8b61ffb97c8e886bb3675ac6b7bbbd1e890bee1a Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 5 Mar 2018 15:45:09 -0800 Subject: [PATCH 14/34] [style] changes to the generated raw data idl for making CI linters happy --- rmw_connext_cpp/CMakeLists.txt | 22 +- .../connext_static_raw_data.hpp | 66 +- .../connext_static_raw_data_plugin.hpp | 610 +++--- .../connext_static_raw_data_support.hpp | 104 +- .../src/connext_static_raw_data.cpp | 376 ++-- .../src/connext_static_raw_data_plugin.cpp | 1918 +++++++++-------- .../src/connext_static_raw_data_support.cpp | 1 - 7 files changed, 1574 insertions(+), 1523 deletions(-) diff --git a/rmw_connext_cpp/CMakeLists.txt b/rmw_connext_cpp/CMakeLists.txt index b1971bb6..4085a81f 100644 --- a/rmw_connext_cpp/CMakeLists.txt +++ b/rmw_connext_cpp/CMakeLists.txt @@ -128,8 +128,26 @@ if(WIN32) endif() if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + find_package(ament_cmake_copyright REQUIRED) + ament_cppcheck() + ament_cpplint("FILTERS" "-legal/copyright") + ament_lint_cmake() + ament_uncrustify() + ament_copyright( + "--exclude" + "connext_static_raw_data.hpp" + "connext_static_raw_data_plugin.hpp" + "connext_static_raw_data_support.hpp" + "connext_static_raw_data.cpp" + "connext_static_raw_data_plugin.cpp" + "connext_static_raw_data_support.cpp" + ) + + # ament_lint_auto_find_test_dependencies() endif() ament_package(CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake") diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp index 0c9d355d..4213287a 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp @@ -7,22 +7,15 @@ For more information, type 'rtiddsgen -help' at a command shell or consult the RTI Connext manual. */ -#ifndef ConnextStaticRawData_1689213465_h -#define ConnextStaticRawData_1689213465_h +#ifndef RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_HPP_ +#define RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_HPP_ -#ifndef NDDS_STANDALONE_TYPE -#ifndef ndds_cpp_h #include "rmw_connext_shared_cpp/ndds_include.hpp" -#endif -#else -#include "ndds_standalone_type.h" -#endif -static const DDS_Long KEY_HASH_LENGTH_16= 16; -extern "C" { - - extern const char *ConnextStaticRawDataTYPENAME; +static const DDS_Long KEY_HASH_LENGTH_16 = 16; +extern "C" { +extern const char * ConnextStaticRawDataTYPENAME; } struct ConnextStaticRawDataSeq; @@ -34,18 +27,17 @@ class ConnextStaticRawDataDataReader; class ConnextStaticRawData { - public: - typedef struct ConnextStaticRawDataSeq Seq; - #ifndef NDDS_STANDALONE_TYPE - typedef ConnextStaticRawDataTypeSupport TypeSupport; - typedef ConnextStaticRawDataDataWriter DataWriter; - typedef ConnextStaticRawDataDataReader DataReader; - #endif - - DDS_Octet key_hash [(KEY_HASH_LENGTH_16)]; - DDS_OctetSeq serialized_key ; - DDS_OctetSeq serialized_data ; - +public: + typedef struct ConnextStaticRawDataSeq Seq; + #ifndef NDDS_STANDALONE_TYPE + typedef ConnextStaticRawDataTypeSupport TypeSupport; + typedef ConnextStaticRawDataDataWriter DataWriter; + typedef ConnextStaticRawDataDataReader DataReader; + #endif + + DDS_Octet key_hash[(KEY_HASH_LENGTH_16)]; + DDS_OctetSeq serialized_key; + DDS_OctetSeq serialized_data; }; #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) /* If the code is building on Windows, start exporting symbols. @@ -54,44 +46,44 @@ class ConnextStaticRawData #define NDDSUSERDllExport __declspec(dllexport) #endif -NDDSUSERDllExport DDS_TypeCode* ConnextStaticRawData_get_typecode(void); /* Type code */ +NDDSUSERDllExport DDS_TypeCode * ConnextStaticRawData_get_typecode(void); DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); NDDSUSERDllExport RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData* self); + ConnextStaticRawData * self); NDDSUSERDllExport RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData* self,RTIBool allocatePointers,RTIBool allocateMemory); + ConnextStaticRawData * self, RTIBool allocatePointers, RTIBool allocateMemory); NDDSUSERDllExport RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData* self, - const struct DDS_TypeAllocationParams_t * allocParams); + ConnextStaticRawData * self, + const struct DDS_TypeAllocationParams_t * allocParams); NDDSUSERDllExport void ConnextStaticRawData_finalize( - ConnextStaticRawData* self); + ConnextStaticRawData * self); NDDSUSERDllExport void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData* self,RTIBool deletePointers); + ConnextStaticRawData * self, RTIBool deletePointers); NDDSUSERDllExport void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData* self, - const struct DDS_TypeDeallocationParams_t * deallocParams); + ConnextStaticRawData * self, + const struct DDS_TypeDeallocationParams_t * deallocParams); NDDSUSERDllExport void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* self, RTIBool deletePointers); + ConnextStaticRawData * self, RTIBool deletePointers); NDDSUSERDllExport RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData* dst, - const ConnextStaticRawData* src); + ConnextStaticRawData * dst, + const ConnextStaticRawData * src); #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) /* If the code is building on Windows, stop exporting symbols. @@ -100,4 +92,4 @@ RTIBool ConnextStaticRawData_copy( #define NDDSUSERDllExport #endif -#endif /* ConnextStaticRawData */ +#endif // RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_HPP_ diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp index 00321bc4..e75eb98c 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp @@ -7,8 +7,8 @@ For more information, type 'rtiddsgen -help' at a command shell or consult the RTI Connext manual. */ -#ifndef ConnextStaticRawDataPlugin_1689213465_h -#define ConnextStaticRawDataPlugin_1689213465_h +#ifndef RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_PLUGIN_HPP_ +#define RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_PLUGIN_HPP_ #include "rmw_connext_cpp/connext_static_raw_data.hpp" @@ -27,308 +27,308 @@ struct RTICdrStream; extern "C" { - /* The type used to store keys for instances of type struct - * AnotherSimple. - * - * By default, this type is struct ConnextStaticRawData - * itself. However, if for some reason this choice is not practical for your - * system (e.g. if sizeof(struct ConnextStaticRawData) - * is very large), you may redefine this typedef in terms of another type of - * your choosing. HOWEVER, if you define the KeyHolder type to be something - * other than struct AnotherSimple, the - * following restriction applies: the key of struct - * ConnextStaticRawData must consist of a - * single field of your redefined KeyHolder type and that field must be the - * first field in struct ConnextStaticRawData. - */ - typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; - - #define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample - #define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer - #define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer - - #define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey - #define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey - - #define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample - #define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample - - /* -------------------------------------------------------------------------------------- - Support functions: - * -------------------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data(void); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData *out, - const ConnextStaticRawData *in); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData *sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData *sample,RTIBool deallocate_pointers); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData *sample); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData *sample, - const char *desc, - unsigned int indent); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_key(void); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder *key); - - /* ---------------------------------------------------------------------------- - Callback functions: - * ---------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern PRESTypePluginParticipantData - ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, - const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, - void *container_plugin_context, - RTICdrTypeCode *typeCode); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data); - - NDDSUSERDllExport extern PRESTypePluginEndpointData - ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, - void *container_plugin_context); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - void *handle); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *out, - const ConnextStaticRawData *in); - - /* ---------------------------------------------------------------------------- - (De)Serialize functions: - * ------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData *sample); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData *sample, - const char * buffer, - unsigned int length); - NDDSUSERDllExport extern DDS_ReturnCode_t - ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample); - - /* -------------------------------------------------------------------------------------- - Key Management functions: - * -------------------------------------------------------------------------------------- */ - NDDSUSERDllExport extern PRESTypePluginKeyKind - ConnextStaticRawDataPlugin_get_key_kind(void); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData ** sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *key, - const ConnextStaticRawData *instance); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *instance, - const ConnextStaticRawDataKeyHolder *key); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t *keyhash, - const ConnextStaticRawData *instance); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - DDS_KeyHash_t *keyhash, - RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos); - - /* Plugin Functions */ - NDDSUSERDllExport extern struct PRESTypePlugin* - ConnextStaticRawDataPlugin_new(void); - - NDDSUSERDllExport - extern struct PRESTypePlugin * - ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); - +/* The type used to store keys for instances of type struct + * AnotherSimple. + * + * By default, this type is struct ConnextStaticRawData + * itself. However, if for some reason this choice is not practical for your + * system (e.g. if sizeof(struct ConnextStaticRawData) + * is very large), you may redefine this typedef in terms of another type of + * your choosing. HOWEVER, if you define the KeyHolder type to be something + * other than struct AnotherSimple, the + * following restriction applies: the key of struct + * ConnextStaticRawData must consist of a + * single field of your redefined KeyHolder type and that field must be the + * first field in struct ConnextStaticRawData. + */ +typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; + +#define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample +#define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer +#define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer + +#define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey +#define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey + +#define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample +#define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample + +/* -------------------------------------------------------------------------------------- + Support functions: + * -------------------------------------------------------------------------------------- */ + +NDDSUSERDllExport extern ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params); + +NDDSUSERDllExport extern ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); + +NDDSUSERDllExport extern ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data(void); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData * out, + const ConnextStaticRawData * in); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData * sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData * sample, RTIBool deallocate_pointers); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData * sample); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData * sample, + const char * desc, + unsigned int indent); + +NDDSUSERDllExport extern ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); + +NDDSUSERDllExport extern ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key(void); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder * key, RTIBool deallocate_pointers); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder * key); + +/* ---------------------------------------------------------------------------- + Callback functions: + * ---------------------------------------------------------------------------- */ + +NDDSUSERDllExport extern PRESTypePluginParticipantData +ConnextStaticRawDataPlugin_on_participant_attached( + void * registration_data, + const struct PRESTypePluginParticipantInfo * participant_info, + RTIBool top_level_registration, + void * container_plugin_context, + RTICdrTypeCode * typeCode); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data); + +NDDSUSERDllExport extern PRESTypePluginEndpointData +ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo * endpoint_info, + RTIBool top_level_registration, + void * container_plugin_context); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + void * handle); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * out, + const ConnextStaticRawData * in); + +/* ---------------------------------------------------------------------------- + (De)Serialize functions: + * ------------------------------------------------------------------------- */ + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void * endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void * endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData * sample); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData ** sample, + RTIBool * drop_sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void * endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData * sample, + const char * buffer, + unsigned int length); + +NDDSUSERDllExport extern DDS_ReturnCode_t +ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData * sample, + char * str, + DDS_UnsignedLong * str_size, + const struct DDS_PrintFormatProperty * property); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream * stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void * endpoint_plugin_qos); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample); + +/* -------------------------------------------------------------------------------------- + Key Management functions: + * -------------------------------------------------------------------------------------- */ +NDDSUSERDllExport extern PRESTypePluginKeyKind +ConnextStaticRawDataPlugin_get_key_kind(void); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void * endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void * endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData ** sample, + RTIBool * drop_sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void * endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void * endpoint_plugin_qos); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder * key, + const ConnextStaticRawData * instance); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * instance, + const ConnextStaticRawDataKeyHolder * key); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t * keyhash, + const ConnextStaticRawData * instance); + +NDDSUSERDllExport extern RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream * stream, + DDS_KeyHash_t * keyhash, + RTIBool deserialize_encapsulation, + void * endpoint_plugin_qos); + +/* Plugin Functions */ +NDDSUSERDllExport extern struct PRESTypePlugin * +ConnextStaticRawDataPlugin_new(void); + +NDDSUSERDllExport +extern struct PRESTypePlugin * +ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); + +NDDSUSERDllExport extern void +ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); } #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) @@ -338,4 +338,4 @@ extern "C" { #define NDDSUSERDllExport #endif -#endif /* ConnextStaticRawDataPlugin_1689213465_h */ +#endif // RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_PLUGIN_HPP_ diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp index 1b6570b9..03fd88e5 100644 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp +++ b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp @@ -7,8 +7,8 @@ For more information, type 'rtiddsgen -help' at a command shell or consult the RTI Connext manual. */ -#ifndef ConnextStaticRawDataSupport_1689213465_h -#define ConnextStaticRawDataSupport_1689213465_h +#ifndef RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_SUPPORT_HPP_ +#define RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_SUPPORT_HPP_ /* Uses */ #include "rmw_connext_cpp/connext_static_raw_data.hpp" @@ -19,9 +19,9 @@ or consult the RTI Connext manual. #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -class __declspec(dllimport) DDSTypeSupport; -class __declspec(dllimport) DDSDataWriter; -class __declspec(dllimport) DDSDataReader; +class __declspec (dllimport) DDSTypeSupport; +class __declspec (dllimport) DDSDataWriter; +class __declspec (dllimport) DDSDataReader; #endif @@ -49,75 +49,83 @@ class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ConnextStaticRawDataTypeSupport(bool osrf) { (void) osrf; - }; + } ~ConnextStaticRawDataTypeSupport(); static DDS_ReturnCode_t register_type( - DDSDomainParticipant* participant, - const char* type_name = "ConnextStaticRawData"); + DDSDomainParticipant * participant, + const char * type_name = "ConnextStaticRawData"); static DDS_ReturnCode_t unregister_type( - DDSDomainParticipant* participant, - const char* type_name = "ConnextStaticRawData"); + DDSDomainParticipant * participant, + const char * type_name = "ConnextStaticRawData"); - static const char* get_type_name(); + static const char * get_type_name(); - static ConnextStaticRawData* create_data_ex(DDS_Boolean allocatePointers); + static ConnextStaticRawData * create_data_ex(DDS_Boolean allocatePointers); - static ConnextStaticRawData* create_data( - const DDS_TypeAllocationParams_t & alloc_params = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + static ConnextStaticRawData * create_data( + const DDS_TypeAllocationParams_t & alloc_params = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); - static DDS_ReturnCode_t delete_data_ex(ConnextStaticRawData* a_data, - DDS_Boolean deletePointers); + static DDS_ReturnCode_t delete_data_ex( + ConnextStaticRawData * a_data, + DDS_Boolean deletePointers); static DDS_ReturnCode_t delete_data( - ConnextStaticRawData* a_data, - const DDS_TypeDeallocationParams_t & dealloc_params = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + ConnextStaticRawData * a_data, + const DDS_TypeDeallocationParams_t & dealloc_params = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); - static void print_data(const ConnextStaticRawData* a_data); + static void print_data(const ConnextStaticRawData * a_data); - static DDS_ReturnCode_t copy_data(ConnextStaticRawData* dst_data, const ConnextStaticRawData* src_data); + static DDS_ReturnCode_t copy_data( + ConnextStaticRawData * dst_data, const ConnextStaticRawData * src_data); - static DDS_ReturnCode_t initialize_data_ex(ConnextStaticRawData* a_data, - DDS_Boolean allocatePointers); + static DDS_ReturnCode_t initialize_data_ex( + ConnextStaticRawData * a_data, + DDS_Boolean allocatePointers); static DDS_ReturnCode_t initialize_data( - ConnextStaticRawData* a_data, - const DDS_TypeAllocationParams_t & alloc_params = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + ConnextStaticRawData * a_data, + const DDS_TypeAllocationParams_t & alloc_params = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + + static DDS_ReturnCode_t finalize_data_ex( + ConnextStaticRawData * a_data, + DDS_Boolean deletePointers); - static DDS_ReturnCode_t finalize_data_ex(ConnextStaticRawData* a_data, - DDS_Boolean deletePointers); static DDS_ReturnCode_t finalize_data( - ConnextStaticRawData* a_data, - const DDS_TypeDeallocationParams_t & dealloc_params = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + ConnextStaticRawData * a_data, + const DDS_TypeDeallocationParams_t & dealloc_params = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + + DDSDataReader * create_datareaderI(DDSDataReader * dataReader); + + DDS_ReturnCode_t destroy_datareaderI(DDSDataReader * dataReader); + + DDSDataWriter * create_datawriterI(DDSDataWriter * dataWriter); - DDSDataReader* create_datareaderI(DDSDataReader* dataReader); - DDS_ReturnCode_t destroy_datareaderI(DDSDataReader* dataReader); - DDSDataWriter* create_datawriterI(DDSDataWriter* dataWriter); - DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter* dataWriter); + DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter * dataWriter); - static DDS_TypeCode* get_typecode(); + static DDS_TypeCode * get_typecode(); static DDS_ReturnCode_t serialize_data_to_cdr_buffer( - char * buffer, - unsigned int & length, - const ConnextStaticRawData *a_data); + char * buffer, + unsigned int & length, + const ConnextStaticRawData * a_data); static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( - ConnextStaticRawData *a_data, - const char * buffer, - unsigned int length); + ConnextStaticRawData * a_data, + const char * buffer, + unsigned int length); static DDS_ReturnCode_t data_to_string( - ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong& str_size, - const DDS_PrintFormatProperty& property); + ConnextStaticRawData * sample, + char * str, + DDS_UnsignedLong & str_size, + const DDS_PrintFormatProperty & property); static void finalize(); @@ -142,4 +150,4 @@ ConnextStaticRawDataSupport_register_external_type( #define NDDSUSERDllExport #endif -#endif /* ConnextStaticRawDataSupport_1689213465_h */ +#endif // RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_SUPPORT_HPP_ diff --git a/rmw_connext_cpp/src/connext_static_raw_data.cpp b/rmw_connext_cpp/src/connext_static_raw_data.cpp index 307ec34c..53daf7aa 100644 --- a/rmw_connext_cpp/src/connext_static_raw_data.cpp +++ b/rmw_connext_cpp/src/connext_static_raw_data.cpp @@ -19,263 +19,271 @@ or consult the RTI Connext manual. #include "cdr/cdr_type.h" #endif -#ifndef osapi_heap_h #include "osapi/osapi_heap.h" #endif -#else -#include "ndds_standalone_type.h" -#endif #include "rmw_connext_cpp/connext_static_raw_data.hpp" #include /* ========================================================================= */ -const char *ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; +const char * ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; -DDS_TypeCode* ConnextStaticRawData_get_typecode() +DDS_TypeCode * ConnextStaticRawData_get_typecode() { - static RTIBool is_initialized = RTI_FALSE; - - static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array =DDS_INITIALIZE_ARRAY_TYPECODE(1,(KEY_HASH_LENGTH_16), NULL,NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); - static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3]= + static RTIBool is_initialized = RTI_FALSE; + + static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array = + DDS_INITIALIZE_ARRAY_TYPECODE(1, (KEY_HASH_LENGTH_16), NULL, NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = + DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX, NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = + DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX, NULL); + static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3] = + { { - - { - (char *)"key_hash",/* Member name */ - { - 0,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_KEY_MEMBER , /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - }, - { - (char *)"serialized_key",/* Member name */ - { - 1,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - }, - { - (char *)"serialized_data",/* Member name */ - { - 2,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - } - }; - - static DDS_TypeCode ConnextStaticRawData_g_tc = - {{ - DDS_TK_STRUCT,/* Kind */ - DDS_BOOLEAN_FALSE, /* Ignored */ - -1, /*Ignored*/ - (char *)"ConnextStaticRawData", /* Name */ - NULL, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - 3, /* Number of members */ - ConnextStaticRawData_g_tc_members, /* Members */ - DDS_VM_NONE /* Ignored */ - }}; /* Type code for ConnextStaticRawData*/ - - if (is_initialized) { - return &ConnextStaticRawData_g_tc; + (char *)"key_hash", /* Member name */ // NOLINT + { + 0, /* Representation ID */ + DDS_BOOLEAN_FALSE, /* Is a pointer? */ + -1, /* Bitfield bits */ + NULL /* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_KEY_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER, /* Member visibility */ + 1, + NULL /* Ignored */ + }, + { + (char *)"serialized_key", /* Member name */ // NOLINT + { + 1, /* Representation ID */ + DDS_BOOLEAN_FALSE, /* Is a pointer? */ + -1, /* Bitfield bits */ + NULL /* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER, /* Member visibility */ + 1, + NULL /* Ignored */ + }, + { + (char *)"serialized_data", /* Member name */ // NOLINT + { + 2, /* Representation ID */ + DDS_BOOLEAN_FALSE, /* Is a pointer? */ + -1, /* Bitfield bits */ + NULL /* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER, /* Member visibility */ + 1, + NULL /* Ignored */ } + }; + + static DDS_TypeCode ConnextStaticRawData_g_tc = + {{ + DDS_TK_STRUCT, /* Kind */ + DDS_BOOLEAN_FALSE, /* Ignored */ + -1, /*Ignored*/ + (char *)"ConnextStaticRawData", /* Name */ // NOLINT + NULL, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + 3, /* Number of members */ + ConnextStaticRawData_g_tc_members, /* Members */ + DDS_VM_NONE /* Ignored */ + }}; /* Type code for ConnextStaticRawData*/ + + if (is_initialized) { + return &ConnextStaticRawData_g_tc; + } - ConnextStaticRawData_g_tc_key_hash_array._data._typeCode =(RTICdrTypeCode *)&DDS_g_tc_octet; + ConnextStaticRawData_g_tc_key_hash_array._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; - ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = + (RTICdrTypeCode *)&DDS_g_tc_octet; - ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = + (RTICdrTypeCode *)&DDS_g_tc_octet; - ConnextStaticRawData_g_tc_members[0]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_key_hash_array; - ConnextStaticRawData_g_tc_members[1]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_key_sequence; - ConnextStaticRawData_g_tc_members[2]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_data_sequence; + ConnextStaticRawData_g_tc_members[0]._representation._typeCode = + (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_key_hash_array; + ConnextStaticRawData_g_tc_members[1]._representation._typeCode = + (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_serialized_key_sequence; + ConnextStaticRawData_g_tc_members[2]._representation._typeCode = + (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_serialized_data_sequence; - is_initialized = RTI_TRUE; + is_initialized = RTI_TRUE; - return &ConnextStaticRawData_g_tc; + return &ConnextStaticRawData_g_tc; } RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData* sample) { - return ConnextStaticRawData_initialize_ex(sample,RTI_TRUE,RTI_TRUE); + ConnextStaticRawData * sample) +{ + return ConnextStaticRawData_initialize_ex(sample, RTI_TRUE, RTI_TRUE); } RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData* sample,RTIBool allocatePointers, RTIBool allocateMemory) + ConnextStaticRawData * sample, RTIBool allocatePointers, RTIBool allocateMemory) { - struct DDS_TypeAllocationParams_t allocParams = + struct DDS_TypeAllocationParams_t allocParams = DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; - allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; - allocParams.allocate_memory = (DDS_Boolean)allocateMemory; + allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; + allocParams.allocate_memory = (DDS_Boolean)allocateMemory; - return ConnextStaticRawData_initialize_w_params( - sample,&allocParams); + return ConnextStaticRawData_initialize_w_params( + sample, &allocParams); } RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData* sample, const struct DDS_TypeAllocationParams_t * allocParams) + ConnextStaticRawData * sample, const struct DDS_TypeAllocationParams_t * allocParams) { - void* buffer = NULL; - if (buffer) {} /* To avoid warnings */ - - if (sample == NULL) { - return RTI_FALSE; + void * buffer = NULL; + if (buffer) {} /* To avoid warnings */ + + if (sample == NULL) { + return RTI_FALSE; + } + if (allocParams == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_initArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) + { + return RTI_FALSE; + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_key); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key, RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_key, (0))) { + return RTI_FALSE; } - if (allocParams == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_initArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; + } else { + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_data); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data, RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_data, (0))) { + return RTI_FALSE; } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_key ); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key , RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_data ); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data , RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_data, 0); - } - return RTI_TRUE; + } else { + DDS_OctetSeq_set_length(&sample->serialized_data, 0); + } + return RTI_TRUE; } void ConnextStaticRawData_finalize( - ConnextStaticRawData* sample) + ConnextStaticRawData * sample) { - ConnextStaticRawData_finalize_ex(sample,RTI_TRUE); + ConnextStaticRawData_finalize_ex(sample, RTI_TRUE); } void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData* sample,RTIBool deletePointers) + ConnextStaticRawData * sample, RTIBool deletePointers) { - struct DDS_TypeDeallocationParams_t deallocParams = + struct DDS_TypeDeallocationParams_t deallocParams = DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - if (sample==NULL) { - return; - } + if (sample == NULL) { + return; + } - deallocParams.delete_pointers = (DDS_Boolean)deletePointers; + deallocParams.delete_pointers = (DDS_Boolean)deletePointers; - ConnextStaticRawData_finalize_w_params( - sample,&deallocParams); + ConnextStaticRawData_finalize_w_params( + sample, &deallocParams); } void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData* sample,const struct DDS_TypeDeallocationParams_t * deallocParams) + ConnextStaticRawData * sample, const struct DDS_TypeDeallocationParams_t * deallocParams) { - if (sample==NULL) { - return; - } + if (sample == NULL) { + return; + } - if (deallocParams == NULL) { - return; - } + if (deallocParams == NULL) { + return; + } - DDS_OctetSeq_finalize(&sample->serialized_key); + DDS_OctetSeq_finalize(&sample->serialized_key); - DDS_OctetSeq_finalize(&sample->serialized_data); + DDS_OctetSeq_finalize(&sample->serialized_data); } void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* sample, RTIBool deletePointers) + ConnextStaticRawData * sample, RTIBool deletePointers) { - struct DDS_TypeDeallocationParams_t deallocParamsTmp = + struct DDS_TypeDeallocationParams_t deallocParamsTmp = DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - struct DDS_TypeDeallocationParams_t * deallocParams = + struct DDS_TypeDeallocationParams_t * deallocParams = &deallocParamsTmp; - if (sample==NULL) { - return; - } - if (deallocParams) {} /* To avoid warnings */ + if (sample == NULL) { + return; + } + if (deallocParams) {} /* To avoid warnings */ - deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; - deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; + deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; + deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; } RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData* dst, - const ConnextStaticRawData* src) + ConnextStaticRawData * dst, + const ConnextStaticRawData * src) { - try { - - if (dst == NULL || src == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_key , - &src->serialized_key )) { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_data , - &src->serialized_data )) { - return RTI_FALSE; - } - - return RTI_TRUE; - - } catch (std::bad_alloc&) { - return RTI_FALSE; + try { + + if (dst == NULL || src == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_copyArray( + dst->key_hash, src->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) + { + return RTI_FALSE; } + if (!DDS_OctetSeq_copy(&dst->serialized_key, + &src->serialized_key)) + { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_data, + &src->serialized_data)) + { + return RTI_FALSE; + } + + return RTI_TRUE; + + } catch (std::bad_alloc &) { + return RTI_FALSE; + } } /** diff --git a/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp index 75861dbb..dc720eee 100644 --- a/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp +++ b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp @@ -62,175 +62,180 @@ or consult the RTI Connext manual. Support functions: * -------------------------------------------------------------------------- */ -ConnextStaticRawData* +ConnextStaticRawData * ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params) + const struct DDS_TypeAllocationParams_t * alloc_params) { - ConnextStaticRawData *sample = NULL; - - sample = new (std::nothrow) ConnextStaticRawData ; - if (sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_w_params(sample,alloc_params)) { - delete sample; - sample=NULL; - } - return sample; + ConnextStaticRawData * sample = NULL; + + sample = new (std::nothrow) ConnextStaticRawData; + if (sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_w_params(sample, alloc_params)) { + delete sample; + sample = NULL; + } + return sample; } ConnextStaticRawData * ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) { - ConnextStaticRawData *sample = NULL; + ConnextStaticRawData * sample = NULL; - sample = new (std::nothrow) ConnextStaticRawData ; + sample = new (std::nothrow) ConnextStaticRawData; - if(sample == NULL) { - return NULL; - } + if (sample == NULL) { + return NULL; + } - if (!ConnextStaticRawData_initialize_ex(sample,allocate_pointers, RTI_TRUE)) { - delete sample; - sample=NULL; - } + if (!ConnextStaticRawData_initialize_ex(sample, allocate_pointers, RTI_TRUE)) { + delete sample; + sample = NULL; + } - return sample; + return sample; } ConnextStaticRawData * ConnextStaticRawDataPluginSupport_create_data(void) { - return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); + return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); } void ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData *sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params) { + ConnextStaticRawData * sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params) +{ - ConnextStaticRawData_finalize_w_params(sample,dealloc_params); + ConnextStaticRawData_finalize_w_params(sample, dealloc_params); - delete sample; - sample=NULL; + delete sample; + sample = NULL; } void ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData *sample,RTIBool deallocate_pointers) { + ConnextStaticRawData * sample, RTIBool deallocate_pointers) +{ - ConnextStaticRawData_finalize_ex(sample,deallocate_pointers); + ConnextStaticRawData_finalize_ex(sample, deallocate_pointers); - delete sample; - sample=NULL; + delete sample; + sample = NULL; } void ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData *sample) { + ConnextStaticRawData * sample) +{ - ConnextStaticRawDataPluginSupport_destroy_data_ex(sample,RTI_TRUE); + ConnextStaticRawDataPluginSupport_destroy_data_ex(sample, RTI_TRUE); } RTIBool ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData *dst, - const ConnextStaticRawData *src) + ConnextStaticRawData * dst, + const ConnextStaticRawData * src) { - return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); + return ConnextStaticRawData_copy(dst, (const ConnextStaticRawData *) src); } void ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData *sample, - const char *desc, - unsigned int indent_level) + const ConnextStaticRawData * sample, + const char * desc, + unsigned int indent_level) { - RTICdrType_printIndent(indent_level); + RTICdrType_printIndent(indent_level); - if (desc != NULL) { - RTILog_debug("%s:\n", desc); - } else { - RTILog_debug("\n"); - } + if (desc != NULL) { + RTILog_debug("%s:\n", desc); + } else { + RTILog_debug("\n"); + } - if (sample == NULL) { - RTILog_debug("NULL\n"); - return; - } + if (sample == NULL) { + RTILog_debug("NULL\n"); + return; + } - RTICdrType_printArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "key_hash", indent_level + 1); - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key ), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } + RTICdrType_printArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "key_hash", indent_level + 1); - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data ), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } } ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers){ - ConnextStaticRawData *key = NULL; +ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers) +{ + ConnextStaticRawData * key = NULL; - key = new (std::nothrow) ConnextStaticRawDataKeyHolder ; + key = new (std::nothrow) ConnextStaticRawDataKeyHolder; - ConnextStaticRawData_initialize_ex(key,allocate_pointers, RTI_TRUE); + ConnextStaticRawData_initialize_ex(key, allocate_pointers, RTI_TRUE); - return key; + return key; } ConnextStaticRawData * ConnextStaticRawDataPluginSupport_create_key(void) { - return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); + return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); } void ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) + ConnextStaticRawDataKeyHolder * key, RTIBool deallocate_pointers) { - ConnextStaticRawData_finalize_ex(key,deallocate_pointers); + ConnextStaticRawData_finalize_ex(key, deallocate_pointers); - delete key; - key=NULL; + delete key; + key = NULL; } void ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder *key) { + ConnextStaticRawDataKeyHolder * key) +{ - ConnextStaticRawDataPluginSupport_destroy_key_ex(key,RTI_TRUE); + ConnextStaticRawDataPluginSupport_destroy_key_ex(key, RTI_TRUE); } @@ -240,122 +245,123 @@ Callback functions: PRESTypePluginParticipantData ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, - const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, - void *container_plugin_context, - RTICdrTypeCode *type_code) + void * registration_data, + const struct PRESTypePluginParticipantInfo * participant_info, + RTIBool top_level_registration, + void * container_plugin_context, + RTICdrTypeCode * type_code) { - if (registration_data) {} /* To avoid warnings */ - if (participant_info) {} /* To avoid warnings */ - if (top_level_registration) {} /* To avoid warnings */ - if (container_plugin_context) {} /* To avoid warnings */ - if (type_code) {} /* To avoid warnings */ + if (registration_data) {} /* To avoid warnings */ + if (participant_info) {} /* To avoid warnings */ + if (top_level_registration) {} /* To avoid warnings */ + if (container_plugin_context) {} /* To avoid warnings */ + if (type_code) {} /* To avoid warnings */ - return PRESTypePluginDefaultParticipantData_new(participant_info); + return PRESTypePluginDefaultParticipantData_new(participant_info); } void ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data) + PRESTypePluginParticipantData participant_data) { - PRESTypePluginDefaultParticipantData_delete(participant_data); + PRESTypePluginDefaultParticipantData_delete(participant_data); } PRESTypePluginEndpointData ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, - void *containerPluginContext) + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo * endpoint_info, + RTIBool top_level_registration, + void * containerPluginContext) { - PRESTypePluginEndpointData epd = NULL; - - unsigned int serializedSampleMaxSize; - - unsigned int serializedKeyMaxSize; - - if (top_level_registration) {} /* To avoid warnings */ - if (containerPluginContext) {} /* To avoid warnings */ - - epd = PRESTypePluginDefaultEndpointData_new( - participant_data, + PRESTypePluginEndpointData epd = NULL; + + unsigned int serializedSampleMaxSize; + + unsigned int serializedKeyMaxSize; + + if (top_level_registration) {} /* To avoid warnings */ + if (containerPluginContext) {} /* To avoid warnings */ + + epd = PRESTypePluginDefaultEndpointData_new( + participant_data, + endpoint_info, + (PRESTypePluginDefaultEndpointDataCreateSampleFunction) + ConnextStaticRawDataPluginSupport_create_data, + (PRESTypePluginDefaultEndpointDataDestroySampleFunction) + ConnextStaticRawDataPluginSupport_destroy_data, + (PRESTypePluginDefaultEndpointDataCreateKeyFunction) + ConnextStaticRawDataPluginSupport_create_key, + (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) + ConnextStaticRawDataPluginSupport_destroy_key); + + if (epd == NULL) { + return NULL; + } + serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( + epd, RTI_FALSE, RTI_CDR_ENCAPSULATION_ID_CDR_BE, 0); + + if (!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( + epd, endpoint_info, serializedKeyMaxSize)) + { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + + if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { + serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + epd, RTI_FALSE, RTI_CDR_ENCAPSULATION_ID_CDR_BE, 0); + + PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); + + if (PRESTypePluginDefaultEndpointData_createWriterPool( + epd, endpoint_info, - (PRESTypePluginDefaultEndpointDataCreateSampleFunction) - ConnextStaticRawDataPluginSupport_create_data, - (PRESTypePluginDefaultEndpointDataDestroySampleFunction) - ConnextStaticRawDataPluginSupport_destroy_data, - (PRESTypePluginDefaultEndpointDataCreateKeyFunction) - ConnextStaticRawDataPluginSupport_create_key , - (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) - ConnextStaticRawDataPluginSupport_destroy_key); - - if (epd == NULL) { - return NULL; - } - serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( - epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); - - if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( - epd,endpoint_info,serializedKeyMaxSize)) + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size, + epd) == RTI_FALSE) { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - - if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { - serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); - - PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); - - if (PRESTypePluginDefaultEndpointData_createWriterPool( - epd, - endpoint_info, - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size, - epd) == RTI_FALSE) { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; } + } - return epd; + return epd; } void ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data) + PRESTypePluginEndpointData endpoint_data) { - PRESTypePluginDefaultEndpointData_delete(endpoint_data); + PRESTypePluginDefaultEndpointData_delete(endpoint_data); } void ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - void *handle) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + void * handle) { - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - PRESTypePluginDefaultEndpointData_returnSample( - endpoint_data, sample, handle); + PRESTypePluginDefaultEndpointData_returnSample( + endpoint_data, sample, handle); } RTIBool ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *dst, - const ConnextStaticRawData *src) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * dst, + const ConnextStaticRawData * src) { - if (endpoint_data) {} /* To avoid warnings */ - return ConnextStaticRawDataPluginSupport_copy_data(dst,src); + if (endpoint_data) {} /* To avoid warnings */ + return ConnextStaticRawDataPluginSupport_copy_data(dst, src); } /* ---------------------------------------------------------------------------- @@ -363,67 +369,68 @@ ConnextStaticRawDataPlugin_copy_sample( * ------------------------------------------------------------------------- */ unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); RTIBool ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void * endpoint_plugin_qos) { - char * position = NULL; - RTIBool retval = RTI_TRUE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - /* This plugin can only be used to publish the top-level DDS Topic-Type - * in which case serialize_encapsulation==TRUE. If that is not - * the case then it is an error. + char * position = NULL; + RTIBool retval = RTI_TRUE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + /* This plugin can only be used to publish the top-level DDS Topic-Type + * in which case serialize_encapsulation==TRUE. If that is not + * the case then it is an error. + */ + if (!serialize_encapsulation) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + + if (serialize_sample) { + /* The sample->serialized_data contains the serialized encapsulation followed by the serialized + * data, so we only need to copy that into + * the CDR stream. Not the key_hash, not the length of the data itself + * The SerializedType sample->serialized_data is always a contiguous buffer */ - if (!serialize_encapsulation) { - return RTI_FALSE; + DDS_Octet * buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); + if (buffer == NULL) { + return RTI_FALSE; } - position = RTICdrStream_resetAlignment(stream); + /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian + * byte order + */ + if (encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { + return RTI_FALSE; + } - if(serialize_sample) { - /* The sample->serialized_data contains the serialized encapsulation followed by the serialized - * data, so we only need to copy that into - * the CDR stream. Not the key_hash, not the length of the data itself - * The SerializedType sample->serialized_data is always a contiguous buffer - */ - DDS_Octet *buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); - if ( buffer == NULL ) { - return RTI_FALSE; - } - - /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian - * byte order - */ - if ( encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { - return RTI_FALSE; - } - - /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ - if (!RTICdrStream_serializePrimitiveArray( - stream, (void*)buffer, - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } + /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ + if (!RTICdrStream_serializePrimitiveArray( + stream, (void *)buffer, + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE)) + { + return RTI_FALSE; } + } - RTICdrStream_restoreAlignment(stream ,position); + RTICdrStream_restoreAlignment(stream, position); - return retval; + return retval; } /** @@ -442,413 +449,420 @@ ConnextStaticRawDataPlugin_serialize( RTICdrStream_getEncapsulationOptions() */ void -ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream *stream) +ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) { - /* See http://issues.omg.org/browse/DDSXTY12-10 */ - DDS_UnsignedShort padding_size_mask = 0x0003; - DDS_UnsignedShort padding_size; - int adjustedBufferLength; - - padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; - adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; - RTICdrStream_setBufferLength(stream, adjustedBufferLength); + /* See http://issues.omg.org/browse/DDSXTY12-10 */ + DDS_UnsignedShort padding_size_mask = 0x0003; + DDS_UnsignedShort padding_size; + int adjustedBufferLength; + + padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; + adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; + RTICdrStream_setBufferLength(stream, adjustedBufferLength); } RTIBool ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void * endpoint_plugin_qos) { - char * position = NULL; - RTIBool done = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - /* This plugin can only be used to publish the top-level DDS Topic-Type - * in which case deserialize_encapsulation==TRUE. If that is not - * the case then it is an error. - */ - if (!deserialize_encapsulation) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - - /* TODO. The call does not belong here. It should be pushed - * inside RTICdrStream_deserializeAndSetCdrEncapsulation + char * position = NULL; + RTIBool done = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + /* This plugin can only be used to publish the top-level DDS Topic-Type + * in which case deserialize_encapsulation==TRUE. If that is not + * the case then it is an error. + */ + if (!deserialize_encapsulation) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + + /* TODO. The call does not belong here. It should be pushed + * inside RTICdrStream_deserializeAndSetCdrEncapsulation + */ + ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); + + if (deserialize_sample) { + /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() + it is done there because SerializedTypePlugin_deserialize_sample does not + have access to the SampleInfo where that information is + */ + + /* We do not set the serialized_key on deserialization */ + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + + /* We copy everything that remains in the CDR stream */ + int bytesLeftInStream = RTICdrStream_getRemainder(stream); + DDS_Octet * cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); + if (cdrBufferPtr == NULL) { + goto fin; + } + + /* Do not call SerializedType_initialize_ex initialize here + because it would override the key_hash field + SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); */ - ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); - - if (deserialize_sample) { - /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() - it is done there because SerializedTypePlugin_deserialize_sample does not - have access to the SampleInfo where that information is - */ - - /* We do not set the serialized_key on deserialization */ - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - - /* We copy everything that remains in the CDR stream */ - int bytesLeftInStream = RTICdrStream_getRemainder(stream); - DDS_Octet *cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); - if (cdrBufferPtr == NULL) { - goto fin; - } - - /* Do not call SerializedType_initialize_ex initialize here - because it would override the key_hash field - SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); - */ - if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { - goto fin; - } - RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); + if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { + goto fin; } + RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); + } - done = RTI_TRUE; + done = RTI_TRUE; - fin: - if ( (done != RTI_TRUE) && - (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) { - return RTI_FALSE; - } +fin: + if ( (done != RTI_TRUE) && + (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) + { + return RTI_FALSE; + } - RTICdrStream_restoreAlignment(stream,position); + RTICdrStream_restoreAlignment(stream, position); - return RTI_TRUE; + return RTI_TRUE; } RTIBool ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData *sample) + char * buffer, + unsigned int * length, + const ConnextStaticRawData * sample) { - struct RTICdrStream stream; - struct PRESTypePluginDefaultEndpointData epd; - RTIBool result; + struct RTICdrStream stream; + struct PRESTypePluginDefaultEndpointData epd; + RTIBool result; - if (length == NULL) { - return RTI_FALSE; - } + if (length == NULL) { + return RTI_FALSE; + } - epd._maxSizeSerializedSample = + epd._maxSizeSerializedSample = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); + NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); - if (buffer == NULL) { - *length = - ConnextStaticRawDataPlugin_get_serialized_sample_size( - (PRESTypePluginEndpointData)&epd, - RTI_TRUE, - RTICdrEncapsulation_getNativeCdrEncapsulationId(), - 0, - sample); - - if (*length == 0) { - return RTI_FALSE; - } - - return RTI_TRUE; + if (buffer == NULL) { + *length = + ConnextStaticRawDataPlugin_get_serialized_sample_size( + (PRESTypePluginEndpointData) & epd, + RTI_TRUE, + RTICdrEncapsulation_getNativeCdrEncapsulationId(), + 0, + sample); + + if (*length == 0) { + return RTI_FALSE; } - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, *length); + return RTI_TRUE; + } + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, *length); - result = ConnextStaticRawDataPlugin_serialize( - (PRESTypePluginEndpointData)&epd, sample, &stream, - RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), - RTI_TRUE, NULL); + result = ConnextStaticRawDataPlugin_serialize( + (PRESTypePluginEndpointData) & epd, sample, &stream, + RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), + RTI_TRUE, NULL); - *length = RTICdrStream_getCurrentPositionOffset(&stream); - return result; + *length = RTICdrStream_getCurrentPositionOffset(&stream); + return result; } RTIBool ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData *sample, - const char * buffer, - unsigned int length) + ConnextStaticRawData * sample, + const char * buffer, + unsigned int length) { - struct RTICdrStream stream; + struct RTICdrStream stream; - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, length); + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, length); - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - return ConnextStaticRawDataPlugin_deserialize_sample( - NULL, sample, - &stream, RTI_TRUE, RTI_TRUE, - NULL); + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + return ConnextStaticRawDataPlugin_deserialize_sample( + NULL, sample, + &stream, RTI_TRUE, RTI_TRUE, + NULL); } DDS_ReturnCode_t ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property) + const ConnextStaticRawData * sample, + char * str, + DDS_UnsignedLong * str_size, + const struct DDS_PrintFormatProperty * property) { - DDS_DynamicData *data = NULL; - char *buffer = NULL; - unsigned int length = 0; - struct DDS_PrintFormat printFormat; - DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; - - if (sample == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (str_size == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (property == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - NULL, - &length, - sample)) { - return DDS_RETCODE_ERROR; - } - - RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); - if (buffer == NULL) { - return DDS_RETCODE_ERROR; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - buffer, - &length, - sample)) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - data = DDS_DynamicData_new( - ConnextStaticRawData_get_typecode(), - &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); - if (data == NULL) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } + DDS_DynamicData * data = NULL; + char * buffer = NULL; + unsigned int length = 0; + struct DDS_PrintFormat printFormat; + DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; + + if (sample == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (str_size == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (property == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + NULL, + &length, + sample)) + { + return DDS_RETCODE_ERROR; + } + + RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); + if (buffer == NULL) { + return DDS_RETCODE_ERROR; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + buffer, + &length, + sample)) + { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } - retCode = DDS_PrintFormatProperty_to_print_format( - property, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } + data = DDS_DynamicData_new( + ConnextStaticRawData_get_typecode(), + &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); + if (data == NULL) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } - retCode = DDS_DynamicDataFormatter_to_string_w_format( - data, - str, - str_size, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } + retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + retCode = DDS_PrintFormatProperty_to_print_format( + property, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_DynamicDataFormatter_to_string_w_format( + data, + str, + str_size, + &printFormat); + if (retCode != DDS_RETCODE_OK) { RTIOsapiHeap_freeBuffer(buffer); DDS_DynamicData_delete(data); - return DDS_RETCODE_OK; + return retCode; + } + + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return DDS_RETCODE_OK; } RTIBool ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData ** sample, + RTIBool * drop_sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void * endpoint_plugin_qos) { - RTIBool result; - const char *METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; - if (drop_sample) {} /* To avoid warnings */ - - stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_sample( - endpoint_data, (sample != NULL)?*sample:NULL, - stream, deserialize_encapsulation, deserialize_sample, - endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } + RTIBool result; + const char * METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; + if (drop_sample) {} /* To avoid warnings */ + + stream->_xTypesState.unassignable = RTI_FALSE; + result = ConnextStaticRawDataPlugin_deserialize_sample( + endpoint_data, (sample != NULL) ? *sample : NULL, + stream, deserialize_encapsulation, deserialize_sample, + endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; } - if (!result && stream->_xTypesState.unassignable ) { + } + if (!result && stream->_xTypesState.unassignable) { - RTICdrLog_exception( - METHOD_NAME, - &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, - "ConnextStaticRawData"); + RTICdrLog_exception( + METHOD_NAME, + &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, + "ConnextStaticRawData"); - } + } - return result; + return result; } RTIBool ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream * stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void * endpoint_plugin_qos) { - char * position = NULL; - - RTIBool done = RTI_FALSE; + char * position = NULL; - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ + RTIBool done = RTI_FALSE; - if(skip_encapsulation) { - if (!RTICdrStream_skipEncapsulation(stream)) { - return RTI_FALSE; - } + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ - position = RTICdrStream_resetAlignment(stream); + if (skip_encapsulation) { + if (!RTICdrStream_skipEncapsulation(stream)) { + return RTI_FALSE; } - if (skip_sample) { - - if (!RTICdrStream_skipPrimitiveArray( - stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - goto fin; - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - } + position = RTICdrStream_resetAlignment(stream); + } - done = RTI_TRUE; - fin: - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; + if (skip_sample) { + + if (!RTICdrStream_skipPrimitiveArray( + stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) + { + goto fin; } - if(skip_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)) + { + goto fin; + } + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)) + { + goto fin; + } } + } - return RTI_TRUE; + done = RTI_TRUE; +fin: + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) + { + return RTI_FALSE; + } + if (skip_encapsulation) { + RTICdrStream_restoreAlignment(stream, position); + } + + return RTI_TRUE; } unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) { - if (endpoint_data) {} /* To avoid warnings */ - if (include_encapsulation) {} - if (encapsulation_id) {} - if (current_alignment) {} + if (endpoint_data) {} /* To avoid warnings */ + if (include_encapsulation) {} + if (encapsulation_id) {} + if (current_alignment) {} - if (overflow != NULL) { - *overflow = RTI_TRUE; - } + if (overflow != NULL) { + *overflow = RTI_TRUE; + } - return RTI_CDR_MAX_SERIALIZED_SIZE; + return RTI_CDR_MAX_SERIALIZED_SIZE; } unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) { - unsigned int size; - RTIBool overflow = RTI_FALSE; + unsigned int size; + RTIBool overflow = RTI_FALSE; - size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + endpoint_data, &overflow, include_encapsulation, encapsulation_id, current_alignment); - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } - return size; + return size; } unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) { - unsigned int initial_alignment = current_alignment; + unsigned int initial_alignment = current_alignment; - unsigned int encapsulation_size = current_alignment; + unsigned int encapsulation_size = current_alignment; - if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ - if (include_encapsulation) { + if (include_encapsulation) { - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } - current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment,0, RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment,0, RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment, 0, RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment, 0, RTI_CDR_OCTET_TYPE); - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; } /* Returns the size of the sample in its serialized form (in bytes). @@ -859,63 +873,63 @@ ConnextStaticRawDataPlugin_get_serialized_sample_min_size( */ unsigned int ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample) + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample) { - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - struct PRESTypePluginDefaultEndpointData epd; - - if (sample==NULL) { - return 0; - } - if (endpoint_data == NULL) { - endpoint_data = (PRESTypePluginEndpointData) &epd; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + struct PRESTypePluginDefaultEndpointData epd; + + if (sample == NULL) { + return 0; + } + if (endpoint_data == NULL) { + endpoint_data = (PRESTypePluginEndpointData) & epd; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; } /* -------------------------------------------------------------------------------------- @@ -925,299 +939,345 @@ Key Management functions: PRESTypePluginKeyKind ConnextStaticRawDataPlugin_get_key_kind(void) { - return PRES_TYPEPLUGIN_NO_KEY; + return PRES_TYPEPLUGIN_NO_KEY; } RTIBool ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void * endpoint_plugin_qos) { - char * position = NULL; + char * position = NULL; - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ - if(serialize_encapsulation) { - if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); + if (serialize_encapsulation) { + if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream, encapsulation_id)) { + return RTI_FALSE; } - if(serialize_key) { + position = RTICdrStream_resetAlignment(stream); + } - if (!RTICdrStream_serializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } + if (serialize_key) { + if (!RTICdrStream_serializePrimitiveArray( + stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) + { + return RTI_FALSE; } - if(serialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } + } - return RTI_TRUE; + if (serialize_encapsulation) { + RTICdrStream_restoreAlignment(stream, position); + } + + return RTI_TRUE; } RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void * endpoint_plugin_qos) { - try { + try { - char * position = NULL; + char * position = NULL; - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ - if(deserialize_encapsulation) { + if (deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } - position = RTICdrStream_resetAlignment(stream); - } - if (deserialize_key) { + position = RTICdrStream_resetAlignment(stream); + } + if (deserialize_key) { - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) + { + return RTI_FALSE; + } - } + } - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } + if (deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream, position); + } - return RTI_TRUE; + return RTI_TRUE; - } catch (std::bad_alloc&) { - return RTI_FALSE; - } + } catch (std::bad_alloc &) { + return RTI_FALSE; + } } RTIBool ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData ** sample, + RTIBool * drop_sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void * endpoint_plugin_qos) { - RTIBool result; - if (drop_sample) {} /* To avoid warnings */ - stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_key_sample( - endpoint_data, (sample != NULL)?*sample:NULL, stream, - deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } + RTIBool result; + if (drop_sample) {} /* To avoid warnings */ + stream->_xTypesState.unassignable = RTI_FALSE; + result = ConnextStaticRawDataPlugin_deserialize_key_sample( + endpoint_data, (sample != NULL) ? *sample : NULL, stream, + deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; } + } - return result; + return result; } unsigned int ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) { - unsigned int initial_alignment = current_alignment; + unsigned int initial_alignment = current_alignment; - unsigned int encapsulation_size = current_alignment; + unsigned int encapsulation_size = current_alignment; - if (endpoint_data) {} /* To avoid warnings */ - if (overflow) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ + if (overflow) {} /* To avoid warnings */ - if (include_encapsulation) { + if (include_encapsulation) { - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } - current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; } unsigned int ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) { - unsigned int size; - RTIBool overflow = RTI_FALSE; + unsigned int size; + RTIBool overflow = RTI_FALSE; - size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + endpoint_data, &overflow, include_encapsulation, encapsulation_id, current_alignment); - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } - return size; + return size; } RTIBool ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream * stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void * endpoint_plugin_qos) { - char * position = NULL; + char * position = NULL; - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - if(deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - position = RTICdrStream_resetAlignment(stream); + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + if (deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; } + position = RTICdrStream_resetAlignment(stream); + } - if (deserialize_key) { + if (deserialize_key) { - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) + { + return RTI_FALSE; + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)) + { + goto fin; + } } - done = RTI_TRUE; - fin: - if(!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - } else { - return RTI_FALSE; + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)) + { + goto fin; + } } - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); + } + + done = RTI_TRUE; +fin: + if (!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) + { + return RTI_FALSE; } + } else { + return RTI_FALSE; + } - return RTI_TRUE; + if (deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream, position); + } + + return RTI_TRUE; } RTIBool ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *dst, - const ConnextStaticRawData *src) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder * dst, + const ConnextStaticRawData * src) { - if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_data) {} /* To avoid warnings */ - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - return RTI_TRUE; + if (!RTICdrType_copyArray( + dst->key_hash, src->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) + { + return RTI_FALSE; + } + return RTI_TRUE; } RTIBool ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *dst, const - ConnextStaticRawDataKeyHolder *src) + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * dst, const + ConnextStaticRawDataKeyHolder * src) { - if (endpoint_data) {} /* To avoid warnings */ - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - return RTI_TRUE; + if (endpoint_data) {} /* To avoid warnings */ + if (!RTICdrType_copyArray( + dst->key_hash, src->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) + { + return RTI_FALSE; + } + return RTI_TRUE; } RTIBool ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t *keyhash, - const ConnextStaticRawData *instance) + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t * keyhash, + const ConnextStaticRawData * instance) { - struct RTICdrStream * md5Stream = NULL; - struct RTICdrStreamState cdrState; - char * buffer = NULL; + struct RTICdrStream * md5Stream = NULL; + struct RTICdrStreamState cdrState; + char * buffer = NULL; - RTICdrStreamState_init(&cdrState); - md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); + RTICdrStreamState_init(&cdrState); + md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); - if (md5Stream == NULL) { - return RTI_FALSE; + if (md5Stream == NULL) { + return RTI_FALSE; + } + + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + int size; + + RTICdrStream_pushState(md5Stream, &cdrState, -1); + + size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( + endpoint_data, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + 0, + instance); + + if (size <= RTICdrStream_getBufferLength(md5Stream)) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTIOsapiHeap_allocateBuffer(&buffer, size, 0); + + if (buffer == NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; } + RTICdrStream_set(md5Stream, buffer, size); + RTIOsapiMemory_zero( + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getBufferLength(md5Stream)); RTICdrStream_resetPosition(md5Stream); RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - if (!ConnextStaticRawDataPlugin_serialize_key( endpoint_data, instance, @@ -1227,140 +1287,106 @@ ConnextStaticRawDataPlugin_instance_to_keyhash( RTI_TRUE, NULL)) { - int size; - - RTICdrStream_pushState(md5Stream, &cdrState, -1); - - size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( - endpoint_data, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - 0, - instance); - - if (size <= RTICdrStream_getBufferLength(md5Stream)) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTIOsapiHeap_allocateBuffer(&buffer,size,0); - - if (buffer == NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTICdrStream_set(md5Stream, buffer, size); - RTIOsapiMemory_zero( - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getBufferLength(md5Stream)); - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - return RTI_FALSE; - } + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + return RTI_FALSE; } + } - if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > + if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || - PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { - RTICdrStream_computeMD5(md5Stream, keyhash->value); - } else { - RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); - RTIOsapiMemory_copy( - keyhash->value, - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getCurrentPositionOffset(md5Stream)); - } - - keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; - - if (buffer != NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - } + PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) + { + RTICdrStream_computeMD5(md5Stream, keyhash->value); + } else { + RTIOsapiMemory_zero(keyhash->value, MIG_RTPS_KEY_HASH_MAX_LENGTH); + RTIOsapiMemory_copy( + keyhash->value, + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getCurrentPositionOffset(md5Stream)); + } + + keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; + + if (buffer != NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + } - return RTI_TRUE; + return RTI_TRUE; } RTIBool ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - DDS_KeyHash_t *keyhash, - RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos) + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream * stream, + DDS_KeyHash_t * keyhash, + RTIBool deserialize_encapsulation, + void * endpoint_plugin_qos) { - char * position = NULL; + char * position = NULL; - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - ConnextStaticRawData * sample=NULL; - - if (endpoint_plugin_qos) {} /* To avoid warnings */ - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + ConnextStaticRawData * sample = NULL; - if(deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } + if (endpoint_plugin_qos) {} /* To avoid warnings */ + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } - position = RTICdrStream_resetAlignment(stream); + if (deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; } - sample = (ConnextStaticRawData *) + position = RTICdrStream_resetAlignment(stream); + } + + sample = (ConnextStaticRawData *) PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); - if (sample == NULL) { - return RTI_FALSE; - } + if (sample == NULL) { + return RTI_FALSE; + } - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) + { + return RTI_FALSE; + } - done = RTI_TRUE; - fin: - if(!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - } else { - return RTI_FALSE; + done = RTI_TRUE; +fin: + if (!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) + { + return RTI_FALSE; } + } else { + return RTI_FALSE; + } - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } + if (deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream, position); + } - if (!ConnextStaticRawDataPlugin_instance_to_keyhash( - endpoint_data, keyhash, sample)) { - return RTI_FALSE; - } + if (!ConnextStaticRawDataPlugin_instance_to_keyhash( + endpoint_data, keyhash, sample)) + { + return RTI_FALSE; + } - return RTI_TRUE; + return RTI_TRUE; } /* ------------------------------------------------------------------------ * Plug-in Installation Methods * ------------------------------------------------------------------------ */ -struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) +struct PRESTypePlugin * ConnextStaticRawDataPlugin_new(void) { return NULL; } @@ -1368,129 +1394,129 @@ struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) struct PRESTypePlugin * ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) { - if (external_type_code == NULL) { - return NULL; - } + if (external_type_code == NULL) { + return NULL; + } - struct PRESTypePlugin *plugin = NULL; - const struct PRESTypePluginVersion PLUGIN_VERSION = + struct PRESTypePlugin * plugin = NULL; + const struct PRESTypePluginVersion PLUGIN_VERSION = PRES_TYPE_PLUGIN_VERSION_2_0; - RTIOsapiHeap_allocateStructure( - &plugin, struct PRESTypePlugin); + RTIOsapiHeap_allocateStructure( + &plugin, struct PRESTypePlugin); - if (plugin == NULL) { - return NULL; - } + if (plugin == NULL) { + return NULL; + } - plugin->version = PLUGIN_VERSION; + plugin->version = PLUGIN_VERSION; - /* set up parent's function pointers */ - plugin->onParticipantAttached = + /* set up parent's function pointers */ + plugin->onParticipantAttached = (PRESTypePluginOnParticipantAttachedCallback) ConnextStaticRawDataPlugin_on_participant_attached; - plugin->onParticipantDetached = + plugin->onParticipantDetached = (PRESTypePluginOnParticipantDetachedCallback) ConnextStaticRawDataPlugin_on_participant_detached; - plugin->onEndpointAttached = + plugin->onEndpointAttached = (PRESTypePluginOnEndpointAttachedCallback) ConnextStaticRawDataPlugin_on_endpoint_attached; - plugin->onEndpointDetached = + plugin->onEndpointDetached = (PRESTypePluginOnEndpointDetachedCallback) ConnextStaticRawDataPlugin_on_endpoint_detached; - plugin->copySampleFnc = + plugin->copySampleFnc = (PRESTypePluginCopySampleFunction) ConnextStaticRawDataPlugin_copy_sample; - plugin->createSampleFnc = + plugin->createSampleFnc = (PRESTypePluginCreateSampleFunction) ConnextStaticRawDataPlugin_create_sample; - plugin->destroySampleFnc = + plugin->destroySampleFnc = (PRESTypePluginDestroySampleFunction) ConnextStaticRawDataPlugin_destroy_sample; - plugin->serializeFnc = + plugin->serializeFnc = (PRESTypePluginSerializeFunction) ConnextStaticRawDataPlugin_serialize; - plugin->deserializeFnc = + plugin->deserializeFnc = (PRESTypePluginDeserializeFunction) ConnextStaticRawDataPlugin_deserialize; - plugin->getSerializedSampleMaxSizeFnc = + plugin->getSerializedSampleMaxSizeFnc = (PRESTypePluginGetSerializedSampleMaxSizeFunction) ConnextStaticRawDataPlugin_get_serialized_sample_max_size; - plugin->getSerializedSampleMinSizeFnc = + plugin->getSerializedSampleMinSizeFnc = (PRESTypePluginGetSerializedSampleMinSizeFunction) ConnextStaticRawDataPlugin_get_serialized_sample_min_size; - plugin->getSampleFnc = + plugin->getSampleFnc = (PRESTypePluginGetSampleFunction) ConnextStaticRawDataPlugin_get_sample; - plugin->returnSampleFnc = + plugin->returnSampleFnc = (PRESTypePluginReturnSampleFunction) ConnextStaticRawDataPlugin_return_sample; - //plugin->getKeyKindFnc = ConnextStaticRawDataPlugin_get_external_key_kind(external_type_code); - plugin->getKeyKindFnc = (PRESTypePluginGetKeyKindFunction) - ConnextStaticRawDataPlugin_get_key_kind; + //plugin->getKeyKindFnc = ConnextStaticRawDataPlugin_get_external_key_kind(external_type_code); + plugin->getKeyKindFnc = (PRESTypePluginGetKeyKindFunction) + ConnextStaticRawDataPlugin_get_key_kind; - plugin->getSerializedKeyMaxSizeFnc = + plugin->getSerializedKeyMaxSizeFnc = (PRESTypePluginGetSerializedKeyMaxSizeFunction) ConnextStaticRawDataPlugin_get_serialized_key_max_size; - plugin->serializeKeyFnc = + plugin->serializeKeyFnc = (PRESTypePluginSerializeKeyFunction) ConnextStaticRawDataPlugin_serialize_key; - plugin->deserializeKeyFnc = + plugin->deserializeKeyFnc = (PRESTypePluginDeserializeKeyFunction) ConnextStaticRawDataPlugin_deserialize_key; - plugin->deserializeKeySampleFnc = + plugin->deserializeKeySampleFnc = (PRESTypePluginDeserializeKeySampleFunction) ConnextStaticRawDataPlugin_deserialize_key_sample; - plugin-> instanceToKeyHashFnc = + plugin->instanceToKeyHashFnc = (PRESTypePluginInstanceToKeyHashFunction) ConnextStaticRawDataPlugin_instance_to_keyhash; - plugin->serializedSampleToKeyHashFnc = + plugin->serializedSampleToKeyHashFnc = (PRESTypePluginSerializedSampleToKeyHashFunction) ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; - plugin->getKeyFnc = + plugin->getKeyFnc = (PRESTypePluginGetKeyFunction) ConnextStaticRawDataPlugin_get_key; - plugin->returnKeyFnc = + plugin->returnKeyFnc = (PRESTypePluginReturnKeyFunction) ConnextStaticRawDataPlugin_return_key; - plugin->instanceToKeyFnc = + plugin->instanceToKeyFnc = (PRESTypePluginInstanceToKeyFunction) ConnextStaticRawDataPlugin_instance_to_key; - plugin->keyToInstanceFnc = + plugin->keyToInstanceFnc = (PRESTypePluginKeyToInstanceFunction) ConnextStaticRawDataPlugin_key_to_instance; - plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ - //plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); - plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; + plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ + //plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); + plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; - plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; + plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; - /* Serialized buffer */ - plugin->getBuffer = + /* Serialized buffer */ + plugin->getBuffer = (PRESTypePluginGetBufferFunction) ConnextStaticRawDataPlugin_get_buffer; - plugin->returnBuffer = + plugin->returnBuffer = (PRESTypePluginReturnBufferFunction) ConnextStaticRawDataPlugin_return_buffer; - plugin->getSerializedSampleSizeFnc = + plugin->getSerializedSampleSizeFnc = (PRESTypePluginGetSerializedSampleSizeFunction) ConnextStaticRawDataPlugin_get_serialized_sample_size; - plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; + plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; - return plugin; + return plugin; } void -ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) +ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin * plugin) { - RTIOsapiHeap_freeStructure(plugin); + RTIOsapiHeap_freeStructure(plugin); } #undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rmw_connext_cpp/src/connext_static_raw_data_support.cpp b/rmw_connext_cpp/src/connext_static_raw_data_support.cpp index 35c1d585..e486af45 100644 --- a/rmw_connext_cpp/src/connext_static_raw_data_support.cpp +++ b/rmw_connext_cpp/src/connext_static_raw_data_support.cpp @@ -173,4 +173,3 @@ ConnextStaticRawDataSupport_register_external_type( return retcode; } - From 042cca6d485df68def9992009dcd5c551290b414 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 5 Mar 2018 15:45:25 -0800 Subject: [PATCH 15/34] rosidl linters --- .../rosidl_typesupport_connext_cpp/message_type_support.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h index 5e1ebf62..3f43c148 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h @@ -27,7 +27,7 @@ typedef struct message_type_support_callbacks_t const char * package_name; const char * message_name; // Function to register type with given dds_participant - DDS_TypeCode * (* get_type_code)(void); + DDS_TypeCode * (*get_type_code)(void); // Function to publish a ROS message with a given DDS data_writer //bool (* publish)(void * dds_data_writer, ConnextStaticCDRStream * cdr_stream); // Function to take a ROS message from a dds data reader From 3a445981ff2c664a009316a6ba18aca217c58e98 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 5 Mar 2018 16:09:05 -0800 Subject: [PATCH 16/34] raw generated code for raw data idl --- .../resources/connext_static_raw_data.idl | 7 + .../connext_static_raw_data.cxx | 313 ++++ .../patch_generated/connext_static_raw_data.h | 106 ++ .../connext_static_raw_dataPlugin.cxx | 1531 +++++++++++++++++ .../connext_static_raw_dataPlugin.h | 340 ++++ .../connext_static_raw_dataSupport.cxx | 117 ++ .../connext_static_raw_dataSupport.h | 62 + .../raw_generated/connext_static_raw_data.cxx | 313 ++++ .../raw_generated/connext_static_raw_data.h | 106 ++ .../connext_static_raw_dataPlugin.cxx | 1531 +++++++++++++++++ .../connext_static_raw_dataPlugin.h | 340 ++++ .../connext_static_raw_dataSupport.cxx | 117 ++ .../connext_static_raw_dataSupport.h | 62 + 13 files changed, 4945 insertions(+) create mode 100644 rmw_connext_cpp/resources/connext_static_raw_data.idl create mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx create mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h create mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx create mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h create mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx create mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h create mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.cxx create mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.h create mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.cxx create mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.h create mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.cxx create mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.h diff --git a/rmw_connext_cpp/resources/connext_static_raw_data.idl b/rmw_connext_cpp/resources/connext_static_raw_data.idl new file mode 100644 index 00000000..44f5704a --- /dev/null +++ b/rmw_connext_cpp/resources/connext_static_raw_data.idl @@ -0,0 +1,7 @@ +const long KEY_HASH_LENGTH_16 = 16; + +struct ConnextStaticRawData { + @key octet key_hash[KEY_HASH_LENGTH_16]; + sequence serialized_key; + sequence serialized_data; +}; diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx new file mode 100644 index 00000000..7b954282 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx @@ -0,0 +1,313 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef NDDS_STANDALONE_TYPE +#ifndef ndds_cpp_h +#include "ndds/ndds_cpp.h" +#endif +#ifndef dds_c_log_impl_h +#include "dds_c/dds_c_log_impl.h" +#endif + +#ifndef cdr_type_h +#include "cdr/cdr_type.h" +#endif + +#ifndef osapi_heap_h +#include "osapi/osapi_heap.h" +#endif +#else +#include "ndds_standalone_type.h" +#endif + +#include "connext_static_raw_data.h" + +#include + +/* ========================================================================= */ +const char *ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; + +DDS_TypeCode* ConnextStaticRawData_get_typecode() +{ + static RTIBool is_initialized = RTI_FALSE; + + static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array =DDS_INITIALIZE_ARRAY_TYPECODE(1,(KEY_HASH_LENGTH_16), NULL,NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); + static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3]= + { + + { + (char *)"key_hash",/* Member name */ + { + 0,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_KEY_MEMBER , /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + }, + { + (char *)"serialized_key",/* Member name */ + { + 1,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + }, + { + (char *)"serialized_data",/* Member name */ + { + 2,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + } + }; + + static DDS_TypeCode ConnextStaticRawData_g_tc = + {{ + DDS_TK_STRUCT,/* Kind */ + DDS_BOOLEAN_FALSE, /* Ignored */ + -1, /*Ignored*/ + (char *)"ConnextStaticRawData", /* Name */ + NULL, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + 3, /* Number of members */ + ConnextStaticRawData_g_tc_members, /* Members */ + DDS_VM_NONE /* Ignored */ + }}; /* Type code for ConnextStaticRawData*/ + + if (is_initialized) { + return &ConnextStaticRawData_g_tc; + } + + ConnextStaticRawData_g_tc_key_hash_array._data._typeCode =(RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_members[0]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_key_hash_array; + ConnextStaticRawData_g_tc_members[1]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_key_sequence; + ConnextStaticRawData_g_tc_members[2]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_data_sequence; + + is_initialized = RTI_TRUE; + + return &ConnextStaticRawData_g_tc; +} + +RTIBool ConnextStaticRawData_initialize( + ConnextStaticRawData* sample) { + return ConnextStaticRawData_initialize_ex(sample,RTI_TRUE,RTI_TRUE); +} + +RTIBool ConnextStaticRawData_initialize_ex( + ConnextStaticRawData* sample,RTIBool allocatePointers, RTIBool allocateMemory) +{ + + struct DDS_TypeAllocationParams_t allocParams = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; + + allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; + allocParams.allocate_memory = (DDS_Boolean)allocateMemory; + + return ConnextStaticRawData_initialize_w_params( + sample,&allocParams); + +} + +RTIBool ConnextStaticRawData_initialize_w_params( + ConnextStaticRawData* sample, const struct DDS_TypeAllocationParams_t * allocParams) +{ + + void* buffer = NULL; + if (buffer) {} /* To avoid warnings */ + + if (sample == NULL) { + return RTI_FALSE; + } + if (allocParams == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_initArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_key ); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key , RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { + return RTI_FALSE; + } + } else { + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_data ); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data , RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { + return RTI_FALSE; + } + } else { + DDS_OctetSeq_set_length(&sample->serialized_data, 0); + } + return RTI_TRUE; +} + +void ConnextStaticRawData_finalize( + ConnextStaticRawData* sample) +{ + + ConnextStaticRawData_finalize_ex(sample,RTI_TRUE); +} + +void ConnextStaticRawData_finalize_ex( + ConnextStaticRawData* sample,RTIBool deletePointers) +{ + struct DDS_TypeDeallocationParams_t deallocParams = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; + + if (sample==NULL) { + return; + } + + deallocParams.delete_pointers = (DDS_Boolean)deletePointers; + + ConnextStaticRawData_finalize_w_params( + sample,&deallocParams); +} + +void ConnextStaticRawData_finalize_w_params( + ConnextStaticRawData* sample,const struct DDS_TypeDeallocationParams_t * deallocParams) +{ + + if (sample==NULL) { + return; + } + + if (deallocParams == NULL) { + return; + } + + DDS_OctetSeq_finalize(&sample->serialized_key); + + DDS_OctetSeq_finalize(&sample->serialized_data); + +} + +void ConnextStaticRawData_finalize_optional_members( + ConnextStaticRawData* sample, RTIBool deletePointers) +{ + struct DDS_TypeDeallocationParams_t deallocParamsTmp = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; + struct DDS_TypeDeallocationParams_t * deallocParams = + &deallocParamsTmp; + + if (sample==NULL) { + return; + } + if (deallocParams) {} /* To avoid warnings */ + + deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; + deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; + +} + +RTIBool ConnextStaticRawData_copy( + ConnextStaticRawData* dst, + const ConnextStaticRawData* src) +{ + try { + + if (dst == NULL || src == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_key , + &src->serialized_key )) { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_data , + &src->serialized_data )) { + return RTI_FALSE; + } + + return RTI_TRUE; + + } catch (std::bad_alloc&) { + return RTI_FALSE; + } +} + +/** +* <> +* +* Defines: TSeq, T +* +* Configure and implement 'ConnextStaticRawData' sequence class. +*/ +#define T ConnextStaticRawData +#define TSeq ConnextStaticRawDataSeq + +#define T_initialize_w_params ConnextStaticRawData_initialize_w_params + +#define T_finalize_w_params ConnextStaticRawData_finalize_w_params +#define T_copy ConnextStaticRawData_copy + +#ifndef NDDS_STANDALONE_TYPE +#include "dds_c/generic/dds_c_sequence_TSeq.gen" +#include "dds_cpp/generic/dds_cpp_sequence_TSeq.gen" +#else +#include "dds_c_sequence_TSeq.gen" +#include "dds_cpp_sequence_TSeq.gen" +#endif + +#undef T_copy +#undef T_finalize_w_params + +#undef T_initialize_w_params + +#undef TSeq +#undef T + diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h new file mode 100644 index 00000000..d3adb4fa --- /dev/null +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h @@ -0,0 +1,106 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef connext_static_raw_data_738563504_h +#define connext_static_raw_data_738563504_h + +#ifndef NDDS_STANDALONE_TYPE +#ifndef ndds_cpp_h +#include "ndds/ndds_cpp.h" +#endif +#else +#include "ndds_standalone_type.h" +#endif + +static const DDS_Long KEY_HASH_LENGTH_16= 16; +extern "C" { + + extern const char *ConnextStaticRawDataTYPENAME; + +} + +struct ConnextStaticRawDataSeq; +#ifndef NDDS_STANDALONE_TYPE +class ConnextStaticRawDataTypeSupport; +class ConnextStaticRawDataDataWriter; +class ConnextStaticRawDataDataReader; +#endif + +class ConnextStaticRawData +{ + public: + typedef struct ConnextStaticRawDataSeq Seq; + #ifndef NDDS_STANDALONE_TYPE + typedef ConnextStaticRawDataTypeSupport TypeSupport; + typedef ConnextStaticRawDataDataWriter DataWriter; + typedef ConnextStaticRawDataDataReader DataReader; + #endif + + DDS_Octet key_hash [(KEY_HASH_LENGTH_16)]; + DDS_OctetSeq serialized_key ; + DDS_OctetSeq serialized_data ; + +}; +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) +#endif + +NDDSUSERDllExport DDS_TypeCode* ConnextStaticRawData_get_typecode(void); /* Type code */ + +DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize( + ConnextStaticRawData* self); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize_ex( + ConnextStaticRawData* self,RTIBool allocatePointers,RTIBool allocateMemory); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize_w_params( + ConnextStaticRawData* self, + const struct DDS_TypeAllocationParams_t * allocParams); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize( + ConnextStaticRawData* self); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_ex( + ConnextStaticRawData* self,RTIBool deletePointers); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_w_params( + ConnextStaticRawData* self, + const struct DDS_TypeDeallocationParams_t * deallocParams); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_optional_members( + ConnextStaticRawData* self, RTIBool deletePointers); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_copy( + ConnextStaticRawData* dst, + const ConnextStaticRawData* src); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* connext_static_raw_data */ + diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx new file mode 100644 index 00000000..78b58d54 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx @@ -0,0 +1,1531 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#include + +#ifndef ndds_cpp_h +#include "ndds/ndds_cpp.h" +#endif + +#ifndef osapi_type_h +#include "osapi/osapi_type.h" +#endif +#ifndef osapi_heap_h +#include "osapi/osapi_heap.h" +#endif + +#ifndef osapi_utility_h +#include "osapi/osapi_utility.h" +#endif + +#ifndef cdr_type_h +#include "cdr/cdr_type.h" +#endif + +#ifndef cdr_type_object_h +#include "cdr/cdr_typeObject.h" +#endif + +#ifndef cdr_encapsulation_h +#include "cdr/cdr_encapsulation.h" +#endif + +#ifndef cdr_stream_h +#include "cdr/cdr_stream.h" +#endif + +#ifndef cdr_log_h +#include "cdr/cdr_log.h" +#endif + +#ifndef pres_typePlugin_h +#include "pres/pres_typePlugin.h" +#endif + +#define RTI_CDR_CURRENT_SUBMODULE RTI_CDR_SUBMODULE_MASK_STREAM + +#include + +#include "connext_static_raw_dataPlugin.h" + +/* ---------------------------------------------------------------------------- +* Type ConnextStaticRawData +* -------------------------------------------------------------------------- */ + +/* ----------------------------------------------------------------------------- +Support functions: +* -------------------------------------------------------------------------- */ + +ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params) +{ + ConnextStaticRawData *sample = NULL; + + sample = new (std::nothrow) ConnextStaticRawData ; + if (sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_w_params(sample,alloc_params)) { + delete sample; + sample=NULL; + } + return sample; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) +{ + ConnextStaticRawData *sample = NULL; + + sample = new (std::nothrow) ConnextStaticRawData ; + + if(sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_ex(sample,allocate_pointers, RTI_TRUE)) { + delete sample; + sample=NULL; + } + + return sample; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data(void) +{ + return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); +} + +void +ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData *sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params) { + + ConnextStaticRawData_finalize_w_params(sample,dealloc_params); + + delete sample; + sample=NULL; +} + +void +ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData *sample,RTIBool deallocate_pointers) { + + ConnextStaticRawData_finalize_ex(sample,deallocate_pointers); + + delete sample; + sample=NULL; +} + +void +ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData *sample) { + + ConnextStaticRawDataPluginSupport_destroy_data_ex(sample,RTI_TRUE); + +} + +RTIBool +ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData *dst, + const ConnextStaticRawData *src) +{ + return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); +} + +void +ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData *sample, + const char *desc, + unsigned int indent_level) +{ + + RTICdrType_printIndent(indent_level); + + if (desc != NULL) { + RTILog_debug("%s:\n", desc); + } else { + RTILog_debug("\n"); + } + + if (sample == NULL) { + RTILog_debug("NULL\n"); + return; + } + + RTICdrType_printArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "key_hash", indent_level + 1); + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key ), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data ), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } + +} +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers){ + ConnextStaticRawData *key = NULL; + + key = new (std::nothrow) ConnextStaticRawDataKeyHolder ; + + ConnextStaticRawData_initialize_ex(key,allocate_pointers, RTI_TRUE); + + return key; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key(void) +{ + return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); +} + +void +ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) +{ + ConnextStaticRawData_finalize_ex(key,deallocate_pointers); + + delete key; + key=NULL; + +} + +void +ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder *key) { + + ConnextStaticRawDataPluginSupport_destroy_key_ex(key,RTI_TRUE); + +} + +/* ---------------------------------------------------------------------------- +Callback functions: +* ---------------------------------------------------------------------------- */ + +PRESTypePluginParticipantData +ConnextStaticRawDataPlugin_on_participant_attached( + void *registration_data, + const struct PRESTypePluginParticipantInfo *participant_info, + RTIBool top_level_registration, + void *container_plugin_context, + RTICdrTypeCode *type_code) +{ + if (registration_data) {} /* To avoid warnings */ + if (participant_info) {} /* To avoid warnings */ + if (top_level_registration) {} /* To avoid warnings */ + if (container_plugin_context) {} /* To avoid warnings */ + if (type_code) {} /* To avoid warnings */ + + return PRESTypePluginDefaultParticipantData_new(participant_info); + +} + +void +ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data) +{ + + PRESTypePluginDefaultParticipantData_delete(participant_data); +} + +PRESTypePluginEndpointData +ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo *endpoint_info, + RTIBool top_level_registration, + void *containerPluginContext) +{ + PRESTypePluginEndpointData epd = NULL; + + unsigned int serializedSampleMaxSize; + + unsigned int serializedKeyMaxSize; + + if (top_level_registration) {} /* To avoid warnings */ + if (containerPluginContext) {} /* To avoid warnings */ + + epd = PRESTypePluginDefaultEndpointData_new( + participant_data, + endpoint_info, + (PRESTypePluginDefaultEndpointDataCreateSampleFunction) + ConnextStaticRawDataPluginSupport_create_data, + (PRESTypePluginDefaultEndpointDataDestroySampleFunction) + ConnextStaticRawDataPluginSupport_destroy_data, + (PRESTypePluginDefaultEndpointDataCreateKeyFunction) + ConnextStaticRawDataPluginSupport_create_key , + (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) + ConnextStaticRawDataPluginSupport_destroy_key); + + if (epd == NULL) { + return NULL; + } + serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( + epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); + + if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( + epd,endpoint_info,serializedKeyMaxSize)) + { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + + if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { + serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); + + PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); + + if (PRESTypePluginDefaultEndpointData_createWriterPool( + epd, + endpoint_info, + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size, + epd) == RTI_FALSE) { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + } + + return epd; +} + +void +ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data) +{ + + PRESTypePluginDefaultEndpointData_delete(endpoint_data); +} + +void +ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + void *handle) +{ + + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + + PRESTypePluginDefaultEndpointData_returnSample( + endpoint_data, sample, handle); +} + +RTIBool +ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *dst, + const ConnextStaticRawData *src) +{ + if (endpoint_data) {} /* To avoid warnings */ + return ConnextStaticRawDataPluginSupport_copy_data(dst,src); +} + +/* ---------------------------------------------------------------------------- +(De)Serialize functions: +* ------------------------------------------------------------------------- */ +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +RTIBool +ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + RTIBool retval = RTI_TRUE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(serialize_encapsulation) { + if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if(serialize_sample) { + + if (!RTICdrStream_serializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + if (!RTICdrStream_serializePrimitiveSequence( + stream, + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + (RTI_INT32_MAX-1), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } else { + if (!RTICdrStream_serializePrimitivePointerSequence( + stream, + (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + (RTI_INT32_MAX-1), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + if (!RTICdrStream_serializePrimitiveSequence( + stream, + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + (RTI_INT32_MAX-1), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } else { + if (!RTICdrStream_serializePrimitivePointerSequence( + stream, + (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + (RTI_INT32_MAX-1), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } + + } + + if(serialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return retval; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos) +{ + + char * position = NULL; + + RTIBool done = RTI_FALSE; + + try { + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + if(deserialize_encapsulation) { + + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + if(deserialize_sample) { + + ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + goto fin; + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { + goto fin; + } + if (!DDS_OctetSeq_set_maximum(&sample->serialized_key,sequence_length)) { + return RTI_FALSE; + } + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + if (!RTICdrStream_deserializePrimitiveSequence( + stream, + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + &sequence_length, + DDS_OctetSeq_get_maximum(&sample->serialized_key), + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } else { + if (!RTICdrStream_deserializePrimitivePointerSequence( + stream, + (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + &sequence_length, + DDS_OctetSeq_get_maximum(&sample->serialized_key), + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + if (!DDS_OctetSeq_set_length(&sample->serialized_key, sequence_length)) { + return RTI_FALSE; + } + + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { + goto fin; + } + if (!DDS_OctetSeq_set_maximum(&sample->serialized_data,sequence_length)) { + return RTI_FALSE; + } + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + if (!RTICdrStream_deserializePrimitiveSequence( + stream, + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + &sequence_length, + DDS_OctetSeq_get_maximum(&sample->serialized_data), + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } else { + if (!RTICdrStream_deserializePrimitivePointerSequence( + stream, + (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + &sequence_length, + DDS_OctetSeq_get_maximum(&sample->serialized_data), + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + if (!DDS_OctetSeq_set_length(&sample->serialized_data, sequence_length)) { + return RTI_FALSE; + } + + } + } + + done = RTI_TRUE; + fin: + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; + + } catch (std::bad_alloc&) { + return RTI_FALSE; + } +} + +RTIBool +ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData *sample) +{ + struct RTICdrStream stream; + struct PRESTypePluginDefaultEndpointData epd; + RTIBool result; + + if (length == NULL) { + return RTI_FALSE; + } + + epd._maxSizeSerializedSample = + ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); + + if (buffer == NULL) { + *length = + ConnextStaticRawDataPlugin_get_serialized_sample_size( + (PRESTypePluginEndpointData)&epd, + RTI_TRUE, + RTICdrEncapsulation_getNativeCdrEncapsulationId(), + 0, + sample); + + if (*length == 0) { + return RTI_FALSE; + } + + return RTI_TRUE; + } + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, *length); + + result = ConnextStaticRawDataPlugin_serialize( + (PRESTypePluginEndpointData)&epd, sample, &stream, + RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), + RTI_TRUE, NULL); + + *length = RTICdrStream_getCurrentPositionOffset(&stream); + return result; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData *sample, + const char * buffer, + unsigned int length) +{ + struct RTICdrStream stream; + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, length); + + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + return ConnextStaticRawDataPlugin_deserialize_sample( + NULL, sample, + &stream, RTI_TRUE, RTI_TRUE, + NULL); +} + +DDS_ReturnCode_t +ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property) +{ + DDS_DynamicData *data = NULL; + char *buffer = NULL; + unsigned int length = 0; + struct DDS_PrintFormat printFormat; + DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; + + if (sample == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (str_size == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (property == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + NULL, + &length, + sample)) { + return DDS_RETCODE_ERROR; + } + + RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); + if (buffer == NULL) { + return DDS_RETCODE_ERROR; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + buffer, + &length, + sample)) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } + + data = DDS_DynamicData_new( + ConnextStaticRawData_get_typecode(), + &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); + if (data == NULL) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } + + retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_PrintFormatProperty_to_print_format( + property, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_DynamicDataFormatter_to_string_w_format( + data, + str, + str_size, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return DDS_RETCODE_OK; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos) +{ + + RTIBool result; + const char *METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; + if (drop_sample) {} /* To avoid warnings */ + + stream->_xTypesState.unassignable = RTI_FALSE; + result= ConnextStaticRawDataPlugin_deserialize_sample( + endpoint_data, (sample != NULL)?*sample:NULL, + stream, deserialize_encapsulation, deserialize_sample, + endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; + } + } + if (!result && stream->_xTypesState.unassignable ) { + + RTICdrLog_exception( + METHOD_NAME, + &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, + "ConnextStaticRawData"); + + } + + return result; + +} + +RTIBool ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(skip_encapsulation) { + if (!RTICdrStream_skipEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if (skip_sample) { + + if (!RTICdrStream_skipPrimitiveArray( + stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + goto fin; + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + } + + done = RTI_TRUE; + fin: + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + if(skip_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + if (endpoint_data) {} /* To avoid warnings */ + if (include_encapsulation) {} + if (encapsulation_id) {} + if (current_alignment) {} + + if (overflow != NULL) { + *overflow = RTI_TRUE; + } + + return RTI_CDR_MAX_SERIALIZED_SIZE; + +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + unsigned int size; + RTIBool overflow = RTI_FALSE; + + size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } + + return size; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + + if (endpoint_data) {} /* To avoid warnings */ + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } + + current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment,0, RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment,0, RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +/* Returns the size of the sample in its serialized form (in bytes). +* It can also be an estimation in excess of the real buffer needed +* during a call to the serialize() function. +* The value reported does not have to include the space for the +* encapsulation flags. +*/ +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + struct PRESTypePluginDefaultEndpointData epd; + + if (sample==NULL) { + return 0; + } + if (endpoint_data == NULL) { + endpoint_data = (PRESTypePluginEndpointData) &epd; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +/* -------------------------------------------------------------------------------------- +Key Management functions: +* -------------------------------------------------------------------------------------- */ + +PRESTypePluginKeyKind +ConnextStaticRawDataPlugin_get_key_kind(void) +{ + return PRES_TYPEPLUGIN_USER_KEY; +} + +RTIBool +ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(serialize_encapsulation) { + if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if(serialize_key) { + + if (!RTICdrStream_serializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + } + + if(serialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + try { + + char * position = NULL; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(deserialize_encapsulation) { + + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + if (deserialize_key) { + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; + + } catch (std::bad_alloc&) { + return RTI_FALSE; + } +} + +RTIBool ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + RTIBool result; + if (drop_sample) {} /* To avoid warnings */ + stream->_xTypesState.unassignable = RTI_FALSE; + result= ConnextStaticRawDataPlugin_deserialize_key_sample( + endpoint_data, (sample != NULL)?*sample:NULL, stream, + deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; + } + } + + return result; + +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + + if (endpoint_data) {} /* To avoid warnings */ + if (overflow) {} /* To avoid warnings */ + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } + + current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + unsigned int size; + RTIBool overflow = RTI_FALSE; + + size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } + + return size; +} + +RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + if(deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + position = RTICdrStream_resetAlignment(stream); + } + + if (deserialize_key) { + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + + } + + done = RTI_TRUE; + fin: + if(!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + } else { + return RTI_FALSE; + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder *dst, + const ConnextStaticRawData *src) +{ + + if (endpoint_data) {} /* To avoid warnings */ + + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *dst, const + ConnextStaticRawDataKeyHolder *src) +{ + + if (endpoint_data) {} /* To avoid warnings */ + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t *keyhash, + const ConnextStaticRawData *instance) +{ + struct RTICdrStream * md5Stream = NULL; + struct RTICdrStreamState cdrState; + char * buffer = NULL; + + RTICdrStreamState_init(&cdrState); + md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); + + if (md5Stream == NULL) { + return RTI_FALSE; + } + + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + int size; + + RTICdrStream_pushState(md5Stream, &cdrState, -1); + + size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( + endpoint_data, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + 0, + instance); + + if (size <= RTICdrStream_getBufferLength(md5Stream)) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTIOsapiHeap_allocateBuffer(&buffer,size,0); + + if (buffer == NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTICdrStream_set(md5Stream, buffer, size); + RTIOsapiMemory_zero( + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getBufferLength(md5Stream)); + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + return RTI_FALSE; + } + } + + if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > + (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || + PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { + RTICdrStream_computeMD5(md5Stream, keyhash->value); + } else { + RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); + RTIOsapiMemory_copy( + keyhash->value, + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getCurrentPositionOffset(md5Stream)); + } + + keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; + + if (buffer != NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + } + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + DDS_KeyHash_t *keyhash, + RTIBool deserialize_encapsulation, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + ConnextStaticRawData * sample=NULL; + + if (endpoint_plugin_qos) {} /* To avoid warnings */ + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + + if(deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + sample = (ConnextStaticRawData *) + PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); + + if (sample == NULL) { + return RTI_FALSE; + } + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + done = RTI_TRUE; + fin: + if(!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + } else { + return RTI_FALSE; + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + if (!ConnextStaticRawDataPlugin_instance_to_keyhash( + endpoint_data, keyhash, sample)) { + return RTI_FALSE; + } + + return RTI_TRUE; +} + +/* ------------------------------------------------------------------------ +* Plug-in Installation Methods +* ------------------------------------------------------------------------ */ +struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) +{ + struct PRESTypePlugin *plugin = NULL; + const struct PRESTypePluginVersion PLUGIN_VERSION = + PRES_TYPE_PLUGIN_VERSION_2_0; + + RTIOsapiHeap_allocateStructure( + &plugin, struct PRESTypePlugin); + + if (plugin == NULL) { + return NULL; + } + + plugin->version = PLUGIN_VERSION; + + /* set up parent's function pointers */ + plugin->onParticipantAttached = + (PRESTypePluginOnParticipantAttachedCallback) + ConnextStaticRawDataPlugin_on_participant_attached; + plugin->onParticipantDetached = + (PRESTypePluginOnParticipantDetachedCallback) + ConnextStaticRawDataPlugin_on_participant_detached; + plugin->onEndpointAttached = + (PRESTypePluginOnEndpointAttachedCallback) + ConnextStaticRawDataPlugin_on_endpoint_attached; + plugin->onEndpointDetached = + (PRESTypePluginOnEndpointDetachedCallback) + ConnextStaticRawDataPlugin_on_endpoint_detached; + + plugin->copySampleFnc = + (PRESTypePluginCopySampleFunction) + ConnextStaticRawDataPlugin_copy_sample; + plugin->createSampleFnc = + (PRESTypePluginCreateSampleFunction) + ConnextStaticRawDataPlugin_create_sample; + plugin->destroySampleFnc = + (PRESTypePluginDestroySampleFunction) + ConnextStaticRawDataPlugin_destroy_sample; + + plugin->serializeFnc = + (PRESTypePluginSerializeFunction) + ConnextStaticRawDataPlugin_serialize; + plugin->deserializeFnc = + (PRESTypePluginDeserializeFunction) + ConnextStaticRawDataPlugin_deserialize; + plugin->getSerializedSampleMaxSizeFnc = + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size; + plugin->getSerializedSampleMinSizeFnc = + (PRESTypePluginGetSerializedSampleMinSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_min_size; + + plugin->getSampleFnc = + (PRESTypePluginGetSampleFunction) + ConnextStaticRawDataPlugin_get_sample; + plugin->returnSampleFnc = + (PRESTypePluginReturnSampleFunction) + ConnextStaticRawDataPlugin_return_sample; + + plugin->getKeyKindFnc = + (PRESTypePluginGetKeyKindFunction) + ConnextStaticRawDataPlugin_get_key_kind; + + plugin->getSerializedKeyMaxSizeFnc = + (PRESTypePluginGetSerializedKeyMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_key_max_size; + plugin->serializeKeyFnc = + (PRESTypePluginSerializeKeyFunction) + ConnextStaticRawDataPlugin_serialize_key; + plugin->deserializeKeyFnc = + (PRESTypePluginDeserializeKeyFunction) + ConnextStaticRawDataPlugin_deserialize_key; + plugin->deserializeKeySampleFnc = + (PRESTypePluginDeserializeKeySampleFunction) + ConnextStaticRawDataPlugin_deserialize_key_sample; + + plugin-> instanceToKeyHashFnc = + (PRESTypePluginInstanceToKeyHashFunction) + ConnextStaticRawDataPlugin_instance_to_keyhash; + plugin->serializedSampleToKeyHashFnc = + (PRESTypePluginSerializedSampleToKeyHashFunction) + ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; + + plugin->getKeyFnc = + (PRESTypePluginGetKeyFunction) + ConnextStaticRawDataPlugin_get_key; + plugin->returnKeyFnc = + (PRESTypePluginReturnKeyFunction) + ConnextStaticRawDataPlugin_return_key; + + plugin->instanceToKeyFnc = + (PRESTypePluginInstanceToKeyFunction) + ConnextStaticRawDataPlugin_instance_to_key; + plugin->keyToInstanceFnc = + (PRESTypePluginKeyToInstanceFunction) + ConnextStaticRawDataPlugin_key_to_instance; + plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ + plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); + + plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; + + /* Serialized buffer */ + plugin->getBuffer = + (PRESTypePluginGetBufferFunction) + ConnextStaticRawDataPlugin_get_buffer; + plugin->returnBuffer = + (PRESTypePluginReturnBufferFunction) + ConnextStaticRawDataPlugin_return_buffer; + plugin->getSerializedSampleSizeFnc = + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size; + + plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; + + return plugin; +} + +void +ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) +{ + RTIOsapiHeap_freeStructure(plugin); +} +#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h new file mode 100644 index 00000000..89b4c67b --- /dev/null +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h @@ -0,0 +1,340 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef connext_static_raw_dataPlugin_738563504_h +#define connext_static_raw_dataPlugin_738563504_h + +#include "connext_static_raw_data.h" + +struct RTICdrStream; + +#ifndef pres_typePlugin_h +#include "pres/pres_typePlugin.h" +#endif + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) +#endif + +extern "C" { + + /* The type used to store keys for instances of type struct + * AnotherSimple. + * + * By default, this type is struct ConnextStaticRawData + * itself. However, if for some reason this choice is not practical for your + * system (e.g. if sizeof(struct ConnextStaticRawData) + * is very large), you may redefine this typedef in terms of another type of + * your choosing. HOWEVER, if you define the KeyHolder type to be something + * other than struct AnotherSimple, the + * following restriction applies: the key of struct + * ConnextStaticRawData must consist of a + * single field of your redefined KeyHolder type and that field must be the + * first field in struct ConnextStaticRawData. + */ + typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; + + #define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample + #define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer + #define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer + + #define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey + #define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey + + #define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample + #define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample + + /* -------------------------------------------------------------------------------------- + Support functions: + * -------------------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data(void); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData *out, + const ConnextStaticRawData *in); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData *sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData *sample,RTIBool deallocate_pointers); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData *sample); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData *sample, + const char *desc, + unsigned int indent); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_key(void); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder *key); + + /* ---------------------------------------------------------------------------- + Callback functions: + * ---------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern PRESTypePluginParticipantData + ConnextStaticRawDataPlugin_on_participant_attached( + void *registration_data, + const struct PRESTypePluginParticipantInfo *participant_info, + RTIBool top_level_registration, + void *container_plugin_context, + RTICdrTypeCode *typeCode); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data); + + NDDSUSERDllExport extern PRESTypePluginEndpointData + ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo *endpoint_info, + RTIBool top_level_registration, + void *container_plugin_context); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + void *handle); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *out, + const ConnextStaticRawData *in); + + /* ---------------------------------------------------------------------------- + (De)Serialize functions: + * ------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData *sample); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData *sample, + const char * buffer, + unsigned int length); + NDDSUSERDllExport extern DDS_ReturnCode_t + ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample); + + /* -------------------------------------------------------------------------------------- + Key Management functions: + * -------------------------------------------------------------------------------------- */ + NDDSUSERDllExport extern PRESTypePluginKeyKind + ConnextStaticRawDataPlugin_get_key_kind(void); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData ** sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder *key, + const ConnextStaticRawData *instance); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *instance, + const ConnextStaticRawDataKeyHolder *key); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t *keyhash, + const ConnextStaticRawData *instance); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + DDS_KeyHash_t *keyhash, + RTIBool deserialize_encapsulation, + void *endpoint_plugin_qos); + + /* Plugin Functions */ + NDDSUSERDllExport extern struct PRESTypePlugin* + ConnextStaticRawDataPlugin_new(void); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); + +} + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* connext_static_raw_dataPlugin_738563504_h */ + diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx new file mode 100644 index 00000000..83be31bb --- /dev/null +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx @@ -0,0 +1,117 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#include "connext_static_raw_dataSupport.h" +#include "connext_static_raw_dataPlugin.h" + +#ifndef dds_c_log_impl_h +#include "dds_c/dds_c_log_impl.h" +#endif + +/* ========================================================================= */ +/** +<> + +Defines: TData, +TDataWriter, +TDataReader, +TTypeSupport + +Configure and implement 'ConnextStaticRawData' support classes. + +Note: Only the #defined classes get defined +*/ + +/* ----------------------------------------------------------------- */ +/* DDSDataWriter +*/ + +/** +<> + +Defines: TDataWriter, TData +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME + +/* Defines */ +#define TDataWriter ConnextStaticRawDataDataWriter +#define TData ConnextStaticRawData + +#include "dds_cpp/generic/dds_cpp_data_TDataWriter.gen" + +#undef TDataWriter +#undef TData + +#undef TTYPENAME + +/* ----------------------------------------------------------------- */ +/* DDSDataReader +*/ + +/** +<> + +Defines: TDataReader, TDataSeq, TData +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME + +/* Defines */ +#define TDataReader ConnextStaticRawDataDataReader +#define TDataSeq ConnextStaticRawDataSeq +#define TData ConnextStaticRawData + +#include "dds_cpp/generic/dds_cpp_data_TDataReader.gen" + +#undef TDataReader +#undef TDataSeq +#undef TData + +#undef TTYPENAME + +/* ----------------------------------------------------------------- */ +/* TypeSupport + +<> + +Requires: TTYPENAME, +TPlugin_new +TPlugin_delete +Defines: TTypeSupport, TData, TDataReader, TDataWriter +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME +#define TPlugin_new ConnextStaticRawDataPlugin_new +#define TPlugin_delete ConnextStaticRawDataPlugin_delete + +/* Defines */ +#define TTypeSupport ConnextStaticRawDataTypeSupport +#define TData ConnextStaticRawData +#define TDataReader ConnextStaticRawDataDataReader +#define TDataWriter ConnextStaticRawDataDataWriter +#define TGENERATE_SER_CODE +#define TGENERATE_TYPECODE + +#include "dds_cpp/generic/dds_cpp_data_TTypeSupport.gen" + +#undef TTypeSupport +#undef TData +#undef TDataReader +#undef TDataWriter +#undef TGENERATE_TYPECODE +#undef TGENERATE_SER_CODE +#undef TTYPENAME +#undef TPlugin_new +#undef TPlugin_delete + diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h new file mode 100644 index 00000000..012a1f61 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h @@ -0,0 +1,62 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef connext_static_raw_dataSupport_738563504_h +#define connext_static_raw_dataSupport_738563504_h + +/* Uses */ +#include "connext_static_raw_data.h" + +#ifndef ndds_cpp_h +#include "ndds/ndds_cpp.h" +#endif + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) + +class __declspec(dllimport) DDSTypeSupport; +class __declspec(dllimport) DDSDataWriter; +class __declspec(dllimport) DDSDataReader; + +#endif + +/* ========================================================================= */ +/** +Uses: T + +Defines: TTypeSupport, TDataWriter, TDataReader + +Organized using the well-documented "Generics Pattern" for +implementing generics in C and C++. +*/ + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) + +#endif + +DDS_TYPESUPPORT_CPP( + ConnextStaticRawDataTypeSupport, + ConnextStaticRawData); + +DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); +DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* connext_static_raw_dataSupport_738563504_h */ + diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.cxx b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.cxx new file mode 100644 index 00000000..7b954282 --- /dev/null +++ b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.cxx @@ -0,0 +1,313 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef NDDS_STANDALONE_TYPE +#ifndef ndds_cpp_h +#include "ndds/ndds_cpp.h" +#endif +#ifndef dds_c_log_impl_h +#include "dds_c/dds_c_log_impl.h" +#endif + +#ifndef cdr_type_h +#include "cdr/cdr_type.h" +#endif + +#ifndef osapi_heap_h +#include "osapi/osapi_heap.h" +#endif +#else +#include "ndds_standalone_type.h" +#endif + +#include "connext_static_raw_data.h" + +#include + +/* ========================================================================= */ +const char *ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; + +DDS_TypeCode* ConnextStaticRawData_get_typecode() +{ + static RTIBool is_initialized = RTI_FALSE; + + static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array =DDS_INITIALIZE_ARRAY_TYPECODE(1,(KEY_HASH_LENGTH_16), NULL,NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); + static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); + static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3]= + { + + { + (char *)"key_hash",/* Member name */ + { + 0,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_KEY_MEMBER , /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + }, + { + (char *)"serialized_key",/* Member name */ + { + 1,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + }, + { + (char *)"serialized_data",/* Member name */ + { + 2,/* Representation ID */ + DDS_BOOLEAN_FALSE,/* Is a pointer? */ + -1, /* Bitfield bits */ + NULL/* Member type code is assigned later */ + }, + 0, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ + DDS_PUBLIC_MEMBER,/* Member visibility */ + 1, + NULL/* Ignored */ + } + }; + + static DDS_TypeCode ConnextStaticRawData_g_tc = + {{ + DDS_TK_STRUCT,/* Kind */ + DDS_BOOLEAN_FALSE, /* Ignored */ + -1, /*Ignored*/ + (char *)"ConnextStaticRawData", /* Name */ + NULL, /* Ignored */ + 0, /* Ignored */ + 0, /* Ignored */ + NULL, /* Ignored */ + 3, /* Number of members */ + ConnextStaticRawData_g_tc_members, /* Members */ + DDS_VM_NONE /* Ignored */ + }}; /* Type code for ConnextStaticRawData*/ + + if (is_initialized) { + return &ConnextStaticRawData_g_tc; + } + + ConnextStaticRawData_g_tc_key_hash_array._data._typeCode =(RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + + ConnextStaticRawData_g_tc_members[0]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_key_hash_array; + ConnextStaticRawData_g_tc_members[1]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_key_sequence; + ConnextStaticRawData_g_tc_members[2]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_data_sequence; + + is_initialized = RTI_TRUE; + + return &ConnextStaticRawData_g_tc; +} + +RTIBool ConnextStaticRawData_initialize( + ConnextStaticRawData* sample) { + return ConnextStaticRawData_initialize_ex(sample,RTI_TRUE,RTI_TRUE); +} + +RTIBool ConnextStaticRawData_initialize_ex( + ConnextStaticRawData* sample,RTIBool allocatePointers, RTIBool allocateMemory) +{ + + struct DDS_TypeAllocationParams_t allocParams = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; + + allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; + allocParams.allocate_memory = (DDS_Boolean)allocateMemory; + + return ConnextStaticRawData_initialize_w_params( + sample,&allocParams); + +} + +RTIBool ConnextStaticRawData_initialize_w_params( + ConnextStaticRawData* sample, const struct DDS_TypeAllocationParams_t * allocParams) +{ + + void* buffer = NULL; + if (buffer) {} /* To avoid warnings */ + + if (sample == NULL) { + return RTI_FALSE; + } + if (allocParams == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_initArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_key ); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key , RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { + return RTI_FALSE; + } + } else { + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + } + if (allocParams->allocate_memory) { + DDS_OctetSeq_initialize(&sample->serialized_data ); + DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data , RTI_INT32_MAX); + if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { + return RTI_FALSE; + } + } else { + DDS_OctetSeq_set_length(&sample->serialized_data, 0); + } + return RTI_TRUE; +} + +void ConnextStaticRawData_finalize( + ConnextStaticRawData* sample) +{ + + ConnextStaticRawData_finalize_ex(sample,RTI_TRUE); +} + +void ConnextStaticRawData_finalize_ex( + ConnextStaticRawData* sample,RTIBool deletePointers) +{ + struct DDS_TypeDeallocationParams_t deallocParams = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; + + if (sample==NULL) { + return; + } + + deallocParams.delete_pointers = (DDS_Boolean)deletePointers; + + ConnextStaticRawData_finalize_w_params( + sample,&deallocParams); +} + +void ConnextStaticRawData_finalize_w_params( + ConnextStaticRawData* sample,const struct DDS_TypeDeallocationParams_t * deallocParams) +{ + + if (sample==NULL) { + return; + } + + if (deallocParams == NULL) { + return; + } + + DDS_OctetSeq_finalize(&sample->serialized_key); + + DDS_OctetSeq_finalize(&sample->serialized_data); + +} + +void ConnextStaticRawData_finalize_optional_members( + ConnextStaticRawData* sample, RTIBool deletePointers) +{ + struct DDS_TypeDeallocationParams_t deallocParamsTmp = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; + struct DDS_TypeDeallocationParams_t * deallocParams = + &deallocParamsTmp; + + if (sample==NULL) { + return; + } + if (deallocParams) {} /* To avoid warnings */ + + deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; + deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; + +} + +RTIBool ConnextStaticRawData_copy( + ConnextStaticRawData* dst, + const ConnextStaticRawData* src) +{ + try { + + if (dst == NULL || src == NULL) { + return RTI_FALSE; + } + + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_key , + &src->serialized_key )) { + return RTI_FALSE; + } + if (!DDS_OctetSeq_copy(&dst->serialized_data , + &src->serialized_data )) { + return RTI_FALSE; + } + + return RTI_TRUE; + + } catch (std::bad_alloc&) { + return RTI_FALSE; + } +} + +/** +* <> +* +* Defines: TSeq, T +* +* Configure and implement 'ConnextStaticRawData' sequence class. +*/ +#define T ConnextStaticRawData +#define TSeq ConnextStaticRawDataSeq + +#define T_initialize_w_params ConnextStaticRawData_initialize_w_params + +#define T_finalize_w_params ConnextStaticRawData_finalize_w_params +#define T_copy ConnextStaticRawData_copy + +#ifndef NDDS_STANDALONE_TYPE +#include "dds_c/generic/dds_c_sequence_TSeq.gen" +#include "dds_cpp/generic/dds_cpp_sequence_TSeq.gen" +#else +#include "dds_c_sequence_TSeq.gen" +#include "dds_cpp_sequence_TSeq.gen" +#endif + +#undef T_copy +#undef T_finalize_w_params + +#undef T_initialize_w_params + +#undef TSeq +#undef T + diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.h b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.h new file mode 100644 index 00000000..d3adb4fa --- /dev/null +++ b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.h @@ -0,0 +1,106 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef connext_static_raw_data_738563504_h +#define connext_static_raw_data_738563504_h + +#ifndef NDDS_STANDALONE_TYPE +#ifndef ndds_cpp_h +#include "ndds/ndds_cpp.h" +#endif +#else +#include "ndds_standalone_type.h" +#endif + +static const DDS_Long KEY_HASH_LENGTH_16= 16; +extern "C" { + + extern const char *ConnextStaticRawDataTYPENAME; + +} + +struct ConnextStaticRawDataSeq; +#ifndef NDDS_STANDALONE_TYPE +class ConnextStaticRawDataTypeSupport; +class ConnextStaticRawDataDataWriter; +class ConnextStaticRawDataDataReader; +#endif + +class ConnextStaticRawData +{ + public: + typedef struct ConnextStaticRawDataSeq Seq; + #ifndef NDDS_STANDALONE_TYPE + typedef ConnextStaticRawDataTypeSupport TypeSupport; + typedef ConnextStaticRawDataDataWriter DataWriter; + typedef ConnextStaticRawDataDataReader DataReader; + #endif + + DDS_Octet key_hash [(KEY_HASH_LENGTH_16)]; + DDS_OctetSeq serialized_key ; + DDS_OctetSeq serialized_data ; + +}; +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) +#endif + +NDDSUSERDllExport DDS_TypeCode* ConnextStaticRawData_get_typecode(void); /* Type code */ + +DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize( + ConnextStaticRawData* self); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize_ex( + ConnextStaticRawData* self,RTIBool allocatePointers,RTIBool allocateMemory); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_initialize_w_params( + ConnextStaticRawData* self, + const struct DDS_TypeAllocationParams_t * allocParams); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize( + ConnextStaticRawData* self); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_ex( + ConnextStaticRawData* self,RTIBool deletePointers); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_w_params( + ConnextStaticRawData* self, + const struct DDS_TypeDeallocationParams_t * deallocParams); + +NDDSUSERDllExport +void ConnextStaticRawData_finalize_optional_members( + ConnextStaticRawData* self, RTIBool deletePointers); + +NDDSUSERDllExport +RTIBool ConnextStaticRawData_copy( + ConnextStaticRawData* dst, + const ConnextStaticRawData* src); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* connext_static_raw_data */ + diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.cxx b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.cxx new file mode 100644 index 00000000..78b58d54 --- /dev/null +++ b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.cxx @@ -0,0 +1,1531 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#include + +#ifndef ndds_cpp_h +#include "ndds/ndds_cpp.h" +#endif + +#ifndef osapi_type_h +#include "osapi/osapi_type.h" +#endif +#ifndef osapi_heap_h +#include "osapi/osapi_heap.h" +#endif + +#ifndef osapi_utility_h +#include "osapi/osapi_utility.h" +#endif + +#ifndef cdr_type_h +#include "cdr/cdr_type.h" +#endif + +#ifndef cdr_type_object_h +#include "cdr/cdr_typeObject.h" +#endif + +#ifndef cdr_encapsulation_h +#include "cdr/cdr_encapsulation.h" +#endif + +#ifndef cdr_stream_h +#include "cdr/cdr_stream.h" +#endif + +#ifndef cdr_log_h +#include "cdr/cdr_log.h" +#endif + +#ifndef pres_typePlugin_h +#include "pres/pres_typePlugin.h" +#endif + +#define RTI_CDR_CURRENT_SUBMODULE RTI_CDR_SUBMODULE_MASK_STREAM + +#include + +#include "connext_static_raw_dataPlugin.h" + +/* ---------------------------------------------------------------------------- +* Type ConnextStaticRawData +* -------------------------------------------------------------------------- */ + +/* ----------------------------------------------------------------------------- +Support functions: +* -------------------------------------------------------------------------- */ + +ConnextStaticRawData* +ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params) +{ + ConnextStaticRawData *sample = NULL; + + sample = new (std::nothrow) ConnextStaticRawData ; + if (sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_w_params(sample,alloc_params)) { + delete sample; + sample=NULL; + } + return sample; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) +{ + ConnextStaticRawData *sample = NULL; + + sample = new (std::nothrow) ConnextStaticRawData ; + + if(sample == NULL) { + return NULL; + } + + if (!ConnextStaticRawData_initialize_ex(sample,allocate_pointers, RTI_TRUE)) { + delete sample; + sample=NULL; + } + + return sample; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_data(void) +{ + return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); +} + +void +ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData *sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params) { + + ConnextStaticRawData_finalize_w_params(sample,dealloc_params); + + delete sample; + sample=NULL; +} + +void +ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData *sample,RTIBool deallocate_pointers) { + + ConnextStaticRawData_finalize_ex(sample,deallocate_pointers); + + delete sample; + sample=NULL; +} + +void +ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData *sample) { + + ConnextStaticRawDataPluginSupport_destroy_data_ex(sample,RTI_TRUE); + +} + +RTIBool +ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData *dst, + const ConnextStaticRawData *src) +{ + return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); +} + +void +ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData *sample, + const char *desc, + unsigned int indent_level) +{ + + RTICdrType_printIndent(indent_level); + + if (desc != NULL) { + RTILog_debug("%s:\n", desc); + } else { + RTILog_debug("\n"); + } + + if (sample == NULL) { + RTILog_debug("NULL\n"); + return; + } + + RTICdrType_printArray( + sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "key_hash", indent_level + 1); + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key ), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_key", indent_level + 1); + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + RTICdrType_printArray( + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_SIZE, + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } else { + RTICdrType_printPointerArray( + DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data ), + (RTICdrTypePrintFunction)RTICdrType_printOctet, + "serialized_data", indent_level + 1); + } + +} +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers){ + ConnextStaticRawData *key = NULL; + + key = new (std::nothrow) ConnextStaticRawDataKeyHolder ; + + ConnextStaticRawData_initialize_ex(key,allocate_pointers, RTI_TRUE); + + return key; +} + +ConnextStaticRawData * +ConnextStaticRawDataPluginSupport_create_key(void) +{ + return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); +} + +void +ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) +{ + ConnextStaticRawData_finalize_ex(key,deallocate_pointers); + + delete key; + key=NULL; + +} + +void +ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder *key) { + + ConnextStaticRawDataPluginSupport_destroy_key_ex(key,RTI_TRUE); + +} + +/* ---------------------------------------------------------------------------- +Callback functions: +* ---------------------------------------------------------------------------- */ + +PRESTypePluginParticipantData +ConnextStaticRawDataPlugin_on_participant_attached( + void *registration_data, + const struct PRESTypePluginParticipantInfo *participant_info, + RTIBool top_level_registration, + void *container_plugin_context, + RTICdrTypeCode *type_code) +{ + if (registration_data) {} /* To avoid warnings */ + if (participant_info) {} /* To avoid warnings */ + if (top_level_registration) {} /* To avoid warnings */ + if (container_plugin_context) {} /* To avoid warnings */ + if (type_code) {} /* To avoid warnings */ + + return PRESTypePluginDefaultParticipantData_new(participant_info); + +} + +void +ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data) +{ + + PRESTypePluginDefaultParticipantData_delete(participant_data); +} + +PRESTypePluginEndpointData +ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo *endpoint_info, + RTIBool top_level_registration, + void *containerPluginContext) +{ + PRESTypePluginEndpointData epd = NULL; + + unsigned int serializedSampleMaxSize; + + unsigned int serializedKeyMaxSize; + + if (top_level_registration) {} /* To avoid warnings */ + if (containerPluginContext) {} /* To avoid warnings */ + + epd = PRESTypePluginDefaultEndpointData_new( + participant_data, + endpoint_info, + (PRESTypePluginDefaultEndpointDataCreateSampleFunction) + ConnextStaticRawDataPluginSupport_create_data, + (PRESTypePluginDefaultEndpointDataDestroySampleFunction) + ConnextStaticRawDataPluginSupport_destroy_data, + (PRESTypePluginDefaultEndpointDataCreateKeyFunction) + ConnextStaticRawDataPluginSupport_create_key , + (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) + ConnextStaticRawDataPluginSupport_destroy_key); + + if (epd == NULL) { + return NULL; + } + serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( + epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); + + if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( + epd,endpoint_info,serializedKeyMaxSize)) + { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + + if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { + serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); + + PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); + + if (PRESTypePluginDefaultEndpointData_createWriterPool( + epd, + endpoint_info, + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size, + epd) == RTI_FALSE) { + PRESTypePluginDefaultEndpointData_delete(epd); + return NULL; + } + } + + return epd; +} + +void +ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data) +{ + + PRESTypePluginDefaultEndpointData_delete(endpoint_data); +} + +void +ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + void *handle) +{ + + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + + PRESTypePluginDefaultEndpointData_returnSample( + endpoint_data, sample, handle); +} + +RTIBool +ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *dst, + const ConnextStaticRawData *src) +{ + if (endpoint_data) {} /* To avoid warnings */ + return ConnextStaticRawDataPluginSupport_copy_data(dst,src); +} + +/* ---------------------------------------------------------------------------- +(De)Serialize functions: +* ------------------------------------------------------------------------- */ +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + +RTIBool +ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + RTIBool retval = RTI_TRUE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(serialize_encapsulation) { + if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if(serialize_sample) { + + if (!RTICdrStream_serializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + if (!RTICdrStream_serializePrimitiveSequence( + stream, + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + (RTI_INT32_MAX-1), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } else { + if (!RTICdrStream_serializePrimitivePointerSequence( + stream, + (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + DDS_OctetSeq_get_length(&sample->serialized_key), + (RTI_INT32_MAX-1), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } + + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + if (!RTICdrStream_serializePrimitiveSequence( + stream, + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + (RTI_INT32_MAX-1), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } else { + if (!RTICdrStream_serializePrimitivePointerSequence( + stream, + (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + DDS_OctetSeq_get_length(&sample->serialized_data), + (RTI_INT32_MAX-1), + RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + } + + } + + if(serialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return retval; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos) +{ + + char * position = NULL; + + RTIBool done = RTI_FALSE; + + try { + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + if(deserialize_encapsulation) { + + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + if(deserialize_sample) { + + ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + goto fin; + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { + goto fin; + } + if (!DDS_OctetSeq_set_maximum(&sample->serialized_key,sequence_length)) { + return RTI_FALSE; + } + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { + if (!RTICdrStream_deserializePrimitiveSequence( + stream, + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), + &sequence_length, + DDS_OctetSeq_get_maximum(&sample->serialized_key), + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } else { + if (!RTICdrStream_deserializePrimitivePointerSequence( + stream, + (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), + &sequence_length, + DDS_OctetSeq_get_maximum(&sample->serialized_key), + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + if (!DDS_OctetSeq_set_length(&sample->serialized_key, sequence_length)) { + return RTI_FALSE; + } + + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { + goto fin; + } + if (!DDS_OctetSeq_set_maximum(&sample->serialized_data,sequence_length)) { + return RTI_FALSE; + } + if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { + if (!RTICdrStream_deserializePrimitiveSequence( + stream, + DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), + &sequence_length, + DDS_OctetSeq_get_maximum(&sample->serialized_data), + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } else { + if (!RTICdrStream_deserializePrimitivePointerSequence( + stream, + (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), + &sequence_length, + DDS_OctetSeq_get_maximum(&sample->serialized_data), + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + if (!DDS_OctetSeq_set_length(&sample->serialized_data, sequence_length)) { + return RTI_FALSE; + } + + } + } + + done = RTI_TRUE; + fin: + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; + + } catch (std::bad_alloc&) { + return RTI_FALSE; + } +} + +RTIBool +ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData *sample) +{ + struct RTICdrStream stream; + struct PRESTypePluginDefaultEndpointData epd; + RTIBool result; + + if (length == NULL) { + return RTI_FALSE; + } + + epd._maxSizeSerializedSample = + ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); + + if (buffer == NULL) { + *length = + ConnextStaticRawDataPlugin_get_serialized_sample_size( + (PRESTypePluginEndpointData)&epd, + RTI_TRUE, + RTICdrEncapsulation_getNativeCdrEncapsulationId(), + 0, + sample); + + if (*length == 0) { + return RTI_FALSE; + } + + return RTI_TRUE; + } + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, *length); + + result = ConnextStaticRawDataPlugin_serialize( + (PRESTypePluginEndpointData)&epd, sample, &stream, + RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), + RTI_TRUE, NULL); + + *length = RTICdrStream_getCurrentPositionOffset(&stream); + return result; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData *sample, + const char * buffer, + unsigned int length) +{ + struct RTICdrStream stream; + + RTICdrStream_init(&stream); + RTICdrStream_set(&stream, (char *)buffer, length); + + ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); + return ConnextStaticRawDataPlugin_deserialize_sample( + NULL, sample, + &stream, RTI_TRUE, RTI_TRUE, + NULL); +} + +DDS_ReturnCode_t +ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property) +{ + DDS_DynamicData *data = NULL; + char *buffer = NULL; + unsigned int length = 0; + struct DDS_PrintFormat printFormat; + DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; + + if (sample == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (str_size == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (property == NULL) { + return DDS_RETCODE_BAD_PARAMETER; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + NULL, + &length, + sample)) { + return DDS_RETCODE_ERROR; + } + + RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); + if (buffer == NULL) { + return DDS_RETCODE_ERROR; + } + + if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + buffer, + &length, + sample)) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } + + data = DDS_DynamicData_new( + ConnextStaticRawData_get_typecode(), + &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); + if (data == NULL) { + RTIOsapiHeap_freeBuffer(buffer); + return DDS_RETCODE_ERROR; + } + + retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_PrintFormatProperty_to_print_format( + property, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + retCode = DDS_DynamicDataFormatter_to_string_w_format( + data, + str, + str_size, + &printFormat); + if (retCode != DDS_RETCODE_OK) { + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return retCode; + } + + RTIOsapiHeap_freeBuffer(buffer); + DDS_DynamicData_delete(data); + return DDS_RETCODE_OK; +} + +RTIBool +ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos) +{ + + RTIBool result; + const char *METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; + if (drop_sample) {} /* To avoid warnings */ + + stream->_xTypesState.unassignable = RTI_FALSE; + result= ConnextStaticRawDataPlugin_deserialize_sample( + endpoint_data, (sample != NULL)?*sample:NULL, + stream, deserialize_encapsulation, deserialize_sample, + endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; + } + } + if (!result && stream->_xTypesState.unassignable ) { + + RTICdrLog_exception( + METHOD_NAME, + &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, + "ConnextStaticRawData"); + + } + + return result; + +} + +RTIBool ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(skip_encapsulation) { + if (!RTICdrStream_skipEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if (skip_sample) { + + if (!RTICdrStream_skipPrimitiveArray( + stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + goto fin; + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + } + + done = RTI_TRUE; + fin: + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + if(skip_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + if (endpoint_data) {} /* To avoid warnings */ + if (include_encapsulation) {} + if (encapsulation_id) {} + if (current_alignment) {} + + if (overflow != NULL) { + *overflow = RTI_TRUE; + } + + return RTI_CDR_MAX_SERIALIZED_SIZE; + +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + unsigned int size; + RTIBool overflow = RTI_FALSE; + + size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } + + return size; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + + if (endpoint_data) {} /* To avoid warnings */ + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } + + current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment,0, RTI_CDR_OCTET_TYPE); + current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( + current_alignment,0, RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +/* Returns the size of the sample in its serialized form (in bytes). +* It can also be an estimation in excess of the real buffer needed +* during a call to the serialize() function. +* The value reported does not have to include the space for the +* encapsulation flags. +*/ +unsigned int +ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + struct PRESTypePluginDefaultEndpointData epd; + + if (sample==NULL) { + return 0; + } + if (endpoint_data == NULL) { + endpoint_data = (PRESTypePluginEndpointData) &epd; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + PRESTypePluginDefaultEndpointData_setBaseAlignment( + endpoint_data, + current_alignment); + } + + current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_key), + RTI_CDR_OCTET_TYPE); + + current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( + PRESTypePluginDefaultEndpointData_getAlignment( + endpoint_data, current_alignment), + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +/* -------------------------------------------------------------------------------------- +Key Management functions: +* -------------------------------------------------------------------------------------- */ + +PRESTypePluginKeyKind +ConnextStaticRawDataPlugin_get_key_kind(void) +{ + return PRES_TYPEPLUGIN_USER_KEY; +} + +RTIBool +ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(serialize_encapsulation) { + if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + if(serialize_key) { + + if (!RTICdrStream_serializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + } + + if(serialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + try { + + char * position = NULL; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if(deserialize_encapsulation) { + + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + if (deserialize_key) { + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; + + } catch (std::bad_alloc&) { + return RTI_FALSE; + } +} + +RTIBool ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + RTIBool result; + if (drop_sample) {} /* To avoid warnings */ + stream->_xTypesState.unassignable = RTI_FALSE; + result= ConnextStaticRawDataPlugin_deserialize_key_sample( + endpoint_data, (sample != NULL)?*sample:NULL, stream, + deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); + if (result) { + if (stream->_xTypesState.unassignable) { + result = RTI_FALSE; + } + } + + return result; + +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + + unsigned int initial_alignment = current_alignment; + + unsigned int encapsulation_size = current_alignment; + + if (endpoint_data) {} /* To avoid warnings */ + if (overflow) {} /* To avoid warnings */ + + if (include_encapsulation) { + + if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { + return 1; + } + RTICdrStream_getEncapsulationSize(encapsulation_size); + encapsulation_size -= current_alignment; + current_alignment = 0; + initial_alignment = 0; + } + + current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( + current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); + + if (include_encapsulation) { + current_alignment += encapsulation_size; + } + return current_alignment - initial_alignment; +} + +unsigned int +ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment) +{ + unsigned int size; + RTIBool overflow = RTI_FALSE; + + size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); + + if (overflow) { + size = RTI_CDR_MAX_SERIALIZED_SIZE; + } + + return size; +} + +RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + if(deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + position = RTICdrStream_resetAlignment(stream); + } + + if (deserialize_key) { + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + + { + RTICdrUnsignedLong sequence_length; + if (!RTICdrStream_skipPrimitiveSequence( + stream, + &sequence_length, + RTI_CDR_OCTET_TYPE)){ + goto fin; + } + } + + } + + done = RTI_TRUE; + fin: + if(!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + } else { + return RTI_FALSE; + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder *dst, + const ConnextStaticRawData *src) +{ + + if (endpoint_data) {} /* To avoid warnings */ + + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *dst, const + ConnextStaticRawDataKeyHolder *src) +{ + + if (endpoint_data) {} /* To avoid warnings */ + if (!RTICdrType_copyArray( + dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { + return RTI_FALSE; + } + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t *keyhash, + const ConnextStaticRawData *instance) +{ + struct RTICdrStream * md5Stream = NULL; + struct RTICdrStreamState cdrState; + char * buffer = NULL; + + RTICdrStreamState_init(&cdrState); + md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); + + if (md5Stream == NULL) { + return RTI_FALSE; + } + + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + int size; + + RTICdrStream_pushState(md5Stream, &cdrState, -1); + + size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( + endpoint_data, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + 0, + instance); + + if (size <= RTICdrStream_getBufferLength(md5Stream)) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTIOsapiHeap_allocateBuffer(&buffer,size,0); + + if (buffer == NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + return RTI_FALSE; + } + + RTICdrStream_set(md5Stream, buffer, size); + RTIOsapiMemory_zero( + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getBufferLength(md5Stream)); + RTICdrStream_resetPosition(md5Stream); + RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); + if (!ConnextStaticRawDataPlugin_serialize_key( + endpoint_data, + instance, + md5Stream, + RTI_FALSE, + RTI_CDR_ENCAPSULATION_ID_CDR_BE, + RTI_TRUE, + NULL)) + { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + return RTI_FALSE; + } + } + + if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > + (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || + PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { + RTICdrStream_computeMD5(md5Stream, keyhash->value); + } else { + RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); + RTIOsapiMemory_copy( + keyhash->value, + RTICdrStream_getBuffer(md5Stream), + RTICdrStream_getCurrentPositionOffset(md5Stream)); + } + + keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; + + if (buffer != NULL) { + RTICdrStream_popState(md5Stream, &cdrState); + RTIOsapiHeap_freeBuffer(buffer); + } + + return RTI_TRUE; +} + +RTIBool +ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + DDS_KeyHash_t *keyhash, + RTIBool deserialize_encapsulation, + void *endpoint_plugin_qos) +{ + char * position = NULL; + + RTIBool done = RTI_FALSE; + RTIBool error = RTI_FALSE; + ConnextStaticRawData * sample=NULL; + + if (endpoint_plugin_qos) {} /* To avoid warnings */ + if (stream == NULL) { + error = RTI_TRUE; + goto fin; + } + + if(deserialize_encapsulation) { + if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + } + + sample = (ConnextStaticRawData *) + PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); + + if (sample == NULL) { + return RTI_FALSE; + } + + if (!RTICdrStream_deserializePrimitiveArray( + stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { + return RTI_FALSE; + } + + done = RTI_TRUE; + fin: + if(!error) { + if (done != RTI_TRUE && + RTICdrStream_getRemainder(stream) >= + RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { + return RTI_FALSE; + } + } else { + return RTI_FALSE; + } + + if(deserialize_encapsulation) { + RTICdrStream_restoreAlignment(stream,position); + } + + if (!ConnextStaticRawDataPlugin_instance_to_keyhash( + endpoint_data, keyhash, sample)) { + return RTI_FALSE; + } + + return RTI_TRUE; +} + +/* ------------------------------------------------------------------------ +* Plug-in Installation Methods +* ------------------------------------------------------------------------ */ +struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) +{ + struct PRESTypePlugin *plugin = NULL; + const struct PRESTypePluginVersion PLUGIN_VERSION = + PRES_TYPE_PLUGIN_VERSION_2_0; + + RTIOsapiHeap_allocateStructure( + &plugin, struct PRESTypePlugin); + + if (plugin == NULL) { + return NULL; + } + + plugin->version = PLUGIN_VERSION; + + /* set up parent's function pointers */ + plugin->onParticipantAttached = + (PRESTypePluginOnParticipantAttachedCallback) + ConnextStaticRawDataPlugin_on_participant_attached; + plugin->onParticipantDetached = + (PRESTypePluginOnParticipantDetachedCallback) + ConnextStaticRawDataPlugin_on_participant_detached; + plugin->onEndpointAttached = + (PRESTypePluginOnEndpointAttachedCallback) + ConnextStaticRawDataPlugin_on_endpoint_attached; + plugin->onEndpointDetached = + (PRESTypePluginOnEndpointDetachedCallback) + ConnextStaticRawDataPlugin_on_endpoint_detached; + + plugin->copySampleFnc = + (PRESTypePluginCopySampleFunction) + ConnextStaticRawDataPlugin_copy_sample; + plugin->createSampleFnc = + (PRESTypePluginCreateSampleFunction) + ConnextStaticRawDataPlugin_create_sample; + plugin->destroySampleFnc = + (PRESTypePluginDestroySampleFunction) + ConnextStaticRawDataPlugin_destroy_sample; + + plugin->serializeFnc = + (PRESTypePluginSerializeFunction) + ConnextStaticRawDataPlugin_serialize; + plugin->deserializeFnc = + (PRESTypePluginDeserializeFunction) + ConnextStaticRawDataPlugin_deserialize; + plugin->getSerializedSampleMaxSizeFnc = + (PRESTypePluginGetSerializedSampleMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_max_size; + plugin->getSerializedSampleMinSizeFnc = + (PRESTypePluginGetSerializedSampleMinSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_min_size; + + plugin->getSampleFnc = + (PRESTypePluginGetSampleFunction) + ConnextStaticRawDataPlugin_get_sample; + plugin->returnSampleFnc = + (PRESTypePluginReturnSampleFunction) + ConnextStaticRawDataPlugin_return_sample; + + plugin->getKeyKindFnc = + (PRESTypePluginGetKeyKindFunction) + ConnextStaticRawDataPlugin_get_key_kind; + + plugin->getSerializedKeyMaxSizeFnc = + (PRESTypePluginGetSerializedKeyMaxSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_key_max_size; + plugin->serializeKeyFnc = + (PRESTypePluginSerializeKeyFunction) + ConnextStaticRawDataPlugin_serialize_key; + plugin->deserializeKeyFnc = + (PRESTypePluginDeserializeKeyFunction) + ConnextStaticRawDataPlugin_deserialize_key; + plugin->deserializeKeySampleFnc = + (PRESTypePluginDeserializeKeySampleFunction) + ConnextStaticRawDataPlugin_deserialize_key_sample; + + plugin-> instanceToKeyHashFnc = + (PRESTypePluginInstanceToKeyHashFunction) + ConnextStaticRawDataPlugin_instance_to_keyhash; + plugin->serializedSampleToKeyHashFnc = + (PRESTypePluginSerializedSampleToKeyHashFunction) + ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; + + plugin->getKeyFnc = + (PRESTypePluginGetKeyFunction) + ConnextStaticRawDataPlugin_get_key; + plugin->returnKeyFnc = + (PRESTypePluginReturnKeyFunction) + ConnextStaticRawDataPlugin_return_key; + + plugin->instanceToKeyFnc = + (PRESTypePluginInstanceToKeyFunction) + ConnextStaticRawDataPlugin_instance_to_key; + plugin->keyToInstanceFnc = + (PRESTypePluginKeyToInstanceFunction) + ConnextStaticRawDataPlugin_key_to_instance; + plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ + plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); + + plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; + + /* Serialized buffer */ + plugin->getBuffer = + (PRESTypePluginGetBufferFunction) + ConnextStaticRawDataPlugin_get_buffer; + plugin->returnBuffer = + (PRESTypePluginReturnBufferFunction) + ConnextStaticRawDataPlugin_return_buffer; + plugin->getSerializedSampleSizeFnc = + (PRESTypePluginGetSerializedSampleSizeFunction) + ConnextStaticRawDataPlugin_get_serialized_sample_size; + + plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; + + return plugin; +} + +void +ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) +{ + RTIOsapiHeap_freeStructure(plugin); +} +#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.h b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.h new file mode 100644 index 00000000..89b4c67b --- /dev/null +++ b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.h @@ -0,0 +1,340 @@ + + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef connext_static_raw_dataPlugin_738563504_h +#define connext_static_raw_dataPlugin_738563504_h + +#include "connext_static_raw_data.h" + +struct RTICdrStream; + +#ifndef pres_typePlugin_h +#include "pres/pres_typePlugin.h" +#endif + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) +#endif + +extern "C" { + + /* The type used to store keys for instances of type struct + * AnotherSimple. + * + * By default, this type is struct ConnextStaticRawData + * itself. However, if for some reason this choice is not practical for your + * system (e.g. if sizeof(struct ConnextStaticRawData) + * is very large), you may redefine this typedef in terms of another type of + * your choosing. HOWEVER, if you define the KeyHolder type to be something + * other than struct AnotherSimple, the + * following restriction applies: the key of struct + * ConnextStaticRawData must consist of a + * single field of your redefined KeyHolder type and that field must be the + * first field in struct ConnextStaticRawData. + */ + typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; + + #define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample + #define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer + #define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer + + #define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey + #define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey + + #define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample + #define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample + + /* -------------------------------------------------------------------------------------- + Support functions: + * -------------------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data_w_params( + const struct DDS_TypeAllocationParams_t * alloc_params); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_data(void); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPluginSupport_copy_data( + ConnextStaticRawData *out, + const ConnextStaticRawData *in); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data_w_params( + ConnextStaticRawData *sample, + const struct DDS_TypeDeallocationParams_t * dealloc_params); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data_ex( + ConnextStaticRawData *sample,RTIBool deallocate_pointers); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_data( + ConnextStaticRawData *sample); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_print_data( + const ConnextStaticRawData *sample, + const char *desc, + unsigned int indent); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); + + NDDSUSERDllExport extern ConnextStaticRawData* + ConnextStaticRawDataPluginSupport_create_key(void); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_key_ex( + ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPluginSupport_destroy_key( + ConnextStaticRawDataKeyHolder *key); + + /* ---------------------------------------------------------------------------- + Callback functions: + * ---------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern PRESTypePluginParticipantData + ConnextStaticRawDataPlugin_on_participant_attached( + void *registration_data, + const struct PRESTypePluginParticipantInfo *participant_info, + RTIBool top_level_registration, + void *container_plugin_context, + RTICdrTypeCode *typeCode); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_on_participant_detached( + PRESTypePluginParticipantData participant_data); + + NDDSUSERDllExport extern PRESTypePluginEndpointData + ConnextStaticRawDataPlugin_on_endpoint_attached( + PRESTypePluginParticipantData participant_data, + const struct PRESTypePluginEndpointInfo *endpoint_info, + RTIBool top_level_registration, + void *container_plugin_context); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_on_endpoint_detached( + PRESTypePluginEndpointData endpoint_data); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_return_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + void *handle); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_copy_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *out, + const ConnextStaticRawData *in); + + /* ---------------------------------------------------------------------------- + (De)Serialize functions: + * ------------------------------------------------------------------------- */ + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( + char * buffer, + unsigned int * length, + const ConnextStaticRawData *sample); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData **sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( + ConnextStaticRawData *sample, + const char * buffer, + unsigned int length); + NDDSUSERDllExport extern DDS_ReturnCode_t + ConnextStaticRawDataPlugin_data_to_string( + const ConnextStaticRawData *sample, + char *str, + DDS_UnsignedLong *str_size, + const struct DDS_PrintFormatProperty *property); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_skip( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + RTIBool skip_encapsulation, + RTIBool skip_sample, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_min_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_sample_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment, + const ConnextStaticRawData * sample); + + /* -------------------------------------------------------------------------------------- + Key Management functions: + * -------------------------------------------------------------------------------------- */ + NDDSUSERDllExport extern PRESTypePluginKeyKind + ConnextStaticRawDataPlugin_get_key_kind(void); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( + PRESTypePluginEndpointData endpoint_data, + RTIBool * overflow, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern unsigned int + ConnextStaticRawDataPlugin_get_serialized_key_max_size( + PRESTypePluginEndpointData endpoint_data, + RTIBool include_encapsulation, + RTIEncapsulationId encapsulation_id, + unsigned int current_alignment); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialize_key( + PRESTypePluginEndpointData endpoint_data, + const ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool serialize_encapsulation, + RTIEncapsulationId encapsulation_id, + RTIBool serialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_key_sample( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData * sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_deserialize_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData ** sample, + RTIBool * drop_sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialized_sample_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *sample, + struct RTICdrStream *stream, + RTIBool deserialize_encapsulation, + RTIBool deserialize_key, + void *endpoint_plugin_qos); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_instance_to_key( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawDataKeyHolder *key, + const ConnextStaticRawData *instance); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_key_to_instance( + PRESTypePluginEndpointData endpoint_data, + ConnextStaticRawData *instance, + const ConnextStaticRawDataKeyHolder *key); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_instance_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + DDS_KeyHash_t *keyhash, + const ConnextStaticRawData *instance); + + NDDSUSERDllExport extern RTIBool + ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + PRESTypePluginEndpointData endpoint_data, + struct RTICdrStream *stream, + DDS_KeyHash_t *keyhash, + RTIBool deserialize_encapsulation, + void *endpoint_plugin_qos); + + /* Plugin Functions */ + NDDSUSERDllExport extern struct PRESTypePlugin* + ConnextStaticRawDataPlugin_new(void); + + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); + +} + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* connext_static_raw_dataPlugin_738563504_h */ + diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.cxx b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.cxx new file mode 100644 index 00000000..83be31bb --- /dev/null +++ b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.cxx @@ -0,0 +1,117 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#include "connext_static_raw_dataSupport.h" +#include "connext_static_raw_dataPlugin.h" + +#ifndef dds_c_log_impl_h +#include "dds_c/dds_c_log_impl.h" +#endif + +/* ========================================================================= */ +/** +<> + +Defines: TData, +TDataWriter, +TDataReader, +TTypeSupport + +Configure and implement 'ConnextStaticRawData' support classes. + +Note: Only the #defined classes get defined +*/ + +/* ----------------------------------------------------------------- */ +/* DDSDataWriter +*/ + +/** +<> + +Defines: TDataWriter, TData +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME + +/* Defines */ +#define TDataWriter ConnextStaticRawDataDataWriter +#define TData ConnextStaticRawData + +#include "dds_cpp/generic/dds_cpp_data_TDataWriter.gen" + +#undef TDataWriter +#undef TData + +#undef TTYPENAME + +/* ----------------------------------------------------------------- */ +/* DDSDataReader +*/ + +/** +<> + +Defines: TDataReader, TDataSeq, TData +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME + +/* Defines */ +#define TDataReader ConnextStaticRawDataDataReader +#define TDataSeq ConnextStaticRawDataSeq +#define TData ConnextStaticRawData + +#include "dds_cpp/generic/dds_cpp_data_TDataReader.gen" + +#undef TDataReader +#undef TDataSeq +#undef TData + +#undef TTYPENAME + +/* ----------------------------------------------------------------- */ +/* TypeSupport + +<> + +Requires: TTYPENAME, +TPlugin_new +TPlugin_delete +Defines: TTypeSupport, TData, TDataReader, TDataWriter +*/ + +/* Requires */ +#define TTYPENAME ConnextStaticRawDataTYPENAME +#define TPlugin_new ConnextStaticRawDataPlugin_new +#define TPlugin_delete ConnextStaticRawDataPlugin_delete + +/* Defines */ +#define TTypeSupport ConnextStaticRawDataTypeSupport +#define TData ConnextStaticRawData +#define TDataReader ConnextStaticRawDataDataReader +#define TDataWriter ConnextStaticRawDataDataWriter +#define TGENERATE_SER_CODE +#define TGENERATE_TYPECODE + +#include "dds_cpp/generic/dds_cpp_data_TTypeSupport.gen" + +#undef TTypeSupport +#undef TData +#undef TDataReader +#undef TDataWriter +#undef TGENERATE_TYPECODE +#undef TGENERATE_SER_CODE +#undef TTYPENAME +#undef TPlugin_new +#undef TPlugin_delete + diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.h b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.h new file mode 100644 index 00000000..012a1f61 --- /dev/null +++ b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.h @@ -0,0 +1,62 @@ + +/* +WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. + +This file was generated from connext_static_raw_data.idl using "rtiddsgen". +The rtiddsgen tool is part of the RTI Connext distribution. +For more information, type 'rtiddsgen -help' at a command shell +or consult the RTI Connext manual. +*/ + +#ifndef connext_static_raw_dataSupport_738563504_h +#define connext_static_raw_dataSupport_738563504_h + +/* Uses */ +#include "connext_static_raw_data.h" + +#ifndef ndds_cpp_h +#include "ndds/ndds_cpp.h" +#endif + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) + +class __declspec(dllimport) DDSTypeSupport; +class __declspec(dllimport) DDSDataWriter; +class __declspec(dllimport) DDSDataReader; + +#endif + +/* ========================================================================= */ +/** +Uses: T + +Defines: TTypeSupport, TDataWriter, TDataReader + +Organized using the well-documented "Generics Pattern" for +implementing generics in C and C++. +*/ + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, start exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport __declspec(dllexport) + +#endif + +DDS_TYPESUPPORT_CPP( + ConnextStaticRawDataTypeSupport, + ConnextStaticRawData); + +DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); +DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); + +#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) +/* If the code is building on Windows, stop exporting symbols. +*/ +#undef NDDSUSERDllExport +#define NDDSUSERDllExport +#endif + +#endif /* connext_static_raw_dataSupport_738563504_h */ + From 8b66d9c898952f3f7eb08671261201e354d71121 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 5 Mar 2018 16:35:17 -0800 Subject: [PATCH 17/34] patch for functional differences to generated code --- .../connext_static_raw_dataPlugin.cxx | 295 ++++++++---------- .../connext_static_raw_dataPlugin.h | 3 + .../connext_static_raw_dataSupport.cxx | 59 ++++ .../connext_static_raw_dataSupport.h | 99 +++++- 4 files changed, 288 insertions(+), 168 deletions(-) diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx index 78b58d54..831d83ff 100644 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx @@ -379,74 +379,81 @@ ConnextStaticRawDataPlugin_serialize( RTIBool serialize_sample, void *endpoint_plugin_qos) { - char * position = NULL; - RTIBool retval = RTI_TRUE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(serialize_encapsulation) { - if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { - return RTI_FALSE; - } + char * position = NULL; + RTIBool retval = RTI_TRUE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + /* This plugin can only be used to publish the top-level DDS Topic-Type + * in which case serialize_encapsulation==TRUE. If that is not + * the case then it is an error. + */ + if (!serialize_encapsulation) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + + if (serialize_sample) { + /* The sample->serialized_data contains the serialized encapsulation followed by the serialized + * data, so we only need to copy that into + * the CDR stream. Not the key_hash, not the length of the data itself + * The SerializedType sample->serialized_data is always a contiguous buffer + */ + DDS_Octet * buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); + if (buffer == NULL) { + return RTI_FALSE; + } - position = RTICdrStream_resetAlignment(stream); + /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian + * byte order + */ + if (encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { + return RTI_FALSE; } - if(serialize_sample) { + /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ + if (!RTICdrStream_serializePrimitiveArray( + stream, (void *)buffer, + DDS_OctetSeq_get_length(&sample->serialized_data), + RTI_CDR_OCTET_TYPE)) + { + return RTI_FALSE; + } + } - if (!RTICdrStream_serializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } + RTICdrStream_restoreAlignment(stream, position); - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - if (!RTICdrStream_serializePrimitiveSequence( - stream, - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - (RTI_INT32_MAX-1), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } else { - if (!RTICdrStream_serializePrimitivePointerSequence( - stream, - (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - (RTI_INT32_MAX-1), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - if (!RTICdrStream_serializePrimitiveSequence( - stream, - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - (RTI_INT32_MAX-1), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } else { - if (!RTICdrStream_serializePrimitivePointerSequence( - stream, - (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - (RTI_INT32_MAX-1), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } + return retval; +} - } +/** + TODO. The code-block below does not belong here. + It should be pushed to the CDR module, perhaps inside + RTICdrStream_deserializeAndSetCdrEncapsulation so that the + stream size is alredy correct when SerializedTypePlugin_deserialize_sample + is called. - if(serialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } + Adjust the size of the CDR stream to not include the alignment + padding. See http://issues.omg.org/browse/DDSXTY12-10 - return retval; + @precondition The RTICdrStream *stream has alreadt processed + the encapsulation header and therefore has set the + encapsulation options returned by + RTICdrStream_getEncapsulationOptions() +*/ +void +ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) +{ + /* See http://issues.omg.org/browse/DDSXTY12-10 */ + DDS_UnsignedShort padding_size_mask = 0x0003; + DDS_UnsignedShort padding_size; + int adjustedBufferLength; + + padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; + adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; + RTICdrStream_setBufferLength(stream, adjustedBufferLength); } RTIBool @@ -458,114 +465,65 @@ ConnextStaticRawDataPlugin_deserialize_sample( RTIBool deserialize_sample, void *endpoint_plugin_qos) { + char * position = NULL; + RTIBool done = RTI_FALSE; + + if (endpoint_data) {} /* To avoid warnings */ + if (endpoint_plugin_qos) {} /* To avoid warnings */ + + /* This plugin can only be used to publish the top-level DDS Topic-Type + * in which case deserialize_encapsulation==TRUE. If that is not + * the case then it is an error. + */ + if (!deserialize_encapsulation) { + return RTI_FALSE; + } + + position = RTICdrStream_resetAlignment(stream); + + /* TODO. The call does not belong here. It should be pushed + * inside RTICdrStream_deserializeAndSetCdrEncapsulation + */ + ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); + + if (deserialize_sample) { + /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() + it is done there because SerializedTypePlugin_deserialize_sample does not + have access to the SampleInfo where that information is + */ + + /* We do not set the serialized_key on deserialization */ + DDS_OctetSeq_set_length(&sample->serialized_key, 0); + + /* We copy everything that remains in the CDR stream */ + int bytesLeftInStream = RTICdrStream_getRemainder(stream); + DDS_Octet * cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); + if (cdrBufferPtr == NULL) { + goto fin; + } - char * position = NULL; - - RTIBool done = RTI_FALSE; - - try { - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - if(deserialize_encapsulation) { - - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - if(deserialize_sample) { - - ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - goto fin; - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { - goto fin; - } - if (!DDS_OctetSeq_set_maximum(&sample->serialized_key,sequence_length)) { - return RTI_FALSE; - } - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - if (!RTICdrStream_deserializePrimitiveSequence( - stream, - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - &sequence_length, - DDS_OctetSeq_get_maximum(&sample->serialized_key), - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } else { - if (!RTICdrStream_deserializePrimitivePointerSequence( - stream, - (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - &sequence_length, - DDS_OctetSeq_get_maximum(&sample->serialized_key), - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - if (!DDS_OctetSeq_set_length(&sample->serialized_key, sequence_length)) { - return RTI_FALSE; - } - - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { - goto fin; - } - if (!DDS_OctetSeq_set_maximum(&sample->serialized_data,sequence_length)) { - return RTI_FALSE; - } - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - if (!RTICdrStream_deserializePrimitiveSequence( - stream, - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - &sequence_length, - DDS_OctetSeq_get_maximum(&sample->serialized_data), - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } else { - if (!RTICdrStream_deserializePrimitivePointerSequence( - stream, - (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - &sequence_length, - DDS_OctetSeq_get_maximum(&sample->serialized_data), - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - if (!DDS_OctetSeq_set_length(&sample->serialized_data, sequence_length)) { - return RTI_FALSE; - } + /* Do not call SerializedType_initialize_ex initialize here + because it would override the key_hash field + SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); + */ + if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { + goto fin; + } + RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); + } - } - } + done = RTI_TRUE; - done = RTI_TRUE; - fin: - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } +fin: + if ( (done != RTI_TRUE) && + (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) + { + return RTI_FALSE; + } - return RTI_TRUE; + RTICdrStream_restoreAlignment(stream, position); - } catch (std::bad_alloc&) { - return RTI_FALSE; - } + return RTI_TRUE; } RTIBool @@ -971,7 +929,9 @@ Key Management functions: PRESTypePluginKeyKind ConnextStaticRawDataPlugin_get_key_kind(void) { - return PRES_TYPEPLUGIN_USER_KEY; + // TODO(karsten1987): Whenever we introduce keys for our data types + // this might have to change. + return PRES_TYPEPLUGIN_NO_KEY; } RTIBool @@ -1408,6 +1368,11 @@ ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( * ------------------------------------------------------------------------ */ struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) { + return NULL; +} + +struct PRESTypePlugin *ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) +{ struct PRESTypePlugin *plugin = NULL; const struct PRESTypePluginVersion PLUGIN_VERSION = PRES_TYPE_PLUGIN_VERSION_2_0; @@ -1503,7 +1468,7 @@ struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) (PRESTypePluginKeyToInstanceFunction) ConnextStaticRawDataPlugin_key_to_instance; plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ - plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); + plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h index 89b4c67b..bd23b77b 100644 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h @@ -324,6 +324,9 @@ extern "C" { NDDSUSERDllExport extern struct PRESTypePlugin* ConnextStaticRawDataPlugin_new(void); + NDDS_USER_DLL_EXPORT extern struct PRESTypePlugin* + ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); + NDDSUSERDllExport extern void ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx index 83be31bb..f2c9c633 100644 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx @@ -115,3 +115,62 @@ Defines: TTypeSupport, TData, TDataReader, TDataWriter #undef TPlugin_new #undef TPlugin_delete +DDS_ReturnCode_t +ConnextStaticRawDataSupport_register_external_type( + DDSDomainParticipant * participant, + const char * type_name, + struct DDS_TypeCode * type_code) +{ + DDSTypeSupport * dds_data_type = NULL; + struct PRESTypePlugin * presTypePlugin = NULL; + DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; + DDS_Boolean delete_data_type = DDS_BOOLEAN_FALSE; + RTIBool already_registered = RTI_FALSE; + + if (type_code == NULL) { + goto finError; + } + + if (participant == NULL) { + goto finError; + } + + /* TODO pass the type_code */ + presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); + if (presTypePlugin == NULL) { + goto finError; + } + + dds_data_type = new ConnextStaticRawDataTypeSupport(true); + if (dds_data_type == NULL) { + fprintf(stderr, "Error while registering external type\n"); + goto finError; + } + delete_data_type = RTI_TRUE; + + presTypePlugin->_userBuffer = (PRESWord *)dds_data_type; + already_registered = participant->is_type_registered(type_name); + + retcode = participant->register_type(type_name, presTypePlugin, NULL, !already_registered); + if (retcode != DDS_RETCODE_OK) { + fprintf(stderr, "error while registering external type\n"); + goto finError; + } + + if (!already_registered) { + delete_data_type = DDS_BOOLEAN_FALSE; + } + + retcode = DDS_RETCODE_OK; + +finError: + if (presTypePlugin != NULL) { + ConnextStaticRawDataPlugin_delete(presTypePlugin); + } + if (delete_data_type) { + delete (ConnextStaticRawDataTypeSupport *)dds_data_type; + dds_data_type = NULL; + } + + return retcode; +} diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h index 012a1f61..51db529a 100644 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h @@ -44,13 +44,106 @@ implementing generics in C and C++. #endif -DDS_TYPESUPPORT_CPP( - ConnextStaticRawDataTypeSupport, - ConnextStaticRawData); +class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport +{ +public: + ConnextStaticRawDataTypeSupport(bool osrf) + { + (void) osrf; + } + + ~ConnextStaticRawDataTypeSupport(); + + static DDS_ReturnCode_t register_type( + DDSDomainParticipant * participant, + const char * type_name = "ConnextStaticRawData"); + + static DDS_ReturnCode_t unregister_type( + DDSDomainParticipant * participant, + const char * type_name = "ConnextStaticRawData"); + + static const char * get_type_name(); + + static ConnextStaticRawData * create_data_ex(DDS_Boolean allocatePointers); + + static ConnextStaticRawData * create_data( + const DDS_TypeAllocationParams_t & alloc_params = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + + static DDS_ReturnCode_t delete_data_ex( + ConnextStaticRawData * a_data, + DDS_Boolean deletePointers); + + static DDS_ReturnCode_t delete_data( + ConnextStaticRawData * a_data, + const DDS_TypeDeallocationParams_t & dealloc_params = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + + static void print_data(const ConnextStaticRawData * a_data); + + static DDS_ReturnCode_t copy_data( + ConnextStaticRawData * dst_data, const ConnextStaticRawData * src_data); + + static DDS_ReturnCode_t initialize_data_ex( + ConnextStaticRawData * a_data, + DDS_Boolean allocatePointers); + + static DDS_ReturnCode_t initialize_data( + ConnextStaticRawData * a_data, + const DDS_TypeAllocationParams_t & alloc_params = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + + static DDS_ReturnCode_t finalize_data_ex( + ConnextStaticRawData * a_data, + DDS_Boolean deletePointers); + + static DDS_ReturnCode_t finalize_data( + ConnextStaticRawData * a_data, + const DDS_TypeDeallocationParams_t & dealloc_params = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + + DDSDataReader * create_datareaderI(DDSDataReader * dataReader); + + DDS_ReturnCode_t destroy_datareaderI(DDSDataReader * dataReader); + + DDSDataWriter * create_datawriterI(DDSDataWriter * dataWriter); + + DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter * dataWriter); + + static DDS_TypeCode * get_typecode(); + + static DDS_ReturnCode_t serialize_data_to_cdr_buffer( + char * buffer, + unsigned int & length, + const ConnextStaticRawData * a_data); + + static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( + ConnextStaticRawData * a_data, + const char * buffer, + unsigned int length); + + static DDS_ReturnCode_t data_to_string( + ConnextStaticRawData * sample, + char * str, + DDS_UnsignedLong & str_size, + const DDS_PrintFormatProperty & property); + + static void finalize(); + +private: + ConnextStaticRawDataTypeSupport(); +}; DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); +NDDSUSERDllExport +DDS_ReturnCode_t +ConnextStaticRawDataSupport_register_external_type( + DDSDomainParticipant * participant, + const char * type_name, + struct DDS_TypeCode * type_code); + #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) /* If the code is building on Windows, stop exporting symbols. */ From 4b90edc87dbeec6291a3cf670890093b754f9c98 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 6 Mar 2018 09:12:22 -0800 Subject: [PATCH 18/34] linters again --- rmw_connext_cpp/src/rmw_take.cpp | 4 +-- .../ndds_include.h | 31 ------------------- .../connext_static_cdr_stream.hpp | 2 -- .../message_type_support.h | 6 ---- 4 files changed, 2 insertions(+), 41 deletions(-) delete mode 100644 rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/ndds_include.h diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index 7a2d02bf..44c79414 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -101,7 +101,8 @@ take( if (!ignore_sample) { cdr_stream->message_length = dds_messages[0].serialized_data.length(); // TODO(karsten1987): This malloc has to go! - cdr_stream->raw_message = (char *) malloc(cdr_stream->message_length * sizeof(char)); + cdr_stream->raw_message = + reinterpret_cast(malloc(cdr_stream->message_length * sizeof(char))); for (unsigned int i = 0; i < cdr_stream->message_length; ++i) { cdr_stream->raw_message[i] = dds_messages[0].serialized_data[i]; } @@ -117,7 +118,6 @@ take( extern "C" { - rmw_ret_t _take( const rmw_subscription_t * subscription, diff --git a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/ndds_include.h b/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/ndds_include.h deleted file mode 100644 index e6892070..00000000 --- a/rosidl_typesupport_connext_c/include/rosidl_typesupport_connext_c/ndds_include.h +++ /dev/null @@ -1,31 +0,0 @@ -// Copyright 2015-2017 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_H_ -#define ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_H_ - -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wunused-parameter" -# ifdef __clang__ -# pragma clang diagnostic ignored "-Wpedantic" -# endif -#endif -#include "ndds/ndds_c.h" -//#include "ndds/ndds_requestreply_cpp.h" -#ifndef _WIN32 -# pragma GCC diagnostic pop -#endif - -#endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__NDDS_INCLUDE_H_ diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp index 69bb2319..255dacbf 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp @@ -17,13 +17,11 @@ extern "C" { - typedef struct ConnextStaticCDRStream { char * raw_message = nullptr; unsigned int message_length = 0; } ConnextStaticCDRStream; - } // extern "C" #endif // ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_CDR_STREAM_HPP_ diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h index 3f43c148..94fee623 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/message_type_support.h @@ -28,12 +28,6 @@ typedef struct message_type_support_callbacks_t const char * message_name; // Function to register type with given dds_participant DDS_TypeCode * (*get_type_code)(void); - // Function to publish a ROS message with a given DDS data_writer - //bool (* publish)(void * dds_data_writer, ConnextStaticCDRStream * cdr_stream); - // Function to take a ROS message from a dds data reader - //bool (* take)( - // void * dds_data_reader, bool ignore_local_publications, ConnextStaticCDRStream * cdr_stream, bool * taken, - // void * sending_publication_handle); bool (* convert_ros_to_dds)( const void * untyped_ros_message, void * untyped_data_message); From a0601f98679e48bb1e6d36d5686ddd3f5c4abe6c Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 6 Mar 2018 09:12:39 -0800 Subject: [PATCH 19/34] [style] changes to generated code again --- .../src/connext_static_raw_data.cpp | 29 ++++++---------- .../src/connext_static_raw_data_plugin.cpp | 34 +------------------ 2 files changed, 12 insertions(+), 51 deletions(-) diff --git a/rmw_connext_cpp/src/connext_static_raw_data.cpp b/rmw_connext_cpp/src/connext_static_raw_data.cpp index 53daf7aa..51013d2b 100644 --- a/rmw_connext_cpp/src/connext_static_raw_data.cpp +++ b/rmw_connext_cpp/src/connext_static_raw_data.cpp @@ -42,7 +42,7 @@ DDS_TypeCode * ConnextStaticRawData_get_typecode() static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3] = { { - (char *)"key_hash", /* Member name */ // NOLINT + (char *)"key_hash", /* Member name */ // NOLINT { 0, /* Representation ID */ DDS_BOOLEAN_FALSE, /* Is a pointer? */ @@ -59,7 +59,7 @@ DDS_TypeCode * ConnextStaticRawData_get_typecode() NULL /* Ignored */ }, { - (char *)"serialized_key", /* Member name */ // NOLINT + (char *)"serialized_key", /* Member name */ // NOLINT { 1, /* Representation ID */ DDS_BOOLEAN_FALSE, /* Is a pointer? */ @@ -76,7 +76,7 @@ DDS_TypeCode * ConnextStaticRawData_get_typecode() NULL /* Ignored */ }, { - (char *)"serialized_data", /* Member name */ // NOLINT + (char *)"serialized_data", /* Member name */ // NOLINT { 2, /* Representation ID */ DDS_BOOLEAN_FALSE, /* Is a pointer? */ @@ -99,7 +99,7 @@ DDS_TypeCode * ConnextStaticRawData_get_typecode() DDS_TK_STRUCT, /* Kind */ DDS_BOOLEAN_FALSE, /* Ignored */ -1, /*Ignored*/ - (char *)"ConnextStaticRawData", /* Name */ // NOLINT + (char *)"ConnextStaticRawData", /* Name */ // NOLINT NULL, /* Ignored */ 0, /* Ignored */ 0, /* Ignored */ @@ -113,20 +113,21 @@ DDS_TypeCode * ConnextStaticRawData_get_typecode() return &ConnextStaticRawData_g_tc; } - ConnextStaticRawData_g_tc_key_hash_array._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; + ConnextStaticRawData_g_tc_key_hash_array._data._typeCode = + (RTICdrTypeCode *)&DDS_g_tc_octet; // NOLINT ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = - (RTICdrTypeCode *)&DDS_g_tc_octet; + (RTICdrTypeCode *)&DDS_g_tc_octet; // NOLINT ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = - (RTICdrTypeCode *)&DDS_g_tc_octet; + (RTICdrTypeCode *)&DDS_g_tc_octet; // NOLINT ConnextStaticRawData_g_tc_members[0]._representation._typeCode = - (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_key_hash_array; + (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_key_hash_array; // NOLINT ConnextStaticRawData_g_tc_members[1]._representation._typeCode = - (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_serialized_key_sequence; + (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_serialized_key_sequence; // NOLINT ConnextStaticRawData_g_tc_members[2]._representation._typeCode = - (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_serialized_data_sequence; + (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_serialized_data_sequence; // NOLINT is_initialized = RTI_TRUE; @@ -142,7 +143,6 @@ RTIBool ConnextStaticRawData_initialize( RTIBool ConnextStaticRawData_initialize_ex( ConnextStaticRawData * sample, RTIBool allocatePointers, RTIBool allocateMemory) { - struct DDS_TypeAllocationParams_t allocParams = DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; @@ -151,13 +151,11 @@ RTIBool ConnextStaticRawData_initialize_ex( return ConnextStaticRawData_initialize_w_params( sample, &allocParams); - } RTIBool ConnextStaticRawData_initialize_w_params( ConnextStaticRawData * sample, const struct DDS_TypeAllocationParams_t * allocParams) { - void * buffer = NULL; if (buffer) {} /* To avoid warnings */ @@ -197,7 +195,6 @@ RTIBool ConnextStaticRawData_initialize_w_params( void ConnextStaticRawData_finalize( ConnextStaticRawData * sample) { - ConnextStaticRawData_finalize_ex(sample, RTI_TRUE); } @@ -220,7 +217,6 @@ void ConnextStaticRawData_finalize_ex( void ConnextStaticRawData_finalize_w_params( ConnextStaticRawData * sample, const struct DDS_TypeDeallocationParams_t * deallocParams) { - if (sample == NULL) { return; } @@ -232,7 +228,6 @@ void ConnextStaticRawData_finalize_w_params( DDS_OctetSeq_finalize(&sample->serialized_key); DDS_OctetSeq_finalize(&sample->serialized_data); - } void ConnextStaticRawData_finalize_optional_members( @@ -250,7 +245,6 @@ void ConnextStaticRawData_finalize_optional_members( deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; - } RTIBool ConnextStaticRawData_copy( @@ -258,7 +252,6 @@ RTIBool ConnextStaticRawData_copy( const ConnextStaticRawData * src) { try { - if (dst == NULL || src == NULL) { return RTI_FALSE; } diff --git a/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp index dc720eee..9d8f2e8d 100644 --- a/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp +++ b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp @@ -110,7 +110,6 @@ ConnextStaticRawDataPluginSupport_destroy_data_w_params( ConnextStaticRawData * sample, const struct DDS_TypeDeallocationParams_t * dealloc_params) { - ConnextStaticRawData_finalize_w_params(sample, dealloc_params); delete sample; @@ -121,7 +120,6 @@ void ConnextStaticRawDataPluginSupport_destroy_data_ex( ConnextStaticRawData * sample, RTIBool deallocate_pointers) { - ConnextStaticRawData_finalize_ex(sample, deallocate_pointers); delete sample; @@ -132,9 +130,7 @@ void ConnextStaticRawDataPluginSupport_destroy_data( ConnextStaticRawData * sample) { - ConnextStaticRawDataPluginSupport_destroy_data_ex(sample, RTI_TRUE); - } RTIBool @@ -151,7 +147,6 @@ ConnextStaticRawDataPluginSupport_print_data( const char * desc, unsigned int indent_level) { - RTICdrType_printIndent(indent_level); if (desc != NULL) { @@ -199,8 +194,8 @@ ConnextStaticRawDataPluginSupport_print_data( (RTICdrTypePrintFunction)RTICdrType_printOctet, "serialized_data", indent_level + 1); } - } + ConnextStaticRawData * ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers) { @@ -227,16 +222,13 @@ ConnextStaticRawDataPluginSupport_destroy_key_ex( delete key; key = NULL; - } void ConnextStaticRawDataPluginSupport_destroy_key( ConnextStaticRawDataKeyHolder * key) { - ConnextStaticRawDataPluginSupport_destroy_key_ex(key, RTI_TRUE); - } /* ---------------------------------------------------------------------------- @@ -258,14 +250,12 @@ ConnextStaticRawDataPlugin_on_participant_attached( if (type_code) {} /* To avoid warnings */ return PRESTypePluginDefaultParticipantData_new(participant_info); - } void ConnextStaticRawDataPlugin_on_participant_detached( PRESTypePluginParticipantData participant_data) { - PRESTypePluginDefaultParticipantData_delete(participant_data); } @@ -337,7 +327,6 @@ void ConnextStaticRawDataPlugin_on_endpoint_detached( PRESTypePluginEndpointData endpoint_data) { - PRESTypePluginDefaultEndpointData_delete(endpoint_data); } @@ -347,7 +336,6 @@ ConnextStaticRawDataPlugin_return_sample( ConnextStaticRawData * sample, void * handle) { - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); PRESTypePluginDefaultEndpointData_returnSample( @@ -692,7 +680,6 @@ ConnextStaticRawDataPlugin_deserialize( RTIBool deserialize_sample, void * endpoint_plugin_qos) { - RTIBool result; const char * METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; if (drop_sample) {} /* To avoid warnings */ @@ -708,12 +695,10 @@ ConnextStaticRawDataPlugin_deserialize( } } if (!result && stream->_xTypesState.unassignable) { - RTICdrLog_exception( METHOD_NAME, &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, "ConnextStaticRawData"); - } return result; @@ -742,7 +727,6 @@ RTIBool ConnextStaticRawDataPlugin_skip( } if (skip_sample) { - if (!RTICdrStream_skipPrimitiveArray( stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { @@ -793,7 +777,6 @@ ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( RTIEncapsulationId encapsulation_id, unsigned int current_alignment) { - if (endpoint_data) {} /* To avoid warnings */ if (include_encapsulation) {} if (encapsulation_id) {} @@ -804,7 +787,6 @@ ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( } return RTI_CDR_MAX_SERIALIZED_SIZE; - } unsigned int @@ -834,7 +816,6 @@ ConnextStaticRawDataPlugin_get_serialized_sample_min_size( RTIEncapsulationId encapsulation_id, unsigned int current_alignment) { - unsigned int initial_alignment = current_alignment; unsigned int encapsulation_size = current_alignment; @@ -842,7 +823,6 @@ ConnextStaticRawDataPlugin_get_serialized_sample_min_size( if (endpoint_data) {} /* To avoid warnings */ if (include_encapsulation) { - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { return 1; } @@ -879,7 +859,6 @@ ConnextStaticRawDataPlugin_get_serialized_sample_size( unsigned int current_alignment, const ConnextStaticRawData * sample) { - unsigned int initial_alignment = current_alignment; unsigned int encapsulation_size = current_alignment; @@ -896,7 +875,6 @@ ConnextStaticRawDataPlugin_get_serialized_sample_size( } if (include_encapsulation) { - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { return 1; } @@ -966,13 +944,11 @@ ConnextStaticRawDataPlugin_serialize_key( } if (serialize_key) { - if (!RTICdrStream_serializePrimitiveArray( stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { return RTI_FALSE; } - } if (serialize_encapsulation) { @@ -991,7 +967,6 @@ RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( void * endpoint_plugin_qos) { try { - char * position = NULL; if (endpoint_data) {} /* To avoid warnings */ @@ -1006,13 +981,11 @@ RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( position = RTICdrStream_resetAlignment(stream); } if (deserialize_key) { - if (!RTICdrStream_deserializePrimitiveArray( stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { return RTI_FALSE; } - } if (deserialize_encapsulation) { @@ -1048,7 +1021,6 @@ RTIBool ConnextStaticRawDataPlugin_deserialize_key( } return result; - } unsigned int @@ -1059,7 +1031,6 @@ ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( RTIEncapsulationId encapsulation_id, unsigned int current_alignment) { - unsigned int initial_alignment = current_alignment; unsigned int encapsulation_size = current_alignment; @@ -1136,7 +1107,6 @@ ConnextStaticRawDataPlugin_serialized_sample_to_key( } if (deserialize_key) { - if (!RTICdrStream_deserializePrimitiveArray( stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { @@ -1193,7 +1163,6 @@ ConnextStaticRawDataPlugin_instance_to_key( ConnextStaticRawDataKeyHolder * dst, const ConnextStaticRawData * src) { - if (endpoint_data) {} /* To avoid warnings */ if (!RTICdrType_copyArray( @@ -1210,7 +1179,6 @@ ConnextStaticRawDataPlugin_key_to_instance( ConnextStaticRawData * dst, const ConnextStaticRawDataKeyHolder * src) { - if (endpoint_data) {} /* To avoid warnings */ if (!RTICdrType_copyArray( dst->key_hash, src->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) From 3265fb6ca27e96fcfdbe24bc1f620cc476c49f41 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 20 Mar 2018 12:29:08 -0700 Subject: [PATCH 20/34] add patch files --- ...tional-differences-to-generated-code.patch | 557 ++++++++++++++++++ .../connext_static_raw_dataPlugin.cxx.patch | 344 +++++++++++ .../connext_static_raw_dataPlugin.h.patch | 11 + .../connext_static_raw_dataSupport.cxx.patch | 66 +++ .../connext_static_raw_dataSupport.h.patch | 113 ++++ 5 files changed, 1091 insertions(+) create mode 100644 rmw_connext_cpp/resources/patch_files/0001-patch-for-functional-differences-to-generated-code.patch create mode 100644 rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch create mode 100644 rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch create mode 100644 rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.cxx.patch create mode 100644 rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch diff --git a/rmw_connext_cpp/resources/patch_files/0001-patch-for-functional-differences-to-generated-code.patch b/rmw_connext_cpp/resources/patch_files/0001-patch-for-functional-differences-to-generated-code.patch new file mode 100644 index 00000000..aff956fe --- /dev/null +++ b/rmw_connext_cpp/resources/patch_files/0001-patch-for-functional-differences-to-generated-code.patch @@ -0,0 +1,557 @@ +From 75284eee62dd86075a781cade020926cf3518c95 Mon Sep 17 00:00:00 2001 +From: Karsten Knese +Date: Mon, 5 Mar 2018 16:35:17 -0800 +Subject: [PATCH] patch for functional differences to generated code + +--- + .../connext_static_raw_dataPlugin.cxx | 295 +++++++++------------ + .../connext_static_raw_dataPlugin.h | 3 + + .../connext_static_raw_dataSupport.cxx | 59 +++++ + .../connext_static_raw_dataSupport.h | 99 ++++++- + 4 files changed, 288 insertions(+), 168 deletions(-) + +diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +index 78b58d5..831d83f 100644 +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +@@ -379,74 +379,81 @@ ConnextStaticRawDataPlugin_serialize( + RTIBool serialize_sample, + void *endpoint_plugin_qos) + { +- char * position = NULL; +- RTIBool retval = RTI_TRUE; +- +- if (endpoint_data) {} /* To avoid warnings */ +- if (endpoint_plugin_qos) {} /* To avoid warnings */ +- +- if(serialize_encapsulation) { +- if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { +- return RTI_FALSE; +- } ++ char * position = NULL; ++ RTIBool retval = RTI_TRUE; ++ ++ if (endpoint_data) {} /* To avoid warnings */ ++ if (endpoint_plugin_qos) {} /* To avoid warnings */ ++ ++ /* This plugin can only be used to publish the top-level DDS Topic-Type ++ * in which case serialize_encapsulation==TRUE. If that is not ++ * the case then it is an error. ++ */ ++ if (!serialize_encapsulation) { ++ return RTI_FALSE; ++ } ++ ++ position = RTICdrStream_resetAlignment(stream); ++ ++ if (serialize_sample) { ++ /* The sample->serialized_data contains the serialized encapsulation followed by the serialized ++ * data, so we only need to copy that into ++ * the CDR stream. Not the key_hash, not the length of the data itself ++ * The SerializedType sample->serialized_data is always a contiguous buffer ++ */ ++ DDS_Octet * buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); ++ if (buffer == NULL) { ++ return RTI_FALSE; ++ } + +- position = RTICdrStream_resetAlignment(stream); ++ /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian ++ * byte order ++ */ ++ if (encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { ++ return RTI_FALSE; + } + +- if(serialize_sample) { ++ /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ ++ if (!RTICdrStream_serializePrimitiveArray( ++ stream, (void *)buffer, ++ DDS_OctetSeq_get_length(&sample->serialized_data), ++ RTI_CDR_OCTET_TYPE)) ++ { ++ return RTI_FALSE; ++ } ++ } + +- if (!RTICdrStream_serializePrimitiveArray( +- stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } ++ RTICdrStream_restoreAlignment(stream, position); + +- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { +- if (!RTICdrStream_serializePrimitiveSequence( +- stream, +- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), +- DDS_OctetSeq_get_length(&sample->serialized_key), +- (RTI_INT32_MAX-1), +- RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } +- } else { +- if (!RTICdrStream_serializePrimitivePointerSequence( +- stream, +- (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), +- DDS_OctetSeq_get_length(&sample->serialized_key), +- (RTI_INT32_MAX-1), +- RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } +- } +- +- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { +- if (!RTICdrStream_serializePrimitiveSequence( +- stream, +- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), +- DDS_OctetSeq_get_length(&sample->serialized_data), +- (RTI_INT32_MAX-1), +- RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } +- } else { +- if (!RTICdrStream_serializePrimitivePointerSequence( +- stream, +- (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), +- DDS_OctetSeq_get_length(&sample->serialized_data), +- (RTI_INT32_MAX-1), +- RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } +- } ++ return retval; ++} + +- } ++/** ++ TODO. The code-block below does not belong here. ++ It should be pushed to the CDR module, perhaps inside ++ RTICdrStream_deserializeAndSetCdrEncapsulation so that the ++ stream size is alredy correct when SerializedTypePlugin_deserialize_sample ++ is called. + +- if(serialize_encapsulation) { +- RTICdrStream_restoreAlignment(stream,position); +- } ++ Adjust the size of the CDR stream to not include the alignment ++ padding. See http://issues.omg.org/browse/DDSXTY12-10 + +- return retval; ++ @precondition The RTICdrStream *stream has alreadt processed ++ the encapsulation header and therefore has set the ++ encapsulation options returned by ++ RTICdrStream_getEncapsulationOptions() ++*/ ++void ++ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) ++{ ++ /* See http://issues.omg.org/browse/DDSXTY12-10 */ ++ DDS_UnsignedShort padding_size_mask = 0x0003; ++ DDS_UnsignedShort padding_size; ++ int adjustedBufferLength; ++ ++ padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; ++ adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; ++ RTICdrStream_setBufferLength(stream, adjustedBufferLength); + } + + RTIBool +@@ -458,114 +465,65 @@ ConnextStaticRawDataPlugin_deserialize_sample( + RTIBool deserialize_sample, + void *endpoint_plugin_qos) + { ++ char * position = NULL; ++ RTIBool done = RTI_FALSE; ++ ++ if (endpoint_data) {} /* To avoid warnings */ ++ if (endpoint_plugin_qos) {} /* To avoid warnings */ ++ ++ /* This plugin can only be used to publish the top-level DDS Topic-Type ++ * in which case deserialize_encapsulation==TRUE. If that is not ++ * the case then it is an error. ++ */ ++ if (!deserialize_encapsulation) { ++ return RTI_FALSE; ++ } ++ ++ position = RTICdrStream_resetAlignment(stream); ++ ++ /* TODO. The call does not belong here. It should be pushed ++ * inside RTICdrStream_deserializeAndSetCdrEncapsulation ++ */ ++ ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); ++ ++ if (deserialize_sample) { ++ /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() ++ it is done there because SerializedTypePlugin_deserialize_sample does not ++ have access to the SampleInfo where that information is ++ */ ++ ++ /* We do not set the serialized_key on deserialization */ ++ DDS_OctetSeq_set_length(&sample->serialized_key, 0); ++ ++ /* We copy everything that remains in the CDR stream */ ++ int bytesLeftInStream = RTICdrStream_getRemainder(stream); ++ DDS_Octet * cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); ++ if (cdrBufferPtr == NULL) { ++ goto fin; ++ } + +- char * position = NULL; +- +- RTIBool done = RTI_FALSE; +- +- try { +- +- if (endpoint_data) {} /* To avoid warnings */ +- if (endpoint_plugin_qos) {} /* To avoid warnings */ +- if(deserialize_encapsulation) { +- +- if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { +- return RTI_FALSE; +- } +- +- position = RTICdrStream_resetAlignment(stream); +- } +- if(deserialize_sample) { +- +- ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); +- +- if (!RTICdrStream_deserializePrimitiveArray( +- stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { +- goto fin; +- } +- +- { +- RTICdrUnsignedLong sequence_length; +- if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { +- goto fin; +- } +- if (!DDS_OctetSeq_set_maximum(&sample->serialized_key,sequence_length)) { +- return RTI_FALSE; +- } +- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { +- if (!RTICdrStream_deserializePrimitiveSequence( +- stream, +- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), +- &sequence_length, +- DDS_OctetSeq_get_maximum(&sample->serialized_key), +- RTI_CDR_OCTET_TYPE)){ +- goto fin; +- } +- } else { +- if (!RTICdrStream_deserializePrimitivePointerSequence( +- stream, +- (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), +- &sequence_length, +- DDS_OctetSeq_get_maximum(&sample->serialized_key), +- RTI_CDR_OCTET_TYPE)){ +- goto fin; +- } +- } +- if (!DDS_OctetSeq_set_length(&sample->serialized_key, sequence_length)) { +- return RTI_FALSE; +- } +- +- } +- { +- RTICdrUnsignedLong sequence_length; +- if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { +- goto fin; +- } +- if (!DDS_OctetSeq_set_maximum(&sample->serialized_data,sequence_length)) { +- return RTI_FALSE; +- } +- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { +- if (!RTICdrStream_deserializePrimitiveSequence( +- stream, +- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), +- &sequence_length, +- DDS_OctetSeq_get_maximum(&sample->serialized_data), +- RTI_CDR_OCTET_TYPE)){ +- goto fin; +- } +- } else { +- if (!RTICdrStream_deserializePrimitivePointerSequence( +- stream, +- (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), +- &sequence_length, +- DDS_OctetSeq_get_maximum(&sample->serialized_data), +- RTI_CDR_OCTET_TYPE)){ +- goto fin; +- } +- } +- if (!DDS_OctetSeq_set_length(&sample->serialized_data, sequence_length)) { +- return RTI_FALSE; +- } ++ /* Do not call SerializedType_initialize_ex initialize here ++ because it would override the key_hash field ++ SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); ++ */ ++ if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { ++ goto fin; ++ } ++ RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); ++ } + +- } +- } ++ done = RTI_TRUE; + +- done = RTI_TRUE; +- fin: +- if (done != RTI_TRUE && +- RTICdrStream_getRemainder(stream) >= +- RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { +- return RTI_FALSE; +- } +- if(deserialize_encapsulation) { +- RTICdrStream_restoreAlignment(stream,position); +- } ++fin: ++ if ( (done != RTI_TRUE) && ++ (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) ++ { ++ return RTI_FALSE; ++ } + +- return RTI_TRUE; ++ RTICdrStream_restoreAlignment(stream, position); + +- } catch (std::bad_alloc&) { +- return RTI_FALSE; +- } ++ return RTI_TRUE; + } + + RTIBool +@@ -971,7 +929,9 @@ Key Management functions: + PRESTypePluginKeyKind + ConnextStaticRawDataPlugin_get_key_kind(void) + { +- return PRES_TYPEPLUGIN_USER_KEY; ++ // TODO(karsten1987): Whenever we introduce keys for our data types ++ // this might have to change. ++ return PRES_TYPEPLUGIN_NO_KEY; + } + + RTIBool +@@ -1408,6 +1368,11 @@ ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + * ------------------------------------------------------------------------ */ + struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) + { ++ return NULL; ++} ++ ++struct PRESTypePlugin *ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) ++{ + struct PRESTypePlugin *plugin = NULL; + const struct PRESTypePluginVersion PLUGIN_VERSION = + PRES_TYPE_PLUGIN_VERSION_2_0; +@@ -1503,7 +1468,7 @@ struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) + (PRESTypePluginKeyToInstanceFunction) + ConnextStaticRawDataPlugin_key_to_instance; + plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ +- plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); ++ plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; + + plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; + +diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h +index 89b4c67..bd23b77 100644 +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h +@@ -324,6 +324,9 @@ extern "C" { + NDDSUSERDllExport extern struct PRESTypePlugin* + ConnextStaticRawDataPlugin_new(void); + ++ NDDS_USER_DLL_EXPORT extern struct PRESTypePlugin* ++ ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); ++ + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); + +diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx +index 83be31b..f2c9c63 100644 +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx +@@ -115,3 +115,62 @@ Defines: TTypeSupport, TData, TDataReader, TDataWriter + #undef TPlugin_new + #undef TPlugin_delete + ++DDS_ReturnCode_t ++ConnextStaticRawDataSupport_register_external_type( ++ DDSDomainParticipant * participant, ++ const char * type_name, ++ struct DDS_TypeCode * type_code) ++{ ++ DDSTypeSupport * dds_data_type = NULL; ++ struct PRESTypePlugin * presTypePlugin = NULL; ++ DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; ++ DDS_Boolean delete_data_type = DDS_BOOLEAN_FALSE; ++ RTIBool already_registered = RTI_FALSE; ++ ++ if (type_code == NULL) { ++ goto finError; ++ } ++ ++ if (participant == NULL) { ++ goto finError; ++ } ++ ++ /* TODO pass the type_code */ ++ presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); ++ if (presTypePlugin == NULL) { ++ goto finError; ++ } ++ ++ dds_data_type = new ConnextStaticRawDataTypeSupport(true); ++ if (dds_data_type == NULL) { ++ fprintf(stderr, "Error while registering external type\n"); ++ goto finError; ++ } ++ delete_data_type = RTI_TRUE; ++ ++ presTypePlugin->_userBuffer = (PRESWord *)dds_data_type; ++ already_registered = participant->is_type_registered(type_name); ++ ++ retcode = participant->register_type(type_name, presTypePlugin, NULL, !already_registered); ++ if (retcode != DDS_RETCODE_OK) { ++ fprintf(stderr, "error while registering external type\n"); ++ goto finError; ++ } ++ ++ if (!already_registered) { ++ delete_data_type = DDS_BOOLEAN_FALSE; ++ } ++ ++ retcode = DDS_RETCODE_OK; ++ ++finError: ++ if (presTypePlugin != NULL) { ++ ConnextStaticRawDataPlugin_delete(presTypePlugin); ++ } ++ if (delete_data_type) { ++ delete (ConnextStaticRawDataTypeSupport *)dds_data_type; ++ dds_data_type = NULL; ++ } ++ ++ return retcode; ++} +diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h +index 012a1f6..51db529 100644 +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h +@@ -44,13 +44,106 @@ implementing generics in C and C++. + + #endif + +-DDS_TYPESUPPORT_CPP( +- ConnextStaticRawDataTypeSupport, +- ConnextStaticRawData); ++class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport ++{ ++public: ++ ConnextStaticRawDataTypeSupport(bool osrf) ++ { ++ (void) osrf; ++ } ++ ++ ~ConnextStaticRawDataTypeSupport(); ++ ++ static DDS_ReturnCode_t register_type( ++ DDSDomainParticipant * participant, ++ const char * type_name = "ConnextStaticRawData"); ++ ++ static DDS_ReturnCode_t unregister_type( ++ DDSDomainParticipant * participant, ++ const char * type_name = "ConnextStaticRawData"); ++ ++ static const char * get_type_name(); ++ ++ static ConnextStaticRawData * create_data_ex(DDS_Boolean allocatePointers); ++ ++ static ConnextStaticRawData * create_data( ++ const DDS_TypeAllocationParams_t & alloc_params = ++ DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); ++ ++ static DDS_ReturnCode_t delete_data_ex( ++ ConnextStaticRawData * a_data, ++ DDS_Boolean deletePointers); ++ ++ static DDS_ReturnCode_t delete_data( ++ ConnextStaticRawData * a_data, ++ const DDS_TypeDeallocationParams_t & dealloc_params = ++ DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); ++ ++ static void print_data(const ConnextStaticRawData * a_data); ++ ++ static DDS_ReturnCode_t copy_data( ++ ConnextStaticRawData * dst_data, const ConnextStaticRawData * src_data); ++ ++ static DDS_ReturnCode_t initialize_data_ex( ++ ConnextStaticRawData * a_data, ++ DDS_Boolean allocatePointers); ++ ++ static DDS_ReturnCode_t initialize_data( ++ ConnextStaticRawData * a_data, ++ const DDS_TypeAllocationParams_t & alloc_params = ++ DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); ++ ++ static DDS_ReturnCode_t finalize_data_ex( ++ ConnextStaticRawData * a_data, ++ DDS_Boolean deletePointers); ++ ++ static DDS_ReturnCode_t finalize_data( ++ ConnextStaticRawData * a_data, ++ const DDS_TypeDeallocationParams_t & dealloc_params = ++ DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); ++ ++ DDSDataReader * create_datareaderI(DDSDataReader * dataReader); ++ ++ DDS_ReturnCode_t destroy_datareaderI(DDSDataReader * dataReader); ++ ++ DDSDataWriter * create_datawriterI(DDSDataWriter * dataWriter); ++ ++ DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter * dataWriter); ++ ++ static DDS_TypeCode * get_typecode(); ++ ++ static DDS_ReturnCode_t serialize_data_to_cdr_buffer( ++ char * buffer, ++ unsigned int & length, ++ const ConnextStaticRawData * a_data); ++ ++ static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( ++ ConnextStaticRawData * a_data, ++ const char * buffer, ++ unsigned int length); ++ ++ static DDS_ReturnCode_t data_to_string( ++ ConnextStaticRawData * sample, ++ char * str, ++ DDS_UnsignedLong & str_size, ++ const DDS_PrintFormatProperty & property); ++ ++ static void finalize(); ++ ++private: ++ ConnextStaticRawDataTypeSupport(); ++}; + + DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); + DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); + ++NDDSUSERDllExport ++DDS_ReturnCode_t ++ConnextStaticRawDataSupport_register_external_type( ++ DDSDomainParticipant * participant, ++ const char * type_name, ++ struct DDS_TypeCode * type_code); ++ + #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) + /* If the code is building on Windows, stop exporting symbols. + */ +-- +2.12.0 + diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch new file mode 100644 index 00000000..b7b30451 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch @@ -0,0 +1,344 @@ +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +@@ -379,74 +379,81 @@ ConnextStaticRawDataPlugin_serialize( + RTIBool serialize_sample, + void *endpoint_plugin_qos) + { +- char * position = NULL; +- RTIBool retval = RTI_TRUE; +- +- if (endpoint_data) {} /* To avoid warnings */ +- if (endpoint_plugin_qos) {} /* To avoid warnings */ +- +- if(serialize_encapsulation) { +- if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { +- return RTI_FALSE; +- } ++ char * position = NULL; ++ RTIBool retval = RTI_TRUE; ++ ++ if (endpoint_data) {} /* To avoid warnings */ ++ if (endpoint_plugin_qos) {} /* To avoid warnings */ ++ ++ /* This plugin can only be used to publish the top-level DDS Topic-Type ++ * in which case serialize_encapsulation==TRUE. If that is not ++ * the case then it is an error. ++ */ ++ if (!serialize_encapsulation) { ++ return RTI_FALSE; ++ } ++ ++ position = RTICdrStream_resetAlignment(stream); ++ ++ if (serialize_sample) { ++ /* The sample->serialized_data contains the serialized encapsulation followed by the serialized ++ * data, so we only need to copy that into ++ * the CDR stream. Not the key_hash, not the length of the data itself ++ * The SerializedType sample->serialized_data is always a contiguous buffer ++ */ ++ DDS_Octet * buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); ++ if (buffer == NULL) { ++ return RTI_FALSE; ++ } + +- position = RTICdrStream_resetAlignment(stream); ++ /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian ++ * byte order ++ */ ++ if (encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { ++ return RTI_FALSE; + } + +- if(serialize_sample) { ++ /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ ++ if (!RTICdrStream_serializePrimitiveArray( ++ stream, (void *)buffer, ++ DDS_OctetSeq_get_length(&sample->serialized_data), ++ RTI_CDR_OCTET_TYPE)) ++ { ++ return RTI_FALSE; ++ } ++ } + +- if (!RTICdrStream_serializePrimitiveArray( +- stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } ++ RTICdrStream_restoreAlignment(stream, position); + +- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { +- if (!RTICdrStream_serializePrimitiveSequence( +- stream, +- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), +- DDS_OctetSeq_get_length(&sample->serialized_key), +- (RTI_INT32_MAX-1), +- RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } +- } else { +- if (!RTICdrStream_serializePrimitivePointerSequence( +- stream, +- (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), +- DDS_OctetSeq_get_length(&sample->serialized_key), +- (RTI_INT32_MAX-1), +- RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } +- } +- +- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { +- if (!RTICdrStream_serializePrimitiveSequence( +- stream, +- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), +- DDS_OctetSeq_get_length(&sample->serialized_data), +- (RTI_INT32_MAX-1), +- RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } +- } else { +- if (!RTICdrStream_serializePrimitivePointerSequence( +- stream, +- (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), +- DDS_OctetSeq_get_length(&sample->serialized_data), +- (RTI_INT32_MAX-1), +- RTI_CDR_OCTET_TYPE)) { +- return RTI_FALSE; +- } +- } ++ return retval; ++} + +- } ++/** ++ TODO. The code-block below does not belong here. ++ It should be pushed to the CDR module, perhaps inside ++ RTICdrStream_deserializeAndSetCdrEncapsulation so that the ++ stream size is alredy correct when SerializedTypePlugin_deserialize_sample ++ is called. + +- if(serialize_encapsulation) { +- RTICdrStream_restoreAlignment(stream,position); +- } ++ Adjust the size of the CDR stream to not include the alignment ++ padding. See http://issues.omg.org/browse/DDSXTY12-10 + +- return retval; ++ @precondition The RTICdrStream *stream has alreadt processed ++ the encapsulation header and therefore has set the ++ encapsulation options returned by ++ RTICdrStream_getEncapsulationOptions() ++*/ ++void ++ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) ++{ ++ /* See http://issues.omg.org/browse/DDSXTY12-10 */ ++ DDS_UnsignedShort padding_size_mask = 0x0003; ++ DDS_UnsignedShort padding_size; ++ int adjustedBufferLength; ++ ++ padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; ++ adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; ++ RTICdrStream_setBufferLength(stream, adjustedBufferLength); + } + + RTIBool +@@ -458,114 +465,65 @@ ConnextStaticRawDataPlugin_deserialize_sample( + RTIBool deserialize_sample, + void *endpoint_plugin_qos) + { ++ char * position = NULL; ++ RTIBool done = RTI_FALSE; ++ ++ if (endpoint_data) {} /* To avoid warnings */ ++ if (endpoint_plugin_qos) {} /* To avoid warnings */ ++ ++ /* This plugin can only be used to publish the top-level DDS Topic-Type ++ * in which case deserialize_encapsulation==TRUE. If that is not ++ * the case then it is an error. ++ */ ++ if (!deserialize_encapsulation) { ++ return RTI_FALSE; ++ } ++ ++ position = RTICdrStream_resetAlignment(stream); ++ ++ /* TODO. The call does not belong here. It should be pushed ++ * inside RTICdrStream_deserializeAndSetCdrEncapsulation ++ */ ++ ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); ++ ++ if (deserialize_sample) { ++ /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() ++ it is done there because SerializedTypePlugin_deserialize_sample does not ++ have access to the SampleInfo where that information is ++ */ ++ ++ /* We do not set the serialized_key on deserialization */ ++ DDS_OctetSeq_set_length(&sample->serialized_key, 0); ++ ++ /* We copy everything that remains in the CDR stream */ ++ int bytesLeftInStream = RTICdrStream_getRemainder(stream); ++ DDS_Octet * cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); ++ if (cdrBufferPtr == NULL) { ++ goto fin; ++ } + +- char * position = NULL; +- +- RTIBool done = RTI_FALSE; +- +- try { +- +- if (endpoint_data) {} /* To avoid warnings */ +- if (endpoint_plugin_qos) {} /* To avoid warnings */ +- if(deserialize_encapsulation) { +- +- if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { +- return RTI_FALSE; +- } +- +- position = RTICdrStream_resetAlignment(stream); +- } +- if(deserialize_sample) { +- +- ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); +- +- if (!RTICdrStream_deserializePrimitiveArray( +- stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { +- goto fin; +- } +- +- { +- RTICdrUnsignedLong sequence_length; +- if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { +- goto fin; +- } +- if (!DDS_OctetSeq_set_maximum(&sample->serialized_key,sequence_length)) { +- return RTI_FALSE; +- } +- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { +- if (!RTICdrStream_deserializePrimitiveSequence( +- stream, +- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), +- &sequence_length, +- DDS_OctetSeq_get_maximum(&sample->serialized_key), +- RTI_CDR_OCTET_TYPE)){ +- goto fin; +- } +- } else { +- if (!RTICdrStream_deserializePrimitivePointerSequence( +- stream, +- (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), +- &sequence_length, +- DDS_OctetSeq_get_maximum(&sample->serialized_key), +- RTI_CDR_OCTET_TYPE)){ +- goto fin; +- } +- } +- if (!DDS_OctetSeq_set_length(&sample->serialized_key, sequence_length)) { +- return RTI_FALSE; +- } +- +- } +- { +- RTICdrUnsignedLong sequence_length; +- if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { +- goto fin; +- } +- if (!DDS_OctetSeq_set_maximum(&sample->serialized_data,sequence_length)) { +- return RTI_FALSE; +- } +- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { +- if (!RTICdrStream_deserializePrimitiveSequence( +- stream, +- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), +- &sequence_length, +- DDS_OctetSeq_get_maximum(&sample->serialized_data), +- RTI_CDR_OCTET_TYPE)){ +- goto fin; +- } +- } else { +- if (!RTICdrStream_deserializePrimitivePointerSequence( +- stream, +- (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), +- &sequence_length, +- DDS_OctetSeq_get_maximum(&sample->serialized_data), +- RTI_CDR_OCTET_TYPE)){ +- goto fin; +- } +- } +- if (!DDS_OctetSeq_set_length(&sample->serialized_data, sequence_length)) { +- return RTI_FALSE; +- } ++ /* Do not call SerializedType_initialize_ex initialize here ++ because it would override the key_hash field ++ SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); ++ */ ++ if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { ++ goto fin; ++ } ++ RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); ++ } + +- } +- } ++ done = RTI_TRUE; + +- done = RTI_TRUE; +- fin: +- if (done != RTI_TRUE && +- RTICdrStream_getRemainder(stream) >= +- RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { +- return RTI_FALSE; +- } +- if(deserialize_encapsulation) { +- RTICdrStream_restoreAlignment(stream,position); +- } ++fin: ++ if ( (done != RTI_TRUE) && ++ (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) ++ { ++ return RTI_FALSE; ++ } + +- return RTI_TRUE; ++ RTICdrStream_restoreAlignment(stream, position); + +- } catch (std::bad_alloc&) { +- return RTI_FALSE; +- } ++ return RTI_TRUE; + } + + RTIBool +@@ -971,7 +929,9 @@ Key Management functions: + PRESTypePluginKeyKind + ConnextStaticRawDataPlugin_get_key_kind(void) + { +- return PRES_TYPEPLUGIN_USER_KEY; ++ // TODO(karsten1987): Whenever we introduce keys for our data types ++ // this might have to change. ++ return PRES_TYPEPLUGIN_NO_KEY; + } + + RTIBool +@@ -1408,6 +1368,11 @@ ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( + * ------------------------------------------------------------------------ */ + struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) + { ++ return NULL; ++} ++ ++struct PRESTypePlugin *ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) ++{ + struct PRESTypePlugin *plugin = NULL; + const struct PRESTypePluginVersion PLUGIN_VERSION = + PRES_TYPE_PLUGIN_VERSION_2_0; +@@ -1503,7 +1468,7 @@ struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) + (PRESTypePluginKeyToInstanceFunction) + ConnextStaticRawDataPlugin_key_to_instance; + plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ +- plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); ++ plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; + + plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch new file mode 100644 index 00000000..811ff6d8 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch @@ -0,0 +1,11 @@ +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h +@@ -324,6 +324,9 @@ extern "C" { + NDDSUSERDllExport extern struct PRESTypePlugin* + ConnextStaticRawDataPlugin_new(void); + ++ NDDS_USER_DLL_EXPORT extern struct PRESTypePlugin* ++ ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); ++ + NDDSUSERDllExport extern void + ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.cxx.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.cxx.patch new file mode 100644 index 00000000..7c3a3922 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.cxx.patch @@ -0,0 +1,66 @@ +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx +@@ -115,3 +115,62 @@ Defines: TTypeSupport, TData, TDataReader, TDataWriter + #undef TPlugin_new + #undef TPlugin_delete + ++DDS_ReturnCode_t ++ConnextStaticRawDataSupport_register_external_type( ++ DDSDomainParticipant * participant, ++ const char * type_name, ++ struct DDS_TypeCode * type_code) ++{ ++ DDSTypeSupport * dds_data_type = NULL; ++ struct PRESTypePlugin * presTypePlugin = NULL; ++ DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; ++ DDS_Boolean delete_data_type = DDS_BOOLEAN_FALSE; ++ RTIBool already_registered = RTI_FALSE; ++ ++ if (type_code == NULL) { ++ goto finError; ++ } ++ ++ if (participant == NULL) { ++ goto finError; ++ } ++ ++ /* TODO pass the type_code */ ++ presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); ++ if (presTypePlugin == NULL) { ++ goto finError; ++ } ++ ++ dds_data_type = new ConnextStaticRawDataTypeSupport(true); ++ if (dds_data_type == NULL) { ++ fprintf(stderr, "Error while registering external type\n"); ++ goto finError; ++ } ++ delete_data_type = RTI_TRUE; ++ ++ presTypePlugin->_userBuffer = (PRESWord *)dds_data_type; ++ already_registered = participant->is_type_registered(type_name); ++ ++ retcode = participant->register_type(type_name, presTypePlugin, NULL, !already_registered); ++ if (retcode != DDS_RETCODE_OK) { ++ fprintf(stderr, "error while registering external type\n"); ++ goto finError; ++ } ++ ++ if (!already_registered) { ++ delete_data_type = DDS_BOOLEAN_FALSE; ++ } ++ ++ retcode = DDS_RETCODE_OK; ++ ++finError: ++ if (presTypePlugin != NULL) { ++ ConnextStaticRawDataPlugin_delete(presTypePlugin); ++ } ++ if (delete_data_type) { ++ delete (ConnextStaticRawDataTypeSupport *)dds_data_type; ++ dds_data_type = NULL; ++ } ++ ++ return retcode; ++} + diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch new file mode 100644 index 00000000..92057623 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch @@ -0,0 +1,113 @@ +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h +@@ -44,13 +44,106 @@ implementing generics in C and C++. + + #endif + +-DDS_TYPESUPPORT_CPP( +- ConnextStaticRawDataTypeSupport, +- ConnextStaticRawData); ++class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport ++{ ++public: ++ ConnextStaticRawDataTypeSupport(bool osrf) ++ { ++ (void) osrf; ++ } ++ ++ ~ConnextStaticRawDataTypeSupport(); ++ ++ static DDS_ReturnCode_t register_type( ++ DDSDomainParticipant * participant, ++ const char * type_name = "ConnextStaticRawData"); ++ ++ static DDS_ReturnCode_t unregister_type( ++ DDSDomainParticipant * participant, ++ const char * type_name = "ConnextStaticRawData"); ++ ++ static const char * get_type_name(); ++ ++ static ConnextStaticRawData * create_data_ex(DDS_Boolean allocatePointers); ++ ++ static ConnextStaticRawData * create_data( ++ const DDS_TypeAllocationParams_t & alloc_params = ++ DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); ++ ++ static DDS_ReturnCode_t delete_data_ex( ++ ConnextStaticRawData * a_data, ++ DDS_Boolean deletePointers); ++ ++ static DDS_ReturnCode_t delete_data( ++ ConnextStaticRawData * a_data, ++ const DDS_TypeDeallocationParams_t & dealloc_params = ++ DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); ++ ++ static void print_data(const ConnextStaticRawData * a_data); ++ ++ static DDS_ReturnCode_t copy_data( ++ ConnextStaticRawData * dst_data, const ConnextStaticRawData * src_data); ++ ++ static DDS_ReturnCode_t initialize_data_ex( ++ ConnextStaticRawData * a_data, ++ DDS_Boolean allocatePointers); ++ ++ static DDS_ReturnCode_t initialize_data( ++ ConnextStaticRawData * a_data, ++ const DDS_TypeAllocationParams_t & alloc_params = ++ DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); ++ ++ static DDS_ReturnCode_t finalize_data_ex( ++ ConnextStaticRawData * a_data, ++ DDS_Boolean deletePointers); ++ ++ static DDS_ReturnCode_t finalize_data( ++ ConnextStaticRawData * a_data, ++ const DDS_TypeDeallocationParams_t & dealloc_params = ++ DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); ++ ++ DDSDataReader * create_datareaderI(DDSDataReader * dataReader); ++ ++ DDS_ReturnCode_t destroy_datareaderI(DDSDataReader * dataReader); ++ ++ DDSDataWriter * create_datawriterI(DDSDataWriter * dataWriter); ++ ++ DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter * dataWriter); ++ ++ static DDS_TypeCode * get_typecode(); ++ ++ static DDS_ReturnCode_t serialize_data_to_cdr_buffer( ++ char * buffer, ++ unsigned int & length, ++ const ConnextStaticRawData * a_data); ++ ++ static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( ++ ConnextStaticRawData * a_data, ++ const char * buffer, ++ unsigned int length); ++ ++ static DDS_ReturnCode_t data_to_string( ++ ConnextStaticRawData * sample, ++ char * str, ++ DDS_UnsignedLong & str_size, ++ const DDS_PrintFormatProperty & property); ++ ++ static void finalize(); ++ ++private: ++ ConnextStaticRawDataTypeSupport(); ++}; + + DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); + DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); + ++NDDSUSERDllExport ++DDS_ReturnCode_t ++ConnextStaticRawDataSupport_register_external_type( ++ DDSDomainParticipant * participant, ++ const char * type_name, ++ struct DDS_TypeCode * type_code); ++ + #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) + /* If the code is building on Windows, stop exporting symbols. + */ + From 53b99d8bbc39d1170299be0110ce9210d6172f95 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Tue, 20 Mar 2018 14:37:51 -0700 Subject: [PATCH 21/34] stuff from our hackathon --- rmw_connext_cpp/CMakeLists.txt | 54 ++++++++++++++++- rmw_connext_cpp/bin/apply-patch.py | 59 +++++++++++++++++++ .../connext_static_raw_data.cxx.patch | 2 + .../connext_static_raw_data.h.patch | 2 + 4 files changed, 114 insertions(+), 3 deletions(-) create mode 100644 rmw_connext_cpp/bin/apply-patch.py create mode 100644 rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch create mode 100644 rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch diff --git a/rmw_connext_cpp/CMakeLists.txt b/rmw_connext_cpp/CMakeLists.txt index 4085a81f..647e3ca9 100644 --- a/rmw_connext_cpp/CMakeLists.txt +++ b/rmw_connext_cpp/CMakeLists.txt @@ -66,12 +66,60 @@ register_rmw_implementation( "cpp:rosidl_typesupport_cpp:rosidl_typesupport_connext_cpp" ) +# generate code for raw data +set(_idl_pp "${Connext_DDSGEN}") +if(NOT "${Connext_DDSGEN_SERVER}" STREQUAL "") + # use the code generator in server mode when available + # because it speeds up the code generation step significantly + set(_idl_pp "${Connext_DDSGEN_SERVER}") +endif() +set(generated_files + ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_data.cxx + ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_data.h + ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_dataPlugin.cxx + ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_dataPlugin.h + ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_dataSupport.cxx + ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_dataSupport.h +) +file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/resources/generated") +add_custom_command( + OUTPUT ${generated_files} + COMMAND "${_idl_pp}" -language C++ -unboundedSupport "connext_static_raw_data.idl" -d ${CMAKE_CURRENT_BINARY_DIR}/resources/generated + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/resources" + COMMENT "Generating raw type support for RTI Connext (using '${_idl_pp}')" + VERBATIM +) + +# patch the generate code for raw data +set(patch_files + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_data.cxx.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_data.h.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataPlugin.h.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataSupport.cxx.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataSupport.h.patch +) +set(patched_files + ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_data.cxx + ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_data.h + ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_dataPlugin.cxx + ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_dataPlugin.h + ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_dataSupport.cxx + ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_dataSupport.h +) +file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/resources/patched") +add_custom_command( + OUTPUT ${patched_files} + COMMAND ${PYTHON_EXECUTABLE} "${CMAKE_SOURCE_DIR}/bin/apply-patch.py" --input ${generated_files} --patch ${patch_files} --out ${patched_files} + DEPENDS ${generated_files} + COMMENT "Patching raw type support for RTI Connext" + VERBATIM +) + add_library( rmw_connext_cpp SHARED - src/connext_static_raw_data.cpp - src/connext_static_raw_data_plugin.cpp - src/connext_static_raw_data_support.cpp + ${patched_files} src/get_client.cpp src/get_participant.cpp src/get_publisher.cpp diff --git a/rmw_connext_cpp/bin/apply-patch.py b/rmw_connext_cpp/bin/apply-patch.py new file mode 100644 index 00000000..7ae12150 --- /dev/null +++ b/rmw_connext_cpp/bin/apply-patch.py @@ -0,0 +1,59 @@ +import argparse +import re + +_hdr_pat = re.compile("^@@ -(\d+),?(\d+)? \+(\d+),?(\d+)? @@.*$") + + +def apply_patch(s, patch, revert=False): + """ + Apply unified diff patch to string s to recover newer string. + + If revert is True, treat s as the newer string, recover older string. + """ + s = s.splitlines(True) + p = patch.splitlines(True) + t = '' + i = sl = 0 + (midx, sign) = (1, '+') if not revert else (3, '-') + while i < len(p) and p[i].startswith(('---', '+++')): + i += 1 # skip header lines + while i < len(p): + m = _hdr_pat.match(p[i]) + if not m: + raise Exception('Cannot process diff in line ' + str(i)) + i += 1 + l = int(m.group(midx)) - 1 + (m.group(midx + 1) == '0') + t += ''.join(s[sl:l]) + sl = l + while i < len(p) and p[i][0] != '@': + if i + 1 < len(p) and p[i + 1][0] == '\\': + line = p[i][:-1] + i += 2 + else: + line = p[i] + i += 1 + if len(line) > 0: + if line[0] == sign or line[0] == ' ': + t += line[1:] + sl += (line[0] != sign) + t += ''.join(s[sl:]) + return t + + +parser = argparse.ArgumentParser() +parser.add_argument('--input', nargs='+') +parser.add_argument('--patch', nargs='+') +parser.add_argument('--out', nargs='+') +args = parser.parse_args() +for i, p, o in zip(args.input, args.patch, args.out): + with open(i, 'r') as h: + content_in = h.read() + with open(p, 'r') as h: + content_patch = h.read() + try: + content_out = apply_patch(content_in, content_patch) + except Exception: + print(i, p, o) + raise + with open(o, 'w') as h: + h.write(content_out) diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch new file mode 100644 index 00000000..2f1bd933 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch @@ -0,0 +1,2 @@ +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch new file mode 100644 index 00000000..dfef5b6c --- /dev/null +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch @@ -0,0 +1,2 @@ +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h From 90e422702ef64af693bbbeb5b25b620ed9a6f23a Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 20 Mar 2018 16:53:43 -0700 Subject: [PATCH 22/34] use patched version of generated files --- rmw_connext_cpp/CMakeLists.txt | 55 +++++++------------ .../connext_static_raw_data.cxx.patch | 13 ++++- .../connext_static_raw_data.h.patch | 13 ++++- .../connext_static_raw_dataPlugin.cxx.patch | 9 +++ .../connext_static_raw_dataPlugin.h.patch | 2 +- .../connext_static_raw_dataSupport.h.patch | 10 +++- rmw_connext_cpp/src/rmw_publish.cpp | 4 +- rmw_connext_cpp/src/rmw_publisher.cpp | 4 +- rmw_connext_cpp/src/rmw_subscription.cpp | 4 +- rmw_connext_cpp/src/rmw_take.cpp | 6 +- 10 files changed, 74 insertions(+), 46 deletions(-) diff --git a/rmw_connext_cpp/CMakeLists.txt b/rmw_connext_cpp/CMakeLists.txt index 647e3ca9..52591cd9 100644 --- a/rmw_connext_cpp/CMakeLists.txt +++ b/rmw_connext_cpp/CMakeLists.txt @@ -73,18 +73,19 @@ if(NOT "${Connext_DDSGEN_SERVER}" STREQUAL "") # because it speeds up the code generation step significantly set(_idl_pp "${Connext_DDSGEN_SERVER}") endif() +set(generated_directory "${CMAKE_CURRENT_BINARY_DIR}/resources/generated") +file(MAKE_DIRECTORY ${generated_directory}) set(generated_files - ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_data.cxx - ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_data.h - ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_dataPlugin.cxx - ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_dataPlugin.h - ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_dataSupport.cxx - ${CMAKE_CURRENT_BINARY_DIR}/resources/generated/connext_static_raw_dataSupport.h + ${generated_directory}/connext_static_raw_data.cxx + ${generated_directory}/connext_static_raw_data.h + ${generated_directory}/connext_static_raw_dataPlugin.cxx + ${generated_directory}/connext_static_raw_dataPlugin.h + ${generated_directory}/connext_static_raw_dataSupport.cxx + ${generated_directory}/connext_static_raw_dataSupport.h ) -file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/resources/generated") add_custom_command( OUTPUT ${generated_files} - COMMAND "${_idl_pp}" -language C++ -unboundedSupport "connext_static_raw_data.idl" -d ${CMAKE_CURRENT_BINARY_DIR}/resources/generated + COMMAND "${_idl_pp}" -language C++ -unboundedSupport "connext_static_raw_data.idl" -d ${generated_directory} WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/resources" COMMENT "Generating raw type support for RTI Connext (using '${_idl_pp}')" VERBATIM @@ -99,15 +100,16 @@ set(patch_files ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataSupport.cxx.patch ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataSupport.h.patch ) +set(patched_directory "${CMAKE_CURRENT_BINARY_DIR}/resources/patched") +file(MAKE_DIRECTORY ${patched_directory}) set(patched_files - ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_data.cxx - ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_data.h - ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_dataPlugin.cxx - ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_dataPlugin.h - ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_dataSupport.cxx - ${CMAKE_CURRENT_BINARY_DIR}/resources/patched/connext_static_raw_dataSupport.h + ${patched_directory}/connext_static_raw_data.cxx + ${patched_directory}/connext_static_raw_data.h + ${patched_directory}/connext_static_raw_dataPlugin.cxx + ${patched_directory}/connext_static_raw_dataPlugin.h + ${patched_directory}/connext_static_raw_dataSupport.cxx + ${patched_directory}/connext_static_raw_dataSupport.h ) -file(MAKE_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/resources/patched") add_custom_command( OUTPUT ${patched_files} COMMAND ${PYTHON_EXECUTABLE} "${CMAKE_SOURCE_DIR}/bin/apply-patch.py" --input ${generated_files} --patch ${patch_files} --out ${patched_files} @@ -159,6 +161,7 @@ ament_target_dependencies(rmw_connext_cpp "rosidl_typesupport_connext_c" "rosidl_typesupport_connext_cpp" "Connext") +target_include_directories(rmw_connext_cpp PUBLIC ${patched_directory}) ament_export_libraries(rmw_connext_cpp) # Causes the visibility macros to use dllexport rather than dllimport @@ -176,26 +179,8 @@ if(WIN32) endif() if(BUILD_TESTING) - find_package(ament_cmake_cppcheck REQUIRED) - find_package(ament_cmake_cpplint REQUIRED) - find_package(ament_cmake_lint_cmake REQUIRED) - find_package(ament_cmake_uncrustify REQUIRED) - find_package(ament_cmake_copyright REQUIRED) - ament_cppcheck() - ament_cpplint("FILTERS" "-legal/copyright") - ament_lint_cmake() - ament_uncrustify() - ament_copyright( - "--exclude" - "connext_static_raw_data.hpp" - "connext_static_raw_data_plugin.hpp" - "connext_static_raw_data_support.hpp" - "connext_static_raw_data.cpp" - "connext_static_raw_data_plugin.cpp" - "connext_static_raw_data_support.cpp" - ) - - # ament_lint_auto_find_test_dependencies() + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() endif() ament_package(CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake") diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch index 2f1bd933..03a07378 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch @@ -1,2 +1,11 @@ ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx +@@ -11,7 +11,7 @@ or consult the RTI Connext manual. + + #ifndef NDDS_STANDALONE_TYPE + #ifndef ndds_cpp_h +-#include "ndds/ndds_cpp.h" ++#include "rmw_connext_shared_cpp/ndds_include.hpp" + #endif + #ifndef dds_c_log_impl_h + #include "dds_c/dds_c_log_impl.h" diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch index dfef5b6c..babcf2da 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch @@ -1,2 +1,11 @@ ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h +@@ -14,7 +14,7 @@ or consult the RTI Connext manual. + + #ifndef NDDS_STANDALONE_TYPE + #ifndef ndds_cpp_h +-#include "ndds/ndds_cpp.h" ++#include "rmw_connext_shared_cpp/ndds_include.hpp" + #endif + #else + #include "ndds_standalone_type.h" diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch index b7b30451..f5634eea 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch @@ -1,5 +1,14 @@ --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +@@ -11,7 +11,7 @@ or consult the RTI Connext manual. + #include + + #ifndef ndds_cpp_h +-#include "ndds/ndds_cpp.h" ++#include "rmw_connext_shared_cpp/ndds_include.hpp" + #endif + + #ifndef osapi_type_h @@ -379,74 +379,81 @@ ConnextStaticRawDataPlugin_serialize( RTIBool serialize_sample, void *endpoint_plugin_qos) diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch index 811ff6d8..059a86ce 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch @@ -4,7 +4,7 @@ NDDSUSERDllExport extern struct PRESTypePlugin* ConnextStaticRawDataPlugin_new(void); -+ NDDS_USER_DLL_EXPORT extern struct PRESTypePlugin* ++ NDDSUSERDllExport extern struct PRESTypePlugin* + ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); + NDDSUSERDllExport extern void diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch index 92057623..e519078d 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch @@ -1,5 +1,14 @@ --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h +++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h +@@ -15,7 +15,7 @@ or consult the RTI Connext manual. + #include "connext_static_raw_data.h" + + #ifndef ndds_cpp_h +-#include "ndds/ndds_cpp.h" ++#include "rmw_connext_shared_cpp/ndds_include.hpp" + #endif + + #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) @@ -44,13 +44,106 @@ implementing generics in C and C++. #endif @@ -110,4 +119,3 @@ #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) /* If the code is building on Windows, stop exporting symbols. */ - diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 6bad22d1..2eca3c32 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -16,12 +16,14 @@ #include "rmw/rmw.h" #include "rmw/types.h" -#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" #include "rmw_connext_cpp/connext_static_publisher_info.hpp" #include "rmw_connext_cpp/identifier.hpp" #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" +// include patched generated code from the build folder +#include "connext_static_raw_dataSupport.h" + bool publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) { diff --git a/rmw_connext_cpp/src/rmw_publisher.cpp b/rmw_connext_cpp/src/rmw_publisher.cpp index cee7cf69..111c9b8d 100644 --- a/rmw_connext_cpp/src/rmw_publisher.cpp +++ b/rmw_connext_cpp/src/rmw_publisher.cpp @@ -23,13 +23,15 @@ #include "rmw_connext_shared_cpp/qos.hpp" #include "rmw_connext_shared_cpp/types.hpp" -#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" #include "rmw_connext_cpp/identifier.hpp" #include "process_topic_and_service_names.hpp" #include "type_support_common.hpp" #include "rmw_connext_cpp/connext_static_publisher_info.hpp" +// include patched generated code from the build folder +#include "connext_static_raw_dataSupport.h" + // Uncomment this to get extra console output about discovery. // This affects code in this file, but there is a similar variable in: // rmw_connext_shared_cpp/shared_functions.cpp diff --git a/rmw_connext_cpp/src/rmw_subscription.cpp b/rmw_connext_cpp/src/rmw_subscription.cpp index fac35287..227aefbb 100644 --- a/rmw_connext_cpp/src/rmw_subscription.cpp +++ b/rmw_connext_cpp/src/rmw_subscription.cpp @@ -22,13 +22,15 @@ #include "rmw_connext_shared_cpp/qos.hpp" #include "rmw_connext_shared_cpp/types.hpp" -#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" #include "rmw_connext_cpp/identifier.hpp" #include "process_topic_and_service_names.hpp" #include "type_support_common.hpp" #include "rmw_connext_cpp/connext_static_subscriber_info.hpp" +// include patched generated code from the build folder +#include "connext_static_raw_dataSupport.h" + // Uncomment this to get extra console output about discovery. // This affects code in this file, but there is a similar variable in: // rmw_connext_shared_cpp/shared_functions.cpp diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index 44c79414..4e814e22 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -19,12 +19,14 @@ #include "rmw_connext_shared_cpp/types.hpp" #include "rmw_connext_cpp/connext_static_subscriber_info.hpp" -#include "rmw_connext_cpp/connext_static_raw_data.hpp" -#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" #include "rmw_connext_cpp/identifier.hpp" #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" +// include patched generated code from the build folder +#include "connext_static_raw_dataSupport.h" +#include "connext_static_raw_data.h" + static bool take( DDSDataReader * dds_data_reader, From 750ab52b893d9928601cc5a6065ac7f7c2763736 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 20 Mar 2018 16:53:52 -0700 Subject: [PATCH 23/34] remove unused files --- .../connext_static_raw_data.hpp | 95 - .../connext_static_raw_data_plugin.hpp | 341 ---- .../connext_static_raw_data_support.hpp | 153 -- ...tional-differences-to-generated-code.patch | 557 ------ .../connext_static_raw_data.cxx | 313 ---- .../patch_generated/connext_static_raw_data.h | 106 -- .../connext_static_raw_dataPlugin.cxx | 1496 ---------------- .../connext_static_raw_dataPlugin.h | 343 ---- .../connext_static_raw_dataSupport.cxx | 176 -- .../connext_static_raw_dataSupport.h | 155 -- .../raw_generated/connext_static_raw_data.cxx | 313 ---- .../raw_generated/connext_static_raw_data.h | 106 -- .../connext_static_raw_dataPlugin.cxx | 1531 ----------------- .../connext_static_raw_dataPlugin.h | 340 ---- .../connext_static_raw_dataSupport.cxx | 117 -- .../connext_static_raw_dataSupport.h | 62 - .../src/connext_static_raw_data.cpp | 311 ---- .../src/connext_static_raw_data_plugin.cpp | 1490 ---------------- .../src/connext_static_raw_data_support.cpp | 175 -- 19 files changed, 8180 deletions(-) delete mode 100644 rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp delete mode 100644 rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp delete mode 100644 rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp delete mode 100644 rmw_connext_cpp/resources/patch_files/0001-patch-for-functional-differences-to-generated-code.patch delete mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx delete mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h delete mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx delete mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h delete mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx delete mode 100644 rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h delete mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.cxx delete mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.h delete mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.cxx delete mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.h delete mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.cxx delete mode 100644 rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.h delete mode 100644 rmw_connext_cpp/src/connext_static_raw_data.cpp delete mode 100644 rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp delete mode 100644 rmw_connext_cpp/src/connext_static_raw_data_support.cpp diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp deleted file mode 100644 index 4213287a..00000000 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data.hpp +++ /dev/null @@ -1,95 +0,0 @@ -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_HPP_ -#define RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_HPP_ - -#include "rmw_connext_shared_cpp/ndds_include.hpp" - -static const DDS_Long KEY_HASH_LENGTH_16 = 16; - -extern "C" { -extern const char * ConnextStaticRawDataTYPENAME; -} - -struct ConnextStaticRawDataSeq; -#ifndef NDDS_STANDALONE_TYPE -class ConnextStaticRawDataTypeSupport; -class ConnextStaticRawDataDataWriter; -class ConnextStaticRawDataDataReader; -#endif - -class ConnextStaticRawData -{ -public: - typedef struct ConnextStaticRawDataSeq Seq; - #ifndef NDDS_STANDALONE_TYPE - typedef ConnextStaticRawDataTypeSupport TypeSupport; - typedef ConnextStaticRawDataDataWriter DataWriter; - typedef ConnextStaticRawDataDataReader DataReader; - #endif - - DDS_Octet key_hash[(KEY_HASH_LENGTH_16)]; - DDS_OctetSeq serialized_key; - DDS_OctetSeq serialized_data; -}; -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) -#endif - -NDDSUSERDllExport DDS_TypeCode * ConnextStaticRawData_get_typecode(void); - -DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData * self); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData * self, RTIBool allocatePointers, RTIBool allocateMemory); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData * self, - const struct DDS_TypeAllocationParams_t * allocParams); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize( - ConnextStaticRawData * self); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData * self, RTIBool deletePointers); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData * self, - const struct DDS_TypeDeallocationParams_t * deallocParams); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData * self, RTIBool deletePointers); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData * dst, - const ConnextStaticRawData * src); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif // RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_HPP_ diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp deleted file mode 100644 index e75eb98c..00000000 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_plugin.hpp +++ /dev/null @@ -1,341 +0,0 @@ -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_PLUGIN_HPP_ -#define RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_PLUGIN_HPP_ - -#include "rmw_connext_cpp/connext_static_raw_data.hpp" - -struct RTICdrStream; - -#ifndef pres_typePlugin_h -#include "pres/pres_typePlugin.h" -#endif - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) -#endif - -extern "C" { - -/* The type used to store keys for instances of type struct - * AnotherSimple. - * - * By default, this type is struct ConnextStaticRawData - * itself. However, if for some reason this choice is not practical for your - * system (e.g. if sizeof(struct ConnextStaticRawData) - * is very large), you may redefine this typedef in terms of another type of - * your choosing. HOWEVER, if you define the KeyHolder type to be something - * other than struct AnotherSimple, the - * following restriction applies: the key of struct - * ConnextStaticRawData must consist of a - * single field of your redefined KeyHolder type and that field must be the - * first field in struct ConnextStaticRawData. - */ -typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; - -#define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample -#define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer -#define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer - -#define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey -#define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey - -#define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample -#define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample - -/* -------------------------------------------------------------------------------------- - Support functions: - * -------------------------------------------------------------------------------------- */ - -NDDSUSERDllExport extern ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params); - -NDDSUSERDllExport extern ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); - -NDDSUSERDllExport extern ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data(void); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData * out, - const ConnextStaticRawData * in); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData * sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData * sample, RTIBool deallocate_pointers); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData * sample); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData * sample, - const char * desc, - unsigned int indent); - -NDDSUSERDllExport extern ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); - -NDDSUSERDllExport extern ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key(void); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder * key, RTIBool deallocate_pointers); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder * key); - -/* ---------------------------------------------------------------------------- - Callback functions: - * ---------------------------------------------------------------------------- */ - -NDDSUSERDllExport extern PRESTypePluginParticipantData -ConnextStaticRawDataPlugin_on_participant_attached( - void * registration_data, - const struct PRESTypePluginParticipantInfo * participant_info, - RTIBool top_level_registration, - void * container_plugin_context, - RTICdrTypeCode * typeCode); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data); - -NDDSUSERDllExport extern PRESTypePluginEndpointData -ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo * endpoint_info, - RTIBool top_level_registration, - void * container_plugin_context); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - void * handle); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * out, - const ConnextStaticRawData * in); - -/* ---------------------------------------------------------------------------- - (De)Serialize functions: - * ------------------------------------------------------------------------- */ - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void * endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void * endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData * sample); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData ** sample, - RTIBool * drop_sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void * endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData * sample, - const char * buffer, - unsigned int length); - -NDDSUSERDllExport extern DDS_ReturnCode_t -ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData * sample, - char * str, - DDS_UnsignedLong * str_size, - const struct DDS_PrintFormatProperty * property); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream * stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void * endpoint_plugin_qos); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample); - -/* -------------------------------------------------------------------------------------- - Key Management functions: - * -------------------------------------------------------------------------------------- */ -NDDSUSERDllExport extern PRESTypePluginKeyKind -ConnextStaticRawDataPlugin_get_key_kind(void); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void * endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void * endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData ** sample, - RTIBool * drop_sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void * endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void * endpoint_plugin_qos); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder * key, - const ConnextStaticRawData * instance); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * instance, - const ConnextStaticRawDataKeyHolder * key); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t * keyhash, - const ConnextStaticRawData * instance); - -NDDSUSERDllExport extern RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream * stream, - DDS_KeyHash_t * keyhash, - RTIBool deserialize_encapsulation, - void * endpoint_plugin_qos); - -/* Plugin Functions */ -NDDSUSERDllExport extern struct PRESTypePlugin * -ConnextStaticRawDataPlugin_new(void); - -NDDSUSERDllExport -extern struct PRESTypePlugin * -ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); - -NDDSUSERDllExport extern void -ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); -} - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif // RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_PLUGIN_HPP_ diff --git a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp b/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp deleted file mode 100644 index 03fd88e5..00000000 --- a/rmw_connext_cpp/include/rmw_connext_cpp/connext_static_raw_data_support.hpp +++ /dev/null @@ -1,153 +0,0 @@ -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_SUPPORT_HPP_ -#define RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_SUPPORT_HPP_ - -/* Uses */ -#include "rmw_connext_cpp/connext_static_raw_data.hpp" - -#ifndef ndds_cpp_h -#include "rmw_connext_shared_cpp/ndds_include.hpp" -#endif - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) - -class __declspec (dllimport) DDSTypeSupport; -class __declspec (dllimport) DDSDataWriter; -class __declspec (dllimport) DDSDataReader; - -#endif - -/* ========================================================================= */ -/** -Uses: T - -Defines: TTypeSupport, TDataWriter, TDataReader - -Organized using the well-documented "Generics Pattern" for -implementing generics in C and C++. -*/ - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) - -#endif - -class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport -{ -public: - ConnextStaticRawDataTypeSupport(bool osrf) - { - (void) osrf; - } - - ~ConnextStaticRawDataTypeSupport(); - - static DDS_ReturnCode_t register_type( - DDSDomainParticipant * participant, - const char * type_name = "ConnextStaticRawData"); - - static DDS_ReturnCode_t unregister_type( - DDSDomainParticipant * participant, - const char * type_name = "ConnextStaticRawData"); - - static const char * get_type_name(); - - static ConnextStaticRawData * create_data_ex(DDS_Boolean allocatePointers); - - static ConnextStaticRawData * create_data( - const DDS_TypeAllocationParams_t & alloc_params = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); - - static DDS_ReturnCode_t delete_data_ex( - ConnextStaticRawData * a_data, - DDS_Boolean deletePointers); - - static DDS_ReturnCode_t delete_data( - ConnextStaticRawData * a_data, - const DDS_TypeDeallocationParams_t & dealloc_params = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); - - static void print_data(const ConnextStaticRawData * a_data); - - static DDS_ReturnCode_t copy_data( - ConnextStaticRawData * dst_data, const ConnextStaticRawData * src_data); - - static DDS_ReturnCode_t initialize_data_ex( - ConnextStaticRawData * a_data, - DDS_Boolean allocatePointers); - - static DDS_ReturnCode_t initialize_data( - ConnextStaticRawData * a_data, - const DDS_TypeAllocationParams_t & alloc_params = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); - - static DDS_ReturnCode_t finalize_data_ex( - ConnextStaticRawData * a_data, - DDS_Boolean deletePointers); - - static DDS_ReturnCode_t finalize_data( - ConnextStaticRawData * a_data, - const DDS_TypeDeallocationParams_t & dealloc_params = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); - - DDSDataReader * create_datareaderI(DDSDataReader * dataReader); - - DDS_ReturnCode_t destroy_datareaderI(DDSDataReader * dataReader); - - DDSDataWriter * create_datawriterI(DDSDataWriter * dataWriter); - - DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter * dataWriter); - - static DDS_TypeCode * get_typecode(); - - static DDS_ReturnCode_t serialize_data_to_cdr_buffer( - char * buffer, - unsigned int & length, - const ConnextStaticRawData * a_data); - - static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( - ConnextStaticRawData * a_data, - const char * buffer, - unsigned int length); - - static DDS_ReturnCode_t data_to_string( - ConnextStaticRawData * sample, - char * str, - DDS_UnsignedLong & str_size, - const DDS_PrintFormatProperty & property); - - static void finalize(); - -private: - ConnextStaticRawDataTypeSupport(); -}; - -DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); -DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); - -NDDSUSERDllExport -DDS_ReturnCode_t -ConnextStaticRawDataSupport_register_external_type( - DDSDomainParticipant * participant, - const char * type_name, - struct DDS_TypeCode * type_code); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif // RMW_CONNEXT_CPP__CONNEXT_STATIC_RAW_DATA_SUPPORT_HPP_ diff --git a/rmw_connext_cpp/resources/patch_files/0001-patch-for-functional-differences-to-generated-code.patch b/rmw_connext_cpp/resources/patch_files/0001-patch-for-functional-differences-to-generated-code.patch deleted file mode 100644 index aff956fe..00000000 --- a/rmw_connext_cpp/resources/patch_files/0001-patch-for-functional-differences-to-generated-code.patch +++ /dev/null @@ -1,557 +0,0 @@ -From 75284eee62dd86075a781cade020926cf3518c95 Mon Sep 17 00:00:00 2001 -From: Karsten Knese -Date: Mon, 5 Mar 2018 16:35:17 -0800 -Subject: [PATCH] patch for functional differences to generated code - ---- - .../connext_static_raw_dataPlugin.cxx | 295 +++++++++------------ - .../connext_static_raw_dataPlugin.h | 3 + - .../connext_static_raw_dataSupport.cxx | 59 +++++ - .../connext_static_raw_dataSupport.h | 99 ++++++- - 4 files changed, 288 insertions(+), 168 deletions(-) - -diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx -index 78b58d5..831d83f 100644 ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx -@@ -379,74 +379,81 @@ ConnextStaticRawDataPlugin_serialize( - RTIBool serialize_sample, - void *endpoint_plugin_qos) - { -- char * position = NULL; -- RTIBool retval = RTI_TRUE; -- -- if (endpoint_data) {} /* To avoid warnings */ -- if (endpoint_plugin_qos) {} /* To avoid warnings */ -- -- if(serialize_encapsulation) { -- if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { -- return RTI_FALSE; -- } -+ char * position = NULL; -+ RTIBool retval = RTI_TRUE; -+ -+ if (endpoint_data) {} /* To avoid warnings */ -+ if (endpoint_plugin_qos) {} /* To avoid warnings */ -+ -+ /* This plugin can only be used to publish the top-level DDS Topic-Type -+ * in which case serialize_encapsulation==TRUE. If that is not -+ * the case then it is an error. -+ */ -+ if (!serialize_encapsulation) { -+ return RTI_FALSE; -+ } -+ -+ position = RTICdrStream_resetAlignment(stream); -+ -+ if (serialize_sample) { -+ /* The sample->serialized_data contains the serialized encapsulation followed by the serialized -+ * data, so we only need to copy that into -+ * the CDR stream. Not the key_hash, not the length of the data itself -+ * The SerializedType sample->serialized_data is always a contiguous buffer -+ */ -+ DDS_Octet * buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); -+ if (buffer == NULL) { -+ return RTI_FALSE; -+ } - -- position = RTICdrStream_resetAlignment(stream); -+ /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian -+ * byte order -+ */ -+ if (encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { -+ return RTI_FALSE; - } - -- if(serialize_sample) { -+ /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ -+ if (!RTICdrStream_serializePrimitiveArray( -+ stream, (void *)buffer, -+ DDS_OctetSeq_get_length(&sample->serialized_data), -+ RTI_CDR_OCTET_TYPE)) -+ { -+ return RTI_FALSE; -+ } -+ } - -- if (!RTICdrStream_serializePrimitiveArray( -- stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { -- return RTI_FALSE; -- } -+ RTICdrStream_restoreAlignment(stream, position); - -- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { -- if (!RTICdrStream_serializePrimitiveSequence( -- stream, -- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), -- DDS_OctetSeq_get_length(&sample->serialized_key), -- (RTI_INT32_MAX-1), -- RTI_CDR_OCTET_TYPE)) { -- return RTI_FALSE; -- } -- } else { -- if (!RTICdrStream_serializePrimitivePointerSequence( -- stream, -- (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), -- DDS_OctetSeq_get_length(&sample->serialized_key), -- (RTI_INT32_MAX-1), -- RTI_CDR_OCTET_TYPE)) { -- return RTI_FALSE; -- } -- } -- -- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { -- if (!RTICdrStream_serializePrimitiveSequence( -- stream, -- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), -- DDS_OctetSeq_get_length(&sample->serialized_data), -- (RTI_INT32_MAX-1), -- RTI_CDR_OCTET_TYPE)) { -- return RTI_FALSE; -- } -- } else { -- if (!RTICdrStream_serializePrimitivePointerSequence( -- stream, -- (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), -- DDS_OctetSeq_get_length(&sample->serialized_data), -- (RTI_INT32_MAX-1), -- RTI_CDR_OCTET_TYPE)) { -- return RTI_FALSE; -- } -- } -+ return retval; -+} - -- } -+/** -+ TODO. The code-block below does not belong here. -+ It should be pushed to the CDR module, perhaps inside -+ RTICdrStream_deserializeAndSetCdrEncapsulation so that the -+ stream size is alredy correct when SerializedTypePlugin_deserialize_sample -+ is called. - -- if(serialize_encapsulation) { -- RTICdrStream_restoreAlignment(stream,position); -- } -+ Adjust the size of the CDR stream to not include the alignment -+ padding. See http://issues.omg.org/browse/DDSXTY12-10 - -- return retval; -+ @precondition The RTICdrStream *stream has alreadt processed -+ the encapsulation header and therefore has set the -+ encapsulation options returned by -+ RTICdrStream_getEncapsulationOptions() -+*/ -+void -+ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) -+{ -+ /* See http://issues.omg.org/browse/DDSXTY12-10 */ -+ DDS_UnsignedShort padding_size_mask = 0x0003; -+ DDS_UnsignedShort padding_size; -+ int adjustedBufferLength; -+ -+ padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; -+ adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; -+ RTICdrStream_setBufferLength(stream, adjustedBufferLength); - } - - RTIBool -@@ -458,114 +465,65 @@ ConnextStaticRawDataPlugin_deserialize_sample( - RTIBool deserialize_sample, - void *endpoint_plugin_qos) - { -+ char * position = NULL; -+ RTIBool done = RTI_FALSE; -+ -+ if (endpoint_data) {} /* To avoid warnings */ -+ if (endpoint_plugin_qos) {} /* To avoid warnings */ -+ -+ /* This plugin can only be used to publish the top-level DDS Topic-Type -+ * in which case deserialize_encapsulation==TRUE. If that is not -+ * the case then it is an error. -+ */ -+ if (!deserialize_encapsulation) { -+ return RTI_FALSE; -+ } -+ -+ position = RTICdrStream_resetAlignment(stream); -+ -+ /* TODO. The call does not belong here. It should be pushed -+ * inside RTICdrStream_deserializeAndSetCdrEncapsulation -+ */ -+ ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); -+ -+ if (deserialize_sample) { -+ /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() -+ it is done there because SerializedTypePlugin_deserialize_sample does not -+ have access to the SampleInfo where that information is -+ */ -+ -+ /* We do not set the serialized_key on deserialization */ -+ DDS_OctetSeq_set_length(&sample->serialized_key, 0); -+ -+ /* We copy everything that remains in the CDR stream */ -+ int bytesLeftInStream = RTICdrStream_getRemainder(stream); -+ DDS_Octet * cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); -+ if (cdrBufferPtr == NULL) { -+ goto fin; -+ } - -- char * position = NULL; -- -- RTIBool done = RTI_FALSE; -- -- try { -- -- if (endpoint_data) {} /* To avoid warnings */ -- if (endpoint_plugin_qos) {} /* To avoid warnings */ -- if(deserialize_encapsulation) { -- -- if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { -- return RTI_FALSE; -- } -- -- position = RTICdrStream_resetAlignment(stream); -- } -- if(deserialize_sample) { -- -- ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); -- -- if (!RTICdrStream_deserializePrimitiveArray( -- stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { -- goto fin; -- } -- -- { -- RTICdrUnsignedLong sequence_length; -- if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { -- goto fin; -- } -- if (!DDS_OctetSeq_set_maximum(&sample->serialized_key,sequence_length)) { -- return RTI_FALSE; -- } -- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { -- if (!RTICdrStream_deserializePrimitiveSequence( -- stream, -- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), -- &sequence_length, -- DDS_OctetSeq_get_maximum(&sample->serialized_key), -- RTI_CDR_OCTET_TYPE)){ -- goto fin; -- } -- } else { -- if (!RTICdrStream_deserializePrimitivePointerSequence( -- stream, -- (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), -- &sequence_length, -- DDS_OctetSeq_get_maximum(&sample->serialized_key), -- RTI_CDR_OCTET_TYPE)){ -- goto fin; -- } -- } -- if (!DDS_OctetSeq_set_length(&sample->serialized_key, sequence_length)) { -- return RTI_FALSE; -- } -- -- } -- { -- RTICdrUnsignedLong sequence_length; -- if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { -- goto fin; -- } -- if (!DDS_OctetSeq_set_maximum(&sample->serialized_data,sequence_length)) { -- return RTI_FALSE; -- } -- if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { -- if (!RTICdrStream_deserializePrimitiveSequence( -- stream, -- DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), -- &sequence_length, -- DDS_OctetSeq_get_maximum(&sample->serialized_data), -- RTI_CDR_OCTET_TYPE)){ -- goto fin; -- } -- } else { -- if (!RTICdrStream_deserializePrimitivePointerSequence( -- stream, -- (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), -- &sequence_length, -- DDS_OctetSeq_get_maximum(&sample->serialized_data), -- RTI_CDR_OCTET_TYPE)){ -- goto fin; -- } -- } -- if (!DDS_OctetSeq_set_length(&sample->serialized_data, sequence_length)) { -- return RTI_FALSE; -- } -+ /* Do not call SerializedType_initialize_ex initialize here -+ because it would override the key_hash field -+ SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); -+ */ -+ if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { -+ goto fin; -+ } -+ RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); -+ } - -- } -- } -+ done = RTI_TRUE; - -- done = RTI_TRUE; -- fin: -- if (done != RTI_TRUE && -- RTICdrStream_getRemainder(stream) >= -- RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { -- return RTI_FALSE; -- } -- if(deserialize_encapsulation) { -- RTICdrStream_restoreAlignment(stream,position); -- } -+fin: -+ if ( (done != RTI_TRUE) && -+ (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) -+ { -+ return RTI_FALSE; -+ } - -- return RTI_TRUE; -+ RTICdrStream_restoreAlignment(stream, position); - -- } catch (std::bad_alloc&) { -- return RTI_FALSE; -- } -+ return RTI_TRUE; - } - - RTIBool -@@ -971,7 +929,9 @@ Key Management functions: - PRESTypePluginKeyKind - ConnextStaticRawDataPlugin_get_key_kind(void) - { -- return PRES_TYPEPLUGIN_USER_KEY; -+ // TODO(karsten1987): Whenever we introduce keys for our data types -+ // this might have to change. -+ return PRES_TYPEPLUGIN_NO_KEY; - } - - RTIBool -@@ -1408,6 +1368,11 @@ ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - * ------------------------------------------------------------------------ */ - struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) - { -+ return NULL; -+} -+ -+struct PRESTypePlugin *ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) -+{ - struct PRESTypePlugin *plugin = NULL; - const struct PRESTypePluginVersion PLUGIN_VERSION = - PRES_TYPE_PLUGIN_VERSION_2_0; -@@ -1503,7 +1468,7 @@ struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) - (PRESTypePluginKeyToInstanceFunction) - ConnextStaticRawDataPlugin_key_to_instance; - plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ -- plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); -+ plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; - - plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; - -diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h -index 89b4c67..bd23b77 100644 ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h -@@ -324,6 +324,9 @@ extern "C" { - NDDSUSERDllExport extern struct PRESTypePlugin* - ConnextStaticRawDataPlugin_new(void); - -+ NDDS_USER_DLL_EXPORT extern struct PRESTypePlugin* -+ ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); -+ - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); - -diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx -index 83be31b..f2c9c63 100644 ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx -@@ -115,3 +115,62 @@ Defines: TTypeSupport, TData, TDataReader, TDataWriter - #undef TPlugin_new - #undef TPlugin_delete - -+DDS_ReturnCode_t -+ConnextStaticRawDataSupport_register_external_type( -+ DDSDomainParticipant * participant, -+ const char * type_name, -+ struct DDS_TypeCode * type_code) -+{ -+ DDSTypeSupport * dds_data_type = NULL; -+ struct PRESTypePlugin * presTypePlugin = NULL; -+ DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; -+ DDS_Boolean delete_data_type = DDS_BOOLEAN_FALSE; -+ RTIBool already_registered = RTI_FALSE; -+ -+ if (type_code == NULL) { -+ goto finError; -+ } -+ -+ if (participant == NULL) { -+ goto finError; -+ } -+ -+ /* TODO pass the type_code */ -+ presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); -+ if (presTypePlugin == NULL) { -+ goto finError; -+ } -+ -+ dds_data_type = new ConnextStaticRawDataTypeSupport(true); -+ if (dds_data_type == NULL) { -+ fprintf(stderr, "Error while registering external type\n"); -+ goto finError; -+ } -+ delete_data_type = RTI_TRUE; -+ -+ presTypePlugin->_userBuffer = (PRESWord *)dds_data_type; -+ already_registered = participant->is_type_registered(type_name); -+ -+ retcode = participant->register_type(type_name, presTypePlugin, NULL, !already_registered); -+ if (retcode != DDS_RETCODE_OK) { -+ fprintf(stderr, "error while registering external type\n"); -+ goto finError; -+ } -+ -+ if (!already_registered) { -+ delete_data_type = DDS_BOOLEAN_FALSE; -+ } -+ -+ retcode = DDS_RETCODE_OK; -+ -+finError: -+ if (presTypePlugin != NULL) { -+ ConnextStaticRawDataPlugin_delete(presTypePlugin); -+ } -+ if (delete_data_type) { -+ delete (ConnextStaticRawDataTypeSupport *)dds_data_type; -+ dds_data_type = NULL; -+ } -+ -+ return retcode; -+} -diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h -index 012a1f6..51db529 100644 ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h -@@ -44,13 +44,106 @@ implementing generics in C and C++. - - #endif - --DDS_TYPESUPPORT_CPP( -- ConnextStaticRawDataTypeSupport, -- ConnextStaticRawData); -+class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport -+{ -+public: -+ ConnextStaticRawDataTypeSupport(bool osrf) -+ { -+ (void) osrf; -+ } -+ -+ ~ConnextStaticRawDataTypeSupport(); -+ -+ static DDS_ReturnCode_t register_type( -+ DDSDomainParticipant * participant, -+ const char * type_name = "ConnextStaticRawData"); -+ -+ static DDS_ReturnCode_t unregister_type( -+ DDSDomainParticipant * participant, -+ const char * type_name = "ConnextStaticRawData"); -+ -+ static const char * get_type_name(); -+ -+ static ConnextStaticRawData * create_data_ex(DDS_Boolean allocatePointers); -+ -+ static ConnextStaticRawData * create_data( -+ const DDS_TypeAllocationParams_t & alloc_params = -+ DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); -+ -+ static DDS_ReturnCode_t delete_data_ex( -+ ConnextStaticRawData * a_data, -+ DDS_Boolean deletePointers); -+ -+ static DDS_ReturnCode_t delete_data( -+ ConnextStaticRawData * a_data, -+ const DDS_TypeDeallocationParams_t & dealloc_params = -+ DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); -+ -+ static void print_data(const ConnextStaticRawData * a_data); -+ -+ static DDS_ReturnCode_t copy_data( -+ ConnextStaticRawData * dst_data, const ConnextStaticRawData * src_data); -+ -+ static DDS_ReturnCode_t initialize_data_ex( -+ ConnextStaticRawData * a_data, -+ DDS_Boolean allocatePointers); -+ -+ static DDS_ReturnCode_t initialize_data( -+ ConnextStaticRawData * a_data, -+ const DDS_TypeAllocationParams_t & alloc_params = -+ DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); -+ -+ static DDS_ReturnCode_t finalize_data_ex( -+ ConnextStaticRawData * a_data, -+ DDS_Boolean deletePointers); -+ -+ static DDS_ReturnCode_t finalize_data( -+ ConnextStaticRawData * a_data, -+ const DDS_TypeDeallocationParams_t & dealloc_params = -+ DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); -+ -+ DDSDataReader * create_datareaderI(DDSDataReader * dataReader); -+ -+ DDS_ReturnCode_t destroy_datareaderI(DDSDataReader * dataReader); -+ -+ DDSDataWriter * create_datawriterI(DDSDataWriter * dataWriter); -+ -+ DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter * dataWriter); -+ -+ static DDS_TypeCode * get_typecode(); -+ -+ static DDS_ReturnCode_t serialize_data_to_cdr_buffer( -+ char * buffer, -+ unsigned int & length, -+ const ConnextStaticRawData * a_data); -+ -+ static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( -+ ConnextStaticRawData * a_data, -+ const char * buffer, -+ unsigned int length); -+ -+ static DDS_ReturnCode_t data_to_string( -+ ConnextStaticRawData * sample, -+ char * str, -+ DDS_UnsignedLong & str_size, -+ const DDS_PrintFormatProperty & property); -+ -+ static void finalize(); -+ -+private: -+ ConnextStaticRawDataTypeSupport(); -+}; - - DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); - DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); - -+NDDSUSERDllExport -+DDS_ReturnCode_t -+ConnextStaticRawDataSupport_register_external_type( -+ DDSDomainParticipant * participant, -+ const char * type_name, -+ struct DDS_TypeCode * type_code); -+ - #if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) - /* If the code is building on Windows, stop exporting symbols. - */ --- -2.12.0 - diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx deleted file mode 100644 index 7b954282..00000000 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx +++ /dev/null @@ -1,313 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef NDDS_STANDALONE_TYPE -#ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" -#endif -#ifndef dds_c_log_impl_h -#include "dds_c/dds_c_log_impl.h" -#endif - -#ifndef cdr_type_h -#include "cdr/cdr_type.h" -#endif - -#ifndef osapi_heap_h -#include "osapi/osapi_heap.h" -#endif -#else -#include "ndds_standalone_type.h" -#endif - -#include "connext_static_raw_data.h" - -#include - -/* ========================================================================= */ -const char *ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; - -DDS_TypeCode* ConnextStaticRawData_get_typecode() -{ - static RTIBool is_initialized = RTI_FALSE; - - static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array =DDS_INITIALIZE_ARRAY_TYPECODE(1,(KEY_HASH_LENGTH_16), NULL,NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); - static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3]= - { - - { - (char *)"key_hash",/* Member name */ - { - 0,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_KEY_MEMBER , /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - }, - { - (char *)"serialized_key",/* Member name */ - { - 1,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - }, - { - (char *)"serialized_data",/* Member name */ - { - 2,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - } - }; - - static DDS_TypeCode ConnextStaticRawData_g_tc = - {{ - DDS_TK_STRUCT,/* Kind */ - DDS_BOOLEAN_FALSE, /* Ignored */ - -1, /*Ignored*/ - (char *)"ConnextStaticRawData", /* Name */ - NULL, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - 3, /* Number of members */ - ConnextStaticRawData_g_tc_members, /* Members */ - DDS_VM_NONE /* Ignored */ - }}; /* Type code for ConnextStaticRawData*/ - - if (is_initialized) { - return &ConnextStaticRawData_g_tc; - } - - ConnextStaticRawData_g_tc_key_hash_array._data._typeCode =(RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_members[0]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_key_hash_array; - ConnextStaticRawData_g_tc_members[1]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_key_sequence; - ConnextStaticRawData_g_tc_members[2]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_data_sequence; - - is_initialized = RTI_TRUE; - - return &ConnextStaticRawData_g_tc; -} - -RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData* sample) { - return ConnextStaticRawData_initialize_ex(sample,RTI_TRUE,RTI_TRUE); -} - -RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData* sample,RTIBool allocatePointers, RTIBool allocateMemory) -{ - - struct DDS_TypeAllocationParams_t allocParams = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; - - allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; - allocParams.allocate_memory = (DDS_Boolean)allocateMemory; - - return ConnextStaticRawData_initialize_w_params( - sample,&allocParams); - -} - -RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData* sample, const struct DDS_TypeAllocationParams_t * allocParams) -{ - - void* buffer = NULL; - if (buffer) {} /* To avoid warnings */ - - if (sample == NULL) { - return RTI_FALSE; - } - if (allocParams == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_initArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_key ); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key , RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_data ); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data , RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_data, 0); - } - return RTI_TRUE; -} - -void ConnextStaticRawData_finalize( - ConnextStaticRawData* sample) -{ - - ConnextStaticRawData_finalize_ex(sample,RTI_TRUE); -} - -void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData* sample,RTIBool deletePointers) -{ - struct DDS_TypeDeallocationParams_t deallocParams = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - - if (sample==NULL) { - return; - } - - deallocParams.delete_pointers = (DDS_Boolean)deletePointers; - - ConnextStaticRawData_finalize_w_params( - sample,&deallocParams); -} - -void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData* sample,const struct DDS_TypeDeallocationParams_t * deallocParams) -{ - - if (sample==NULL) { - return; - } - - if (deallocParams == NULL) { - return; - } - - DDS_OctetSeq_finalize(&sample->serialized_key); - - DDS_OctetSeq_finalize(&sample->serialized_data); - -} - -void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* sample, RTIBool deletePointers) -{ - struct DDS_TypeDeallocationParams_t deallocParamsTmp = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - struct DDS_TypeDeallocationParams_t * deallocParams = - &deallocParamsTmp; - - if (sample==NULL) { - return; - } - if (deallocParams) {} /* To avoid warnings */ - - deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; - deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; - -} - -RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData* dst, - const ConnextStaticRawData* src) -{ - try { - - if (dst == NULL || src == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_key , - &src->serialized_key )) { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_data , - &src->serialized_data )) { - return RTI_FALSE; - } - - return RTI_TRUE; - - } catch (std::bad_alloc&) { - return RTI_FALSE; - } -} - -/** -* <> -* -* Defines: TSeq, T -* -* Configure and implement 'ConnextStaticRawData' sequence class. -*/ -#define T ConnextStaticRawData -#define TSeq ConnextStaticRawDataSeq - -#define T_initialize_w_params ConnextStaticRawData_initialize_w_params - -#define T_finalize_w_params ConnextStaticRawData_finalize_w_params -#define T_copy ConnextStaticRawData_copy - -#ifndef NDDS_STANDALONE_TYPE -#include "dds_c/generic/dds_c_sequence_TSeq.gen" -#include "dds_cpp/generic/dds_cpp_sequence_TSeq.gen" -#else -#include "dds_c_sequence_TSeq.gen" -#include "dds_cpp_sequence_TSeq.gen" -#endif - -#undef T_copy -#undef T_finalize_w_params - -#undef T_initialize_w_params - -#undef TSeq -#undef T - diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h deleted file mode 100644 index d3adb4fa..00000000 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h +++ /dev/null @@ -1,106 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef connext_static_raw_data_738563504_h -#define connext_static_raw_data_738563504_h - -#ifndef NDDS_STANDALONE_TYPE -#ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" -#endif -#else -#include "ndds_standalone_type.h" -#endif - -static const DDS_Long KEY_HASH_LENGTH_16= 16; -extern "C" { - - extern const char *ConnextStaticRawDataTYPENAME; - -} - -struct ConnextStaticRawDataSeq; -#ifndef NDDS_STANDALONE_TYPE -class ConnextStaticRawDataTypeSupport; -class ConnextStaticRawDataDataWriter; -class ConnextStaticRawDataDataReader; -#endif - -class ConnextStaticRawData -{ - public: - typedef struct ConnextStaticRawDataSeq Seq; - #ifndef NDDS_STANDALONE_TYPE - typedef ConnextStaticRawDataTypeSupport TypeSupport; - typedef ConnextStaticRawDataDataWriter DataWriter; - typedef ConnextStaticRawDataDataReader DataReader; - #endif - - DDS_Octet key_hash [(KEY_HASH_LENGTH_16)]; - DDS_OctetSeq serialized_key ; - DDS_OctetSeq serialized_data ; - -}; -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) -#endif - -NDDSUSERDllExport DDS_TypeCode* ConnextStaticRawData_get_typecode(void); /* Type code */ - -DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData* self); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData* self,RTIBool allocatePointers,RTIBool allocateMemory); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData* self, - const struct DDS_TypeAllocationParams_t * allocParams); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize( - ConnextStaticRawData* self); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData* self,RTIBool deletePointers); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData* self, - const struct DDS_TypeDeallocationParams_t * deallocParams); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* self, RTIBool deletePointers); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData* dst, - const ConnextStaticRawData* src); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* connext_static_raw_data */ - diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx deleted file mode 100644 index 831d83ff..00000000 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +++ /dev/null @@ -1,1496 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#include - -#ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" -#endif - -#ifndef osapi_type_h -#include "osapi/osapi_type.h" -#endif -#ifndef osapi_heap_h -#include "osapi/osapi_heap.h" -#endif - -#ifndef osapi_utility_h -#include "osapi/osapi_utility.h" -#endif - -#ifndef cdr_type_h -#include "cdr/cdr_type.h" -#endif - -#ifndef cdr_type_object_h -#include "cdr/cdr_typeObject.h" -#endif - -#ifndef cdr_encapsulation_h -#include "cdr/cdr_encapsulation.h" -#endif - -#ifndef cdr_stream_h -#include "cdr/cdr_stream.h" -#endif - -#ifndef cdr_log_h -#include "cdr/cdr_log.h" -#endif - -#ifndef pres_typePlugin_h -#include "pres/pres_typePlugin.h" -#endif - -#define RTI_CDR_CURRENT_SUBMODULE RTI_CDR_SUBMODULE_MASK_STREAM - -#include - -#include "connext_static_raw_dataPlugin.h" - -/* ---------------------------------------------------------------------------- -* Type ConnextStaticRawData -* -------------------------------------------------------------------------- */ - -/* ----------------------------------------------------------------------------- -Support functions: -* -------------------------------------------------------------------------- */ - -ConnextStaticRawData* -ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params) -{ - ConnextStaticRawData *sample = NULL; - - sample = new (std::nothrow) ConnextStaticRawData ; - if (sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_w_params(sample,alloc_params)) { - delete sample; - sample=NULL; - } - return sample; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) -{ - ConnextStaticRawData *sample = NULL; - - sample = new (std::nothrow) ConnextStaticRawData ; - - if(sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_ex(sample,allocate_pointers, RTI_TRUE)) { - delete sample; - sample=NULL; - } - - return sample; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data(void) -{ - return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); -} - -void -ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData *sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params) { - - ConnextStaticRawData_finalize_w_params(sample,dealloc_params); - - delete sample; - sample=NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData *sample,RTIBool deallocate_pointers) { - - ConnextStaticRawData_finalize_ex(sample,deallocate_pointers); - - delete sample; - sample=NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData *sample) { - - ConnextStaticRawDataPluginSupport_destroy_data_ex(sample,RTI_TRUE); - -} - -RTIBool -ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData *dst, - const ConnextStaticRawData *src) -{ - return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); -} - -void -ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData *sample, - const char *desc, - unsigned int indent_level) -{ - - RTICdrType_printIndent(indent_level); - - if (desc != NULL) { - RTILog_debug("%s:\n", desc); - } else { - RTILog_debug("\n"); - } - - if (sample == NULL) { - RTILog_debug("NULL\n"); - return; - } - - RTICdrType_printArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "key_hash", indent_level + 1); - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key ), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data ), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } - -} -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers){ - ConnextStaticRawData *key = NULL; - - key = new (std::nothrow) ConnextStaticRawDataKeyHolder ; - - ConnextStaticRawData_initialize_ex(key,allocate_pointers, RTI_TRUE); - - return key; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key(void) -{ - return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); -} - -void -ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) -{ - ConnextStaticRawData_finalize_ex(key,deallocate_pointers); - - delete key; - key=NULL; - -} - -void -ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder *key) { - - ConnextStaticRawDataPluginSupport_destroy_key_ex(key,RTI_TRUE); - -} - -/* ---------------------------------------------------------------------------- -Callback functions: -* ---------------------------------------------------------------------------- */ - -PRESTypePluginParticipantData -ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, - const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, - void *container_plugin_context, - RTICdrTypeCode *type_code) -{ - if (registration_data) {} /* To avoid warnings */ - if (participant_info) {} /* To avoid warnings */ - if (top_level_registration) {} /* To avoid warnings */ - if (container_plugin_context) {} /* To avoid warnings */ - if (type_code) {} /* To avoid warnings */ - - return PRESTypePluginDefaultParticipantData_new(participant_info); - -} - -void -ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data) -{ - - PRESTypePluginDefaultParticipantData_delete(participant_data); -} - -PRESTypePluginEndpointData -ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, - void *containerPluginContext) -{ - PRESTypePluginEndpointData epd = NULL; - - unsigned int serializedSampleMaxSize; - - unsigned int serializedKeyMaxSize; - - if (top_level_registration) {} /* To avoid warnings */ - if (containerPluginContext) {} /* To avoid warnings */ - - epd = PRESTypePluginDefaultEndpointData_new( - participant_data, - endpoint_info, - (PRESTypePluginDefaultEndpointDataCreateSampleFunction) - ConnextStaticRawDataPluginSupport_create_data, - (PRESTypePluginDefaultEndpointDataDestroySampleFunction) - ConnextStaticRawDataPluginSupport_destroy_data, - (PRESTypePluginDefaultEndpointDataCreateKeyFunction) - ConnextStaticRawDataPluginSupport_create_key , - (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) - ConnextStaticRawDataPluginSupport_destroy_key); - - if (epd == NULL) { - return NULL; - } - serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( - epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); - - if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( - epd,endpoint_info,serializedKeyMaxSize)) - { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - - if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { - serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); - - PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); - - if (PRESTypePluginDefaultEndpointData_createWriterPool( - epd, - endpoint_info, - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size, - epd) == RTI_FALSE) { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - } - - return epd; -} - -void -ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data) -{ - - PRESTypePluginDefaultEndpointData_delete(endpoint_data); -} - -void -ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - void *handle) -{ - - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - - PRESTypePluginDefaultEndpointData_returnSample( - endpoint_data, sample, handle); -} - -RTIBool -ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *dst, - const ConnextStaticRawData *src) -{ - if (endpoint_data) {} /* To avoid warnings */ - return ConnextStaticRawDataPluginSupport_copy_data(dst,src); -} - -/* ---------------------------------------------------------------------------- -(De)Serialize functions: -* ------------------------------------------------------------------------- */ -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -RTIBool -ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void *endpoint_plugin_qos) -{ - char * position = NULL; - RTIBool retval = RTI_TRUE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - /* This plugin can only be used to publish the top-level DDS Topic-Type - * in which case serialize_encapsulation==TRUE. If that is not - * the case then it is an error. - */ - if (!serialize_encapsulation) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - - if (serialize_sample) { - /* The sample->serialized_data contains the serialized encapsulation followed by the serialized - * data, so we only need to copy that into - * the CDR stream. Not the key_hash, not the length of the data itself - * The SerializedType sample->serialized_data is always a contiguous buffer - */ - DDS_Octet * buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); - if (buffer == NULL) { - return RTI_FALSE; - } - - /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian - * byte order - */ - if (encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { - return RTI_FALSE; - } - - /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ - if (!RTICdrStream_serializePrimitiveArray( - stream, (void *)buffer, - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE)) - { - return RTI_FALSE; - } - } - - RTICdrStream_restoreAlignment(stream, position); - - return retval; -} - -/** - TODO. The code-block below does not belong here. - It should be pushed to the CDR module, perhaps inside - RTICdrStream_deserializeAndSetCdrEncapsulation so that the - stream size is alredy correct when SerializedTypePlugin_deserialize_sample - is called. - - Adjust the size of the CDR stream to not include the alignment - padding. See http://issues.omg.org/browse/DDSXTY12-10 - - @precondition The RTICdrStream *stream has alreadt processed - the encapsulation header and therefore has set the - encapsulation options returned by - RTICdrStream_getEncapsulationOptions() -*/ -void -ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) -{ - /* See http://issues.omg.org/browse/DDSXTY12-10 */ - DDS_UnsignedShort padding_size_mask = 0x0003; - DDS_UnsignedShort padding_size; - int adjustedBufferLength; - - padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; - adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; - RTICdrStream_setBufferLength(stream, adjustedBufferLength); -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos) -{ - char * position = NULL; - RTIBool done = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - /* This plugin can only be used to publish the top-level DDS Topic-Type - * in which case deserialize_encapsulation==TRUE. If that is not - * the case then it is an error. - */ - if (!deserialize_encapsulation) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - - /* TODO. The call does not belong here. It should be pushed - * inside RTICdrStream_deserializeAndSetCdrEncapsulation - */ - ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); - - if (deserialize_sample) { - /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() - it is done there because SerializedTypePlugin_deserialize_sample does not - have access to the SampleInfo where that information is - */ - - /* We do not set the serialized_key on deserialization */ - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - - /* We copy everything that remains in the CDR stream */ - int bytesLeftInStream = RTICdrStream_getRemainder(stream); - DDS_Octet * cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); - if (cdrBufferPtr == NULL) { - goto fin; - } - - /* Do not call SerializedType_initialize_ex initialize here - because it would override the key_hash field - SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); - */ - if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { - goto fin; - } - RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); - } - - done = RTI_TRUE; - -fin: - if ( (done != RTI_TRUE) && - (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) - { - return RTI_FALSE; - } - - RTICdrStream_restoreAlignment(stream, position); - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData *sample) -{ - struct RTICdrStream stream; - struct PRESTypePluginDefaultEndpointData epd; - RTIBool result; - - if (length == NULL) { - return RTI_FALSE; - } - - epd._maxSizeSerializedSample = - ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); - - if (buffer == NULL) { - *length = - ConnextStaticRawDataPlugin_get_serialized_sample_size( - (PRESTypePluginEndpointData)&epd, - RTI_TRUE, - RTICdrEncapsulation_getNativeCdrEncapsulationId(), - 0, - sample); - - if (*length == 0) { - return RTI_FALSE; - } - - return RTI_TRUE; - } - - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, *length); - - result = ConnextStaticRawDataPlugin_serialize( - (PRESTypePluginEndpointData)&epd, sample, &stream, - RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), - RTI_TRUE, NULL); - - *length = RTICdrStream_getCurrentPositionOffset(&stream); - return result; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData *sample, - const char * buffer, - unsigned int length) -{ - struct RTICdrStream stream; - - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, length); - - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - return ConnextStaticRawDataPlugin_deserialize_sample( - NULL, sample, - &stream, RTI_TRUE, RTI_TRUE, - NULL); -} - -DDS_ReturnCode_t -ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property) -{ - DDS_DynamicData *data = NULL; - char *buffer = NULL; - unsigned int length = 0; - struct DDS_PrintFormat printFormat; - DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; - - if (sample == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (str_size == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (property == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - NULL, - &length, - sample)) { - return DDS_RETCODE_ERROR; - } - - RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); - if (buffer == NULL) { - return DDS_RETCODE_ERROR; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - buffer, - &length, - sample)) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - data = DDS_DynamicData_new( - ConnextStaticRawData_get_typecode(), - &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); - if (data == NULL) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - retCode = DDS_PrintFormatProperty_to_print_format( - property, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - retCode = DDS_DynamicDataFormatter_to_string_w_format( - data, - str, - str_size, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return DDS_RETCODE_OK; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos) -{ - - RTIBool result; - const char *METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; - if (drop_sample) {} /* To avoid warnings */ - - stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_sample( - endpoint_data, (sample != NULL)?*sample:NULL, - stream, deserialize_encapsulation, deserialize_sample, - endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } - } - if (!result && stream->_xTypesState.unassignable ) { - - RTICdrLog_exception( - METHOD_NAME, - &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, - "ConnextStaticRawData"); - - } - - return result; - -} - -RTIBool ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(skip_encapsulation) { - if (!RTICdrStream_skipEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if (skip_sample) { - - if (!RTICdrStream_skipPrimitiveArray( - stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - goto fin; - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - } - - done = RTI_TRUE; - fin: - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - if(skip_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - if (endpoint_data) {} /* To avoid warnings */ - if (include_encapsulation) {} - if (encapsulation_id) {} - if (current_alignment) {} - - if (overflow != NULL) { - *overflow = RTI_TRUE; - } - - return RTI_CDR_MAX_SERIALIZED_SIZE; - -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int size; - RTIBool overflow = RTI_FALSE; - - size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); - - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } - - return size; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - - if (endpoint_data) {} /* To avoid warnings */ - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - } - - current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment,0, RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment,0, RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -/* Returns the size of the sample in its serialized form (in bytes). -* It can also be an estimation in excess of the real buffer needed -* during a call to the serialize() function. -* The value reported does not have to include the space for the -* encapsulation flags. -*/ -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - struct PRESTypePluginDefaultEndpointData epd; - - if (sample==NULL) { - return 0; - } - if (endpoint_data == NULL) { - endpoint_data = (PRESTypePluginEndpointData) &epd; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -/* -------------------------------------------------------------------------------------- -Key Management functions: -* -------------------------------------------------------------------------------------- */ - -PRESTypePluginKeyKind -ConnextStaticRawDataPlugin_get_key_kind(void) -{ - // TODO(karsten1987): Whenever we introduce keys for our data types - // this might have to change. - return PRES_TYPEPLUGIN_NO_KEY; -} - -RTIBool -ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(serialize_encapsulation) { - if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if(serialize_key) { - - if (!RTICdrStream_serializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - } - - if(serialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - try { - - char * position = NULL; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(deserialize_encapsulation) { - - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - if (deserialize_key) { - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; - - } catch (std::bad_alloc&) { - return RTI_FALSE; - } -} - -RTIBool ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - RTIBool result; - if (drop_sample) {} /* To avoid warnings */ - stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_key_sample( - endpoint_data, (sample != NULL)?*sample:NULL, stream, - deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } - } - - return result; - -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - - if (endpoint_data) {} /* To avoid warnings */ - if (overflow) {} /* To avoid warnings */ - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - } - - current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int size; - RTIBool overflow = RTI_FALSE; - - size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); - - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } - - return size; -} - -RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - if(deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - position = RTICdrStream_resetAlignment(stream); - } - - if (deserialize_key) { - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - - } - - done = RTI_TRUE; - fin: - if(!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - } else { - return RTI_FALSE; - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *dst, - const ConnextStaticRawData *src) -{ - - if (endpoint_data) {} /* To avoid warnings */ - - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *dst, const - ConnextStaticRawDataKeyHolder *src) -{ - - if (endpoint_data) {} /* To avoid warnings */ - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t *keyhash, - const ConnextStaticRawData *instance) -{ - struct RTICdrStream * md5Stream = NULL; - struct RTICdrStreamState cdrState; - char * buffer = NULL; - - RTICdrStreamState_init(&cdrState); - md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); - - if (md5Stream == NULL) { - return RTI_FALSE; - } - - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - int size; - - RTICdrStream_pushState(md5Stream, &cdrState, -1); - - size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( - endpoint_data, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - 0, - instance); - - if (size <= RTICdrStream_getBufferLength(md5Stream)) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTIOsapiHeap_allocateBuffer(&buffer,size,0); - - if (buffer == NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTICdrStream_set(md5Stream, buffer, size); - RTIOsapiMemory_zero( - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getBufferLength(md5Stream)); - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - return RTI_FALSE; - } - } - - if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > - (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || - PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { - RTICdrStream_computeMD5(md5Stream, keyhash->value); - } else { - RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); - RTIOsapiMemory_copy( - keyhash->value, - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getCurrentPositionOffset(md5Stream)); - } - - keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; - - if (buffer != NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - } - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - DDS_KeyHash_t *keyhash, - RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - ConnextStaticRawData * sample=NULL; - - if (endpoint_plugin_qos) {} /* To avoid warnings */ - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - - if(deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - sample = (ConnextStaticRawData *) - PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); - - if (sample == NULL) { - return RTI_FALSE; - } - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - done = RTI_TRUE; - fin: - if(!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - } else { - return RTI_FALSE; - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - if (!ConnextStaticRawDataPlugin_instance_to_keyhash( - endpoint_data, keyhash, sample)) { - return RTI_FALSE; - } - - return RTI_TRUE; -} - -/* ------------------------------------------------------------------------ -* Plug-in Installation Methods -* ------------------------------------------------------------------------ */ -struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) -{ - return NULL; -} - -struct PRESTypePlugin *ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) -{ - struct PRESTypePlugin *plugin = NULL; - const struct PRESTypePluginVersion PLUGIN_VERSION = - PRES_TYPE_PLUGIN_VERSION_2_0; - - RTIOsapiHeap_allocateStructure( - &plugin, struct PRESTypePlugin); - - if (plugin == NULL) { - return NULL; - } - - plugin->version = PLUGIN_VERSION; - - /* set up parent's function pointers */ - plugin->onParticipantAttached = - (PRESTypePluginOnParticipantAttachedCallback) - ConnextStaticRawDataPlugin_on_participant_attached; - plugin->onParticipantDetached = - (PRESTypePluginOnParticipantDetachedCallback) - ConnextStaticRawDataPlugin_on_participant_detached; - plugin->onEndpointAttached = - (PRESTypePluginOnEndpointAttachedCallback) - ConnextStaticRawDataPlugin_on_endpoint_attached; - plugin->onEndpointDetached = - (PRESTypePluginOnEndpointDetachedCallback) - ConnextStaticRawDataPlugin_on_endpoint_detached; - - plugin->copySampleFnc = - (PRESTypePluginCopySampleFunction) - ConnextStaticRawDataPlugin_copy_sample; - plugin->createSampleFnc = - (PRESTypePluginCreateSampleFunction) - ConnextStaticRawDataPlugin_create_sample; - plugin->destroySampleFnc = - (PRESTypePluginDestroySampleFunction) - ConnextStaticRawDataPlugin_destroy_sample; - - plugin->serializeFnc = - (PRESTypePluginSerializeFunction) - ConnextStaticRawDataPlugin_serialize; - plugin->deserializeFnc = - (PRESTypePluginDeserializeFunction) - ConnextStaticRawDataPlugin_deserialize; - plugin->getSerializedSampleMaxSizeFnc = - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size; - plugin->getSerializedSampleMinSizeFnc = - (PRESTypePluginGetSerializedSampleMinSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_min_size; - - plugin->getSampleFnc = - (PRESTypePluginGetSampleFunction) - ConnextStaticRawDataPlugin_get_sample; - plugin->returnSampleFnc = - (PRESTypePluginReturnSampleFunction) - ConnextStaticRawDataPlugin_return_sample; - - plugin->getKeyKindFnc = - (PRESTypePluginGetKeyKindFunction) - ConnextStaticRawDataPlugin_get_key_kind; - - plugin->getSerializedKeyMaxSizeFnc = - (PRESTypePluginGetSerializedKeyMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_key_max_size; - plugin->serializeKeyFnc = - (PRESTypePluginSerializeKeyFunction) - ConnextStaticRawDataPlugin_serialize_key; - plugin->deserializeKeyFnc = - (PRESTypePluginDeserializeKeyFunction) - ConnextStaticRawDataPlugin_deserialize_key; - plugin->deserializeKeySampleFnc = - (PRESTypePluginDeserializeKeySampleFunction) - ConnextStaticRawDataPlugin_deserialize_key_sample; - - plugin-> instanceToKeyHashFnc = - (PRESTypePluginInstanceToKeyHashFunction) - ConnextStaticRawDataPlugin_instance_to_keyhash; - plugin->serializedSampleToKeyHashFnc = - (PRESTypePluginSerializedSampleToKeyHashFunction) - ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; - - plugin->getKeyFnc = - (PRESTypePluginGetKeyFunction) - ConnextStaticRawDataPlugin_get_key; - plugin->returnKeyFnc = - (PRESTypePluginReturnKeyFunction) - ConnextStaticRawDataPlugin_return_key; - - plugin->instanceToKeyFnc = - (PRESTypePluginInstanceToKeyFunction) - ConnextStaticRawDataPlugin_instance_to_key; - plugin->keyToInstanceFnc = - (PRESTypePluginKeyToInstanceFunction) - ConnextStaticRawDataPlugin_key_to_instance; - plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ - plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; - - plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; - - /* Serialized buffer */ - plugin->getBuffer = - (PRESTypePluginGetBufferFunction) - ConnextStaticRawDataPlugin_get_buffer; - plugin->returnBuffer = - (PRESTypePluginReturnBufferFunction) - ConnextStaticRawDataPlugin_return_buffer; - plugin->getSerializedSampleSizeFnc = - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size; - - plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; - - return plugin; -} - -void -ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) -{ - RTIOsapiHeap_freeStructure(plugin); -} -#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h deleted file mode 100644 index bd23b77b..00000000 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h +++ /dev/null @@ -1,343 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef connext_static_raw_dataPlugin_738563504_h -#define connext_static_raw_dataPlugin_738563504_h - -#include "connext_static_raw_data.h" - -struct RTICdrStream; - -#ifndef pres_typePlugin_h -#include "pres/pres_typePlugin.h" -#endif - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) -#endif - -extern "C" { - - /* The type used to store keys for instances of type struct - * AnotherSimple. - * - * By default, this type is struct ConnextStaticRawData - * itself. However, if for some reason this choice is not practical for your - * system (e.g. if sizeof(struct ConnextStaticRawData) - * is very large), you may redefine this typedef in terms of another type of - * your choosing. HOWEVER, if you define the KeyHolder type to be something - * other than struct AnotherSimple, the - * following restriction applies: the key of struct - * ConnextStaticRawData must consist of a - * single field of your redefined KeyHolder type and that field must be the - * first field in struct ConnextStaticRawData. - */ - typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; - - #define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample - #define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer - #define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer - - #define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey - #define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey - - #define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample - #define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample - - /* -------------------------------------------------------------------------------------- - Support functions: - * -------------------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data(void); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData *out, - const ConnextStaticRawData *in); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData *sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData *sample,RTIBool deallocate_pointers); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData *sample); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData *sample, - const char *desc, - unsigned int indent); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_key(void); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder *key); - - /* ---------------------------------------------------------------------------- - Callback functions: - * ---------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern PRESTypePluginParticipantData - ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, - const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, - void *container_plugin_context, - RTICdrTypeCode *typeCode); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data); - - NDDSUSERDllExport extern PRESTypePluginEndpointData - ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, - void *container_plugin_context); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - void *handle); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *out, - const ConnextStaticRawData *in); - - /* ---------------------------------------------------------------------------- - (De)Serialize functions: - * ------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData *sample); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData *sample, - const char * buffer, - unsigned int length); - NDDSUSERDllExport extern DDS_ReturnCode_t - ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample); - - /* -------------------------------------------------------------------------------------- - Key Management functions: - * -------------------------------------------------------------------------------------- */ - NDDSUSERDllExport extern PRESTypePluginKeyKind - ConnextStaticRawDataPlugin_get_key_kind(void); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData ** sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *key, - const ConnextStaticRawData *instance); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *instance, - const ConnextStaticRawDataKeyHolder *key); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t *keyhash, - const ConnextStaticRawData *instance); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - DDS_KeyHash_t *keyhash, - RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos); - - /* Plugin Functions */ - NDDSUSERDllExport extern struct PRESTypePlugin* - ConnextStaticRawDataPlugin_new(void); - - NDDS_USER_DLL_EXPORT extern struct PRESTypePlugin* - ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); - -} - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* connext_static_raw_dataPlugin_738563504_h */ - diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx deleted file mode 100644 index f2c9c633..00000000 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx +++ /dev/null @@ -1,176 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#include "connext_static_raw_dataSupport.h" -#include "connext_static_raw_dataPlugin.h" - -#ifndef dds_c_log_impl_h -#include "dds_c/dds_c_log_impl.h" -#endif - -/* ========================================================================= */ -/** -<> - -Defines: TData, -TDataWriter, -TDataReader, -TTypeSupport - -Configure and implement 'ConnextStaticRawData' support classes. - -Note: Only the #defined classes get defined -*/ - -/* ----------------------------------------------------------------- */ -/* DDSDataWriter -*/ - -/** -<> - -Defines: TDataWriter, TData -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME - -/* Defines */ -#define TDataWriter ConnextStaticRawDataDataWriter -#define TData ConnextStaticRawData - -#include "dds_cpp/generic/dds_cpp_data_TDataWriter.gen" - -#undef TDataWriter -#undef TData - -#undef TTYPENAME - -/* ----------------------------------------------------------------- */ -/* DDSDataReader -*/ - -/** -<> - -Defines: TDataReader, TDataSeq, TData -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME - -/* Defines */ -#define TDataReader ConnextStaticRawDataDataReader -#define TDataSeq ConnextStaticRawDataSeq -#define TData ConnextStaticRawData - -#include "dds_cpp/generic/dds_cpp_data_TDataReader.gen" - -#undef TDataReader -#undef TDataSeq -#undef TData - -#undef TTYPENAME - -/* ----------------------------------------------------------------- */ -/* TypeSupport - -<> - -Requires: TTYPENAME, -TPlugin_new -TPlugin_delete -Defines: TTypeSupport, TData, TDataReader, TDataWriter -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME -#define TPlugin_new ConnextStaticRawDataPlugin_new -#define TPlugin_delete ConnextStaticRawDataPlugin_delete - -/* Defines */ -#define TTypeSupport ConnextStaticRawDataTypeSupport -#define TData ConnextStaticRawData -#define TDataReader ConnextStaticRawDataDataReader -#define TDataWriter ConnextStaticRawDataDataWriter -#define TGENERATE_SER_CODE -#define TGENERATE_TYPECODE - -#include "dds_cpp/generic/dds_cpp_data_TTypeSupport.gen" - -#undef TTypeSupport -#undef TData -#undef TDataReader -#undef TDataWriter -#undef TGENERATE_TYPECODE -#undef TGENERATE_SER_CODE -#undef TTYPENAME -#undef TPlugin_new -#undef TPlugin_delete - -DDS_ReturnCode_t -ConnextStaticRawDataSupport_register_external_type( - DDSDomainParticipant * participant, - const char * type_name, - struct DDS_TypeCode * type_code) -{ - DDSTypeSupport * dds_data_type = NULL; - struct PRESTypePlugin * presTypePlugin = NULL; - DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; - DDS_Boolean delete_data_type = DDS_BOOLEAN_FALSE; - RTIBool already_registered = RTI_FALSE; - - if (type_code == NULL) { - goto finError; - } - - if (participant == NULL) { - goto finError; - } - - /* TODO pass the type_code */ - presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); - if (presTypePlugin == NULL) { - goto finError; - } - - dds_data_type = new ConnextStaticRawDataTypeSupport(true); - if (dds_data_type == NULL) { - fprintf(stderr, "Error while registering external type\n"); - goto finError; - } - delete_data_type = RTI_TRUE; - - presTypePlugin->_userBuffer = (PRESWord *)dds_data_type; - already_registered = participant->is_type_registered(type_name); - - retcode = participant->register_type(type_name, presTypePlugin, NULL, !already_registered); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "error while registering external type\n"); - goto finError; - } - - if (!already_registered) { - delete_data_type = DDS_BOOLEAN_FALSE; - } - - retcode = DDS_RETCODE_OK; - -finError: - if (presTypePlugin != NULL) { - ConnextStaticRawDataPlugin_delete(presTypePlugin); - } - if (delete_data_type) { - delete (ConnextStaticRawDataTypeSupport *)dds_data_type; - dds_data_type = NULL; - } - - return retcode; -} diff --git a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h deleted file mode 100644 index 51db529a..00000000 --- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h +++ /dev/null @@ -1,155 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef connext_static_raw_dataSupport_738563504_h -#define connext_static_raw_dataSupport_738563504_h - -/* Uses */ -#include "connext_static_raw_data.h" - -#ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" -#endif - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) - -class __declspec(dllimport) DDSTypeSupport; -class __declspec(dllimport) DDSDataWriter; -class __declspec(dllimport) DDSDataReader; - -#endif - -/* ========================================================================= */ -/** -Uses: T - -Defines: TTypeSupport, TDataWriter, TDataReader - -Organized using the well-documented "Generics Pattern" for -implementing generics in C and C++. -*/ - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) - -#endif - -class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport -{ -public: - ConnextStaticRawDataTypeSupport(bool osrf) - { - (void) osrf; - } - - ~ConnextStaticRawDataTypeSupport(); - - static DDS_ReturnCode_t register_type( - DDSDomainParticipant * participant, - const char * type_name = "ConnextStaticRawData"); - - static DDS_ReturnCode_t unregister_type( - DDSDomainParticipant * participant, - const char * type_name = "ConnextStaticRawData"); - - static const char * get_type_name(); - - static ConnextStaticRawData * create_data_ex(DDS_Boolean allocatePointers); - - static ConnextStaticRawData * create_data( - const DDS_TypeAllocationParams_t & alloc_params = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); - - static DDS_ReturnCode_t delete_data_ex( - ConnextStaticRawData * a_data, - DDS_Boolean deletePointers); - - static DDS_ReturnCode_t delete_data( - ConnextStaticRawData * a_data, - const DDS_TypeDeallocationParams_t & dealloc_params = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); - - static void print_data(const ConnextStaticRawData * a_data); - - static DDS_ReturnCode_t copy_data( - ConnextStaticRawData * dst_data, const ConnextStaticRawData * src_data); - - static DDS_ReturnCode_t initialize_data_ex( - ConnextStaticRawData * a_data, - DDS_Boolean allocatePointers); - - static DDS_ReturnCode_t initialize_data( - ConnextStaticRawData * a_data, - const DDS_TypeAllocationParams_t & alloc_params = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); - - static DDS_ReturnCode_t finalize_data_ex( - ConnextStaticRawData * a_data, - DDS_Boolean deletePointers); - - static DDS_ReturnCode_t finalize_data( - ConnextStaticRawData * a_data, - const DDS_TypeDeallocationParams_t & dealloc_params = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); - - DDSDataReader * create_datareaderI(DDSDataReader * dataReader); - - DDS_ReturnCode_t destroy_datareaderI(DDSDataReader * dataReader); - - DDSDataWriter * create_datawriterI(DDSDataWriter * dataWriter); - - DDS_ReturnCode_t destroy_datawriterI(DDSDataWriter * dataWriter); - - static DDS_TypeCode * get_typecode(); - - static DDS_ReturnCode_t serialize_data_to_cdr_buffer( - char * buffer, - unsigned int & length, - const ConnextStaticRawData * a_data); - - static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( - ConnextStaticRawData * a_data, - const char * buffer, - unsigned int length); - - static DDS_ReturnCode_t data_to_string( - ConnextStaticRawData * sample, - char * str, - DDS_UnsignedLong & str_size, - const DDS_PrintFormatProperty & property); - - static void finalize(); - -private: - ConnextStaticRawDataTypeSupport(); -}; - -DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); -DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); - -NDDSUSERDllExport -DDS_ReturnCode_t -ConnextStaticRawDataSupport_register_external_type( - DDSDomainParticipant * participant, - const char * type_name, - struct DDS_TypeCode * type_code); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* connext_static_raw_dataSupport_738563504_h */ - diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.cxx b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.cxx deleted file mode 100644 index 7b954282..00000000 --- a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.cxx +++ /dev/null @@ -1,313 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef NDDS_STANDALONE_TYPE -#ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" -#endif -#ifndef dds_c_log_impl_h -#include "dds_c/dds_c_log_impl.h" -#endif - -#ifndef cdr_type_h -#include "cdr/cdr_type.h" -#endif - -#ifndef osapi_heap_h -#include "osapi/osapi_heap.h" -#endif -#else -#include "ndds_standalone_type.h" -#endif - -#include "connext_static_raw_data.h" - -#include - -/* ========================================================================= */ -const char *ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; - -DDS_TypeCode* ConnextStaticRawData_get_typecode() -{ - static RTIBool is_initialized = RTI_FALSE; - - static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array =DDS_INITIALIZE_ARRAY_TYPECODE(1,(KEY_HASH_LENGTH_16), NULL,NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX,NULL); - static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3]= - { - - { - (char *)"key_hash",/* Member name */ - { - 0,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_KEY_MEMBER , /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - }, - { - (char *)"serialized_key",/* Member name */ - { - 1,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - }, - { - (char *)"serialized_data",/* Member name */ - { - 2,/* Representation ID */ - DDS_BOOLEAN_FALSE,/* Is a pointer? */ - -1, /* Bitfield bits */ - NULL/* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER,/* Member visibility */ - 1, - NULL/* Ignored */ - } - }; - - static DDS_TypeCode ConnextStaticRawData_g_tc = - {{ - DDS_TK_STRUCT,/* Kind */ - DDS_BOOLEAN_FALSE, /* Ignored */ - -1, /*Ignored*/ - (char *)"ConnextStaticRawData", /* Name */ - NULL, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - 3, /* Number of members */ - ConnextStaticRawData_g_tc_members, /* Members */ - DDS_VM_NONE /* Ignored */ - }}; /* Type code for ConnextStaticRawData*/ - - if (is_initialized) { - return &ConnextStaticRawData_g_tc; - } - - ConnextStaticRawData_g_tc_key_hash_array._data._typeCode =(RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = (RTICdrTypeCode *)&DDS_g_tc_octet; - - ConnextStaticRawData_g_tc_members[0]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_key_hash_array; - ConnextStaticRawData_g_tc_members[1]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_key_sequence; - ConnextStaticRawData_g_tc_members[2]._representation._typeCode = (RTICdrTypeCode *)& ConnextStaticRawData_g_tc_serialized_data_sequence; - - is_initialized = RTI_TRUE; - - return &ConnextStaticRawData_g_tc; -} - -RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData* sample) { - return ConnextStaticRawData_initialize_ex(sample,RTI_TRUE,RTI_TRUE); -} - -RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData* sample,RTIBool allocatePointers, RTIBool allocateMemory) -{ - - struct DDS_TypeAllocationParams_t allocParams = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; - - allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; - allocParams.allocate_memory = (DDS_Boolean)allocateMemory; - - return ConnextStaticRawData_initialize_w_params( - sample,&allocParams); - -} - -RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData* sample, const struct DDS_TypeAllocationParams_t * allocParams) -{ - - void* buffer = NULL; - if (buffer) {} /* To avoid warnings */ - - if (sample == NULL) { - return RTI_FALSE; - } - if (allocParams == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_initArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_key ); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key , RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_key , (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_data ); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data , RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_data , (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_data, 0); - } - return RTI_TRUE; -} - -void ConnextStaticRawData_finalize( - ConnextStaticRawData* sample) -{ - - ConnextStaticRawData_finalize_ex(sample,RTI_TRUE); -} - -void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData* sample,RTIBool deletePointers) -{ - struct DDS_TypeDeallocationParams_t deallocParams = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - - if (sample==NULL) { - return; - } - - deallocParams.delete_pointers = (DDS_Boolean)deletePointers; - - ConnextStaticRawData_finalize_w_params( - sample,&deallocParams); -} - -void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData* sample,const struct DDS_TypeDeallocationParams_t * deallocParams) -{ - - if (sample==NULL) { - return; - } - - if (deallocParams == NULL) { - return; - } - - DDS_OctetSeq_finalize(&sample->serialized_key); - - DDS_OctetSeq_finalize(&sample->serialized_data); - -} - -void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* sample, RTIBool deletePointers) -{ - struct DDS_TypeDeallocationParams_t deallocParamsTmp = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - struct DDS_TypeDeallocationParams_t * deallocParams = - &deallocParamsTmp; - - if (sample==NULL) { - return; - } - if (deallocParams) {} /* To avoid warnings */ - - deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; - deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; - -} - -RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData* dst, - const ConnextStaticRawData* src) -{ - try { - - if (dst == NULL || src == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_key , - &src->serialized_key )) { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_data , - &src->serialized_data )) { - return RTI_FALSE; - } - - return RTI_TRUE; - - } catch (std::bad_alloc&) { - return RTI_FALSE; - } -} - -/** -* <> -* -* Defines: TSeq, T -* -* Configure and implement 'ConnextStaticRawData' sequence class. -*/ -#define T ConnextStaticRawData -#define TSeq ConnextStaticRawDataSeq - -#define T_initialize_w_params ConnextStaticRawData_initialize_w_params - -#define T_finalize_w_params ConnextStaticRawData_finalize_w_params -#define T_copy ConnextStaticRawData_copy - -#ifndef NDDS_STANDALONE_TYPE -#include "dds_c/generic/dds_c_sequence_TSeq.gen" -#include "dds_cpp/generic/dds_cpp_sequence_TSeq.gen" -#else -#include "dds_c_sequence_TSeq.gen" -#include "dds_cpp_sequence_TSeq.gen" -#endif - -#undef T_copy -#undef T_finalize_w_params - -#undef T_initialize_w_params - -#undef TSeq -#undef T - diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.h b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.h deleted file mode 100644 index d3adb4fa..00000000 --- a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_data.h +++ /dev/null @@ -1,106 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef connext_static_raw_data_738563504_h -#define connext_static_raw_data_738563504_h - -#ifndef NDDS_STANDALONE_TYPE -#ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" -#endif -#else -#include "ndds_standalone_type.h" -#endif - -static const DDS_Long KEY_HASH_LENGTH_16= 16; -extern "C" { - - extern const char *ConnextStaticRawDataTYPENAME; - -} - -struct ConnextStaticRawDataSeq; -#ifndef NDDS_STANDALONE_TYPE -class ConnextStaticRawDataTypeSupport; -class ConnextStaticRawDataDataWriter; -class ConnextStaticRawDataDataReader; -#endif - -class ConnextStaticRawData -{ - public: - typedef struct ConnextStaticRawDataSeq Seq; - #ifndef NDDS_STANDALONE_TYPE - typedef ConnextStaticRawDataTypeSupport TypeSupport; - typedef ConnextStaticRawDataDataWriter DataWriter; - typedef ConnextStaticRawDataDataReader DataReader; - #endif - - DDS_Octet key_hash [(KEY_HASH_LENGTH_16)]; - DDS_OctetSeq serialized_key ; - DDS_OctetSeq serialized_data ; - -}; -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) -#endif - -NDDSUSERDllExport DDS_TypeCode* ConnextStaticRawData_get_typecode(void); /* Type code */ - -DDS_SEQUENCE(ConnextStaticRawDataSeq, ConnextStaticRawData); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData* self); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData* self,RTIBool allocatePointers,RTIBool allocateMemory); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData* self, - const struct DDS_TypeAllocationParams_t * allocParams); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize( - ConnextStaticRawData* self); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData* self,RTIBool deletePointers); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData* self, - const struct DDS_TypeDeallocationParams_t * deallocParams); - -NDDSUSERDllExport -void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData* self, RTIBool deletePointers); - -NDDSUSERDllExport -RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData* dst, - const ConnextStaticRawData* src); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* connext_static_raw_data */ - diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.cxx b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.cxx deleted file mode 100644 index 78b58d54..00000000 --- a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.cxx +++ /dev/null @@ -1,1531 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#include - -#ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" -#endif - -#ifndef osapi_type_h -#include "osapi/osapi_type.h" -#endif -#ifndef osapi_heap_h -#include "osapi/osapi_heap.h" -#endif - -#ifndef osapi_utility_h -#include "osapi/osapi_utility.h" -#endif - -#ifndef cdr_type_h -#include "cdr/cdr_type.h" -#endif - -#ifndef cdr_type_object_h -#include "cdr/cdr_typeObject.h" -#endif - -#ifndef cdr_encapsulation_h -#include "cdr/cdr_encapsulation.h" -#endif - -#ifndef cdr_stream_h -#include "cdr/cdr_stream.h" -#endif - -#ifndef cdr_log_h -#include "cdr/cdr_log.h" -#endif - -#ifndef pres_typePlugin_h -#include "pres/pres_typePlugin.h" -#endif - -#define RTI_CDR_CURRENT_SUBMODULE RTI_CDR_SUBMODULE_MASK_STREAM - -#include - -#include "connext_static_raw_dataPlugin.h" - -/* ---------------------------------------------------------------------------- -* Type ConnextStaticRawData -* -------------------------------------------------------------------------- */ - -/* ----------------------------------------------------------------------------- -Support functions: -* -------------------------------------------------------------------------- */ - -ConnextStaticRawData* -ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params) -{ - ConnextStaticRawData *sample = NULL; - - sample = new (std::nothrow) ConnextStaticRawData ; - if (sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_w_params(sample,alloc_params)) { - delete sample; - sample=NULL; - } - return sample; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) -{ - ConnextStaticRawData *sample = NULL; - - sample = new (std::nothrow) ConnextStaticRawData ; - - if(sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_ex(sample,allocate_pointers, RTI_TRUE)) { - delete sample; - sample=NULL; - } - - return sample; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data(void) -{ - return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); -} - -void -ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData *sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params) { - - ConnextStaticRawData_finalize_w_params(sample,dealloc_params); - - delete sample; - sample=NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData *sample,RTIBool deallocate_pointers) { - - ConnextStaticRawData_finalize_ex(sample,deallocate_pointers); - - delete sample; - sample=NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData *sample) { - - ConnextStaticRawDataPluginSupport_destroy_data_ex(sample,RTI_TRUE); - -} - -RTIBool -ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData *dst, - const ConnextStaticRawData *src) -{ - return ConnextStaticRawData_copy(dst,(const ConnextStaticRawData*) src); -} - -void -ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData *sample, - const char *desc, - unsigned int indent_level) -{ - - RTICdrType_printIndent(indent_level); - - if (desc != NULL) { - RTILog_debug("%s:\n", desc); - } else { - RTILog_debug("\n"); - } - - if (sample == NULL) { - RTILog_debug("NULL\n"); - return; - } - - RTICdrType_printArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "key_hash", indent_level + 1); - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key ), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data ), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } - -} -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers){ - ConnextStaticRawData *key = NULL; - - key = new (std::nothrow) ConnextStaticRawDataKeyHolder ; - - ConnextStaticRawData_initialize_ex(key,allocate_pointers, RTI_TRUE); - - return key; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key(void) -{ - return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); -} - -void -ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers) -{ - ConnextStaticRawData_finalize_ex(key,deallocate_pointers); - - delete key; - key=NULL; - -} - -void -ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder *key) { - - ConnextStaticRawDataPluginSupport_destroy_key_ex(key,RTI_TRUE); - -} - -/* ---------------------------------------------------------------------------- -Callback functions: -* ---------------------------------------------------------------------------- */ - -PRESTypePluginParticipantData -ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, - const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, - void *container_plugin_context, - RTICdrTypeCode *type_code) -{ - if (registration_data) {} /* To avoid warnings */ - if (participant_info) {} /* To avoid warnings */ - if (top_level_registration) {} /* To avoid warnings */ - if (container_plugin_context) {} /* To avoid warnings */ - if (type_code) {} /* To avoid warnings */ - - return PRESTypePluginDefaultParticipantData_new(participant_info); - -} - -void -ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data) -{ - - PRESTypePluginDefaultParticipantData_delete(participant_data); -} - -PRESTypePluginEndpointData -ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, - void *containerPluginContext) -{ - PRESTypePluginEndpointData epd = NULL; - - unsigned int serializedSampleMaxSize; - - unsigned int serializedKeyMaxSize; - - if (top_level_registration) {} /* To avoid warnings */ - if (containerPluginContext) {} /* To avoid warnings */ - - epd = PRESTypePluginDefaultEndpointData_new( - participant_data, - endpoint_info, - (PRESTypePluginDefaultEndpointDataCreateSampleFunction) - ConnextStaticRawDataPluginSupport_create_data, - (PRESTypePluginDefaultEndpointDataDestroySampleFunction) - ConnextStaticRawDataPluginSupport_destroy_data, - (PRESTypePluginDefaultEndpointDataCreateKeyFunction) - ConnextStaticRawDataPluginSupport_create_key , - (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) - ConnextStaticRawDataPluginSupport_destroy_key); - - if (epd == NULL) { - return NULL; - } - serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( - epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); - - if(!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( - epd,endpoint_info,serializedKeyMaxSize)) - { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - - if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { - serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - epd,RTI_FALSE,RTI_CDR_ENCAPSULATION_ID_CDR_BE,0); - - PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); - - if (PRESTypePluginDefaultEndpointData_createWriterPool( - epd, - endpoint_info, - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size, - epd) == RTI_FALSE) { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - } - - return epd; -} - -void -ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data) -{ - - PRESTypePluginDefaultEndpointData_delete(endpoint_data); -} - -void -ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - void *handle) -{ - - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - - PRESTypePluginDefaultEndpointData_returnSample( - endpoint_data, sample, handle); -} - -RTIBool -ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *dst, - const ConnextStaticRawData *src) -{ - if (endpoint_data) {} /* To avoid warnings */ - return ConnextStaticRawDataPluginSupport_copy_data(dst,src); -} - -/* ---------------------------------------------------------------------------- -(De)Serialize functions: -* ------------------------------------------------------------------------- */ -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -RTIBool -ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void *endpoint_plugin_qos) -{ - char * position = NULL; - RTIBool retval = RTI_TRUE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(serialize_encapsulation) { - if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if(serialize_sample) { - - if (!RTICdrStream_serializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - if (!RTICdrStream_serializePrimitiveSequence( - stream, - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - (RTI_INT32_MAX-1), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } else { - if (!RTICdrStream_serializePrimitivePointerSequence( - stream, - (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - (RTI_INT32_MAX-1), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - if (!RTICdrStream_serializePrimitiveSequence( - stream, - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - (RTI_INT32_MAX-1), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } else { - if (!RTICdrStream_serializePrimitivePointerSequence( - stream, - (const void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - (RTI_INT32_MAX-1), - RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - } - - } - - if(serialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return retval; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos) -{ - - char * position = NULL; - - RTIBool done = RTI_FALSE; - - try { - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - if(deserialize_encapsulation) { - - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - if(deserialize_sample) { - - ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - goto fin; - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { - goto fin; - } - if (!DDS_OctetSeq_set_maximum(&sample->serialized_key,sequence_length)) { - return RTI_FALSE; - } - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - if (!RTICdrStream_deserializePrimitiveSequence( - stream, - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - &sequence_length, - DDS_OctetSeq_get_maximum(&sample->serialized_key), - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } else { - if (!RTICdrStream_deserializePrimitivePointerSequence( - stream, - (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - &sequence_length, - DDS_OctetSeq_get_maximum(&sample->serialized_key), - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - if (!DDS_OctetSeq_set_length(&sample->serialized_key, sequence_length)) { - return RTI_FALSE; - } - - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_lookUnsignedLong(stream,&sequence_length)) { - goto fin; - } - if (!DDS_OctetSeq_set_maximum(&sample->serialized_data,sequence_length)) { - return RTI_FALSE; - } - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - if (!RTICdrStream_deserializePrimitiveSequence( - stream, - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - &sequence_length, - DDS_OctetSeq_get_maximum(&sample->serialized_data), - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } else { - if (!RTICdrStream_deserializePrimitivePointerSequence( - stream, - (void **) DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - &sequence_length, - DDS_OctetSeq_get_maximum(&sample->serialized_data), - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - if (!DDS_OctetSeq_set_length(&sample->serialized_data, sequence_length)) { - return RTI_FALSE; - } - - } - } - - done = RTI_TRUE; - fin: - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; - - } catch (std::bad_alloc&) { - return RTI_FALSE; - } -} - -RTIBool -ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData *sample) -{ - struct RTICdrStream stream; - struct PRESTypePluginDefaultEndpointData epd; - RTIBool result; - - if (length == NULL) { - return RTI_FALSE; - } - - epd._maxSizeSerializedSample = - ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); - - if (buffer == NULL) { - *length = - ConnextStaticRawDataPlugin_get_serialized_sample_size( - (PRESTypePluginEndpointData)&epd, - RTI_TRUE, - RTICdrEncapsulation_getNativeCdrEncapsulationId(), - 0, - sample); - - if (*length == 0) { - return RTI_FALSE; - } - - return RTI_TRUE; - } - - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, *length); - - result = ConnextStaticRawDataPlugin_serialize( - (PRESTypePluginEndpointData)&epd, sample, &stream, - RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), - RTI_TRUE, NULL); - - *length = RTICdrStream_getCurrentPositionOffset(&stream); - return result; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData *sample, - const char * buffer, - unsigned int length) -{ - struct RTICdrStream stream; - - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, length); - - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - return ConnextStaticRawDataPlugin_deserialize_sample( - NULL, sample, - &stream, RTI_TRUE, RTI_TRUE, - NULL); -} - -DDS_ReturnCode_t -ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property) -{ - DDS_DynamicData *data = NULL; - char *buffer = NULL; - unsigned int length = 0; - struct DDS_PrintFormat printFormat; - DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; - - if (sample == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (str_size == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (property == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - NULL, - &length, - sample)) { - return DDS_RETCODE_ERROR; - } - - RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); - if (buffer == NULL) { - return DDS_RETCODE_ERROR; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - buffer, - &length, - sample)) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - data = DDS_DynamicData_new( - ConnextStaticRawData_get_typecode(), - &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); - if (data == NULL) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - retCode = DDS_PrintFormatProperty_to_print_format( - property, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - retCode = DDS_DynamicDataFormatter_to_string_w_format( - data, - str, - str_size, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return DDS_RETCODE_OK; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos) -{ - - RTIBool result; - const char *METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; - if (drop_sample) {} /* To avoid warnings */ - - stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_sample( - endpoint_data, (sample != NULL)?*sample:NULL, - stream, deserialize_encapsulation, deserialize_sample, - endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } - } - if (!result && stream->_xTypesState.unassignable ) { - - RTICdrLog_exception( - METHOD_NAME, - &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, - "ConnextStaticRawData"); - - } - - return result; - -} - -RTIBool ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(skip_encapsulation) { - if (!RTICdrStream_skipEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if (skip_sample) { - - if (!RTICdrStream_skipPrimitiveArray( - stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - goto fin; - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - } - - done = RTI_TRUE; - fin: - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - if(skip_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - if (endpoint_data) {} /* To avoid warnings */ - if (include_encapsulation) {} - if (encapsulation_id) {} - if (current_alignment) {} - - if (overflow != NULL) { - *overflow = RTI_TRUE; - } - - return RTI_CDR_MAX_SERIALIZED_SIZE; - -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int size; - RTIBool overflow = RTI_FALSE; - - size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); - - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } - - return size; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - - if (endpoint_data) {} /* To avoid warnings */ - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - } - - current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment,0, RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment,0, RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -/* Returns the size of the sample in its serialized form (in bytes). -* It can also be an estimation in excess of the real buffer needed -* during a call to the serialize() function. -* The value reported does not have to include the space for the -* encapsulation flags. -*/ -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - struct PRESTypePluginDefaultEndpointData epd; - - if (sample==NULL) { - return 0; - } - if (endpoint_data == NULL) { - endpoint_data = (PRESTypePluginEndpointData) &epd; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -/* -------------------------------------------------------------------------------------- -Key Management functions: -* -------------------------------------------------------------------------------------- */ - -PRESTypePluginKeyKind -ConnextStaticRawDataPlugin_get_key_kind(void) -{ - return PRES_TYPEPLUGIN_USER_KEY; -} - -RTIBool -ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(serialize_encapsulation) { - if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream , encapsulation_id)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if(serialize_key) { - - if (!RTICdrStream_serializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - } - - if(serialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - try { - - char * position = NULL; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if(deserialize_encapsulation) { - - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - if (deserialize_key) { - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; - - } catch (std::bad_alloc&) { - return RTI_FALSE; - } -} - -RTIBool ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - RTIBool result; - if (drop_sample) {} /* To avoid warnings */ - stream->_xTypesState.unassignable = RTI_FALSE; - result= ConnextStaticRawDataPlugin_deserialize_key_sample( - endpoint_data, (sample != NULL)?*sample:NULL, stream, - deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } - } - - return result; - -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - - if (endpoint_data) {} /* To avoid warnings */ - if (overflow) {} /* To avoid warnings */ - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - } - - current_alignment +=RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int size; - RTIBool overflow = RTI_FALSE; - - size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - endpoint_data,&overflow,include_encapsulation,encapsulation_id,current_alignment); - - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } - - return size; -} - -RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - if(deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - position = RTICdrStream_resetAlignment(stream); - } - - if (deserialize_key) { - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)){ - goto fin; - } - } - - } - - done = RTI_TRUE; - fin: - if(!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - } else { - return RTI_FALSE; - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *dst, - const ConnextStaticRawData *src) -{ - - if (endpoint_data) {} /* To avoid warnings */ - - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *dst, const - ConnextStaticRawDataKeyHolder *src) -{ - - if (endpoint_data) {} /* To avoid warnings */ - if (!RTICdrType_copyArray( - dst->key_hash ,src->key_hash,((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) { - return RTI_FALSE; - } - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t *keyhash, - const ConnextStaticRawData *instance) -{ - struct RTICdrStream * md5Stream = NULL; - struct RTICdrStreamState cdrState; - char * buffer = NULL; - - RTICdrStreamState_init(&cdrState); - md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); - - if (md5Stream == NULL) { - return RTI_FALSE; - } - - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - int size; - - RTICdrStream_pushState(md5Stream, &cdrState, -1); - - size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( - endpoint_data, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - 0, - instance); - - if (size <= RTICdrStream_getBufferLength(md5Stream)) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTIOsapiHeap_allocateBuffer(&buffer,size,0); - - if (buffer == NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTICdrStream_set(md5Stream, buffer, size); - RTIOsapiMemory_zero( - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getBufferLength(md5Stream)); - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - return RTI_FALSE; - } - } - - if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > - (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || - PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) { - RTICdrStream_computeMD5(md5Stream, keyhash->value); - } else { - RTIOsapiMemory_zero(keyhash->value,MIG_RTPS_KEY_HASH_MAX_LENGTH); - RTIOsapiMemory_copy( - keyhash->value, - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getCurrentPositionOffset(md5Stream)); - } - - keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; - - if (buffer != NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - } - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - DDS_KeyHash_t *keyhash, - RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - ConnextStaticRawData * sample=NULL; - - if (endpoint_plugin_qos) {} /* To avoid warnings */ - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - - if(deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - sample = (ConnextStaticRawData *) - PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); - - if (sample == NULL) { - return RTI_FALSE; - } - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { - return RTI_FALSE; - } - - done = RTI_TRUE; - fin: - if(!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) { - return RTI_FALSE; - } - } else { - return RTI_FALSE; - } - - if(deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream,position); - } - - if (!ConnextStaticRawDataPlugin_instance_to_keyhash( - endpoint_data, keyhash, sample)) { - return RTI_FALSE; - } - - return RTI_TRUE; -} - -/* ------------------------------------------------------------------------ -* Plug-in Installation Methods -* ------------------------------------------------------------------------ */ -struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) -{ - struct PRESTypePlugin *plugin = NULL; - const struct PRESTypePluginVersion PLUGIN_VERSION = - PRES_TYPE_PLUGIN_VERSION_2_0; - - RTIOsapiHeap_allocateStructure( - &plugin, struct PRESTypePlugin); - - if (plugin == NULL) { - return NULL; - } - - plugin->version = PLUGIN_VERSION; - - /* set up parent's function pointers */ - plugin->onParticipantAttached = - (PRESTypePluginOnParticipantAttachedCallback) - ConnextStaticRawDataPlugin_on_participant_attached; - plugin->onParticipantDetached = - (PRESTypePluginOnParticipantDetachedCallback) - ConnextStaticRawDataPlugin_on_participant_detached; - plugin->onEndpointAttached = - (PRESTypePluginOnEndpointAttachedCallback) - ConnextStaticRawDataPlugin_on_endpoint_attached; - plugin->onEndpointDetached = - (PRESTypePluginOnEndpointDetachedCallback) - ConnextStaticRawDataPlugin_on_endpoint_detached; - - plugin->copySampleFnc = - (PRESTypePluginCopySampleFunction) - ConnextStaticRawDataPlugin_copy_sample; - plugin->createSampleFnc = - (PRESTypePluginCreateSampleFunction) - ConnextStaticRawDataPlugin_create_sample; - plugin->destroySampleFnc = - (PRESTypePluginDestroySampleFunction) - ConnextStaticRawDataPlugin_destroy_sample; - - plugin->serializeFnc = - (PRESTypePluginSerializeFunction) - ConnextStaticRawDataPlugin_serialize; - plugin->deserializeFnc = - (PRESTypePluginDeserializeFunction) - ConnextStaticRawDataPlugin_deserialize; - plugin->getSerializedSampleMaxSizeFnc = - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size; - plugin->getSerializedSampleMinSizeFnc = - (PRESTypePluginGetSerializedSampleMinSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_min_size; - - plugin->getSampleFnc = - (PRESTypePluginGetSampleFunction) - ConnextStaticRawDataPlugin_get_sample; - plugin->returnSampleFnc = - (PRESTypePluginReturnSampleFunction) - ConnextStaticRawDataPlugin_return_sample; - - plugin->getKeyKindFnc = - (PRESTypePluginGetKeyKindFunction) - ConnextStaticRawDataPlugin_get_key_kind; - - plugin->getSerializedKeyMaxSizeFnc = - (PRESTypePluginGetSerializedKeyMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_key_max_size; - plugin->serializeKeyFnc = - (PRESTypePluginSerializeKeyFunction) - ConnextStaticRawDataPlugin_serialize_key; - plugin->deserializeKeyFnc = - (PRESTypePluginDeserializeKeyFunction) - ConnextStaticRawDataPlugin_deserialize_key; - plugin->deserializeKeySampleFnc = - (PRESTypePluginDeserializeKeySampleFunction) - ConnextStaticRawDataPlugin_deserialize_key_sample; - - plugin-> instanceToKeyHashFnc = - (PRESTypePluginInstanceToKeyHashFunction) - ConnextStaticRawDataPlugin_instance_to_keyhash; - plugin->serializedSampleToKeyHashFnc = - (PRESTypePluginSerializedSampleToKeyHashFunction) - ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; - - plugin->getKeyFnc = - (PRESTypePluginGetKeyFunction) - ConnextStaticRawDataPlugin_get_key; - plugin->returnKeyFnc = - (PRESTypePluginReturnKeyFunction) - ConnextStaticRawDataPlugin_return_key; - - plugin->instanceToKeyFnc = - (PRESTypePluginInstanceToKeyFunction) - ConnextStaticRawDataPlugin_instance_to_key; - plugin->keyToInstanceFnc = - (PRESTypePluginKeyToInstanceFunction) - ConnextStaticRawDataPlugin_key_to_instance; - plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ - plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); - - plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; - - /* Serialized buffer */ - plugin->getBuffer = - (PRESTypePluginGetBufferFunction) - ConnextStaticRawDataPlugin_get_buffer; - plugin->returnBuffer = - (PRESTypePluginReturnBufferFunction) - ConnextStaticRawDataPlugin_return_buffer; - plugin->getSerializedSampleSizeFnc = - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size; - - plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; - - return plugin; -} - -void -ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *plugin) -{ - RTIOsapiHeap_freeStructure(plugin); -} -#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.h b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.h deleted file mode 100644 index 89b4c67b..00000000 --- a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataPlugin.h +++ /dev/null @@ -1,340 +0,0 @@ - - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef connext_static_raw_dataPlugin_738563504_h -#define connext_static_raw_dataPlugin_738563504_h - -#include "connext_static_raw_data.h" - -struct RTICdrStream; - -#ifndef pres_typePlugin_h -#include "pres/pres_typePlugin.h" -#endif - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) -#endif - -extern "C" { - - /* The type used to store keys for instances of type struct - * AnotherSimple. - * - * By default, this type is struct ConnextStaticRawData - * itself. However, if for some reason this choice is not practical for your - * system (e.g. if sizeof(struct ConnextStaticRawData) - * is very large), you may redefine this typedef in terms of another type of - * your choosing. HOWEVER, if you define the KeyHolder type to be something - * other than struct AnotherSimple, the - * following restriction applies: the key of struct - * ConnextStaticRawData must consist of a - * single field of your redefined KeyHolder type and that field must be the - * first field in struct ConnextStaticRawData. - */ - typedef class ConnextStaticRawData ConnextStaticRawDataKeyHolder; - - #define ConnextStaticRawDataPlugin_get_sample PRESTypePluginDefaultEndpointData_getSample - #define ConnextStaticRawDataPlugin_get_buffer PRESTypePluginDefaultEndpointData_getBuffer - #define ConnextStaticRawDataPlugin_return_buffer PRESTypePluginDefaultEndpointData_returnBuffer - - #define ConnextStaticRawDataPlugin_get_key PRESTypePluginDefaultEndpointData_getKey - #define ConnextStaticRawDataPlugin_return_key PRESTypePluginDefaultEndpointData_returnKey - - #define ConnextStaticRawDataPlugin_create_sample PRESTypePluginDefaultEndpointData_createSample - #define ConnextStaticRawDataPlugin_destroy_sample PRESTypePluginDefaultEndpointData_deleteSample - - /* -------------------------------------------------------------------------------------- - Support functions: - * -------------------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_data(void); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData *out, - const ConnextStaticRawData *in); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData *sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData *sample,RTIBool deallocate_pointers); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData *sample); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData *sample, - const char *desc, - unsigned int indent); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers); - - NDDSUSERDllExport extern ConnextStaticRawData* - ConnextStaticRawDataPluginSupport_create_key(void); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder *key,RTIBool deallocate_pointers); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder *key); - - /* ---------------------------------------------------------------------------- - Callback functions: - * ---------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern PRESTypePluginParticipantData - ConnextStaticRawDataPlugin_on_participant_attached( - void *registration_data, - const struct PRESTypePluginParticipantInfo *participant_info, - RTIBool top_level_registration, - void *container_plugin_context, - RTICdrTypeCode *typeCode); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data); - - NDDSUSERDllExport extern PRESTypePluginEndpointData - ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo *endpoint_info, - RTIBool top_level_registration, - void *container_plugin_context); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - void *handle); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *out, - const ConnextStaticRawData *in); - - /* ---------------------------------------------------------------------------- - (De)Serialize functions: - * ------------------------------------------------------------------------- */ - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData *sample); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData **sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData *sample, - const char * buffer, - unsigned int length); - NDDSUSERDllExport extern DDS_ReturnCode_t - ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData *sample, - char *str, - DDS_UnsignedLong *str_size, - const struct DDS_PrintFormatProperty *property); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample); - - /* -------------------------------------------------------------------------------------- - Key Management functions: - * -------------------------------------------------------------------------------------- */ - NDDSUSERDllExport extern PRESTypePluginKeyKind - ConnextStaticRawDataPlugin_get_key_kind(void); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern unsigned int - ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData ** sample, - RTIBool * drop_sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *sample, - struct RTICdrStream *stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void *endpoint_plugin_qos); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder *key, - const ConnextStaticRawData *instance); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData *instance, - const ConnextStaticRawDataKeyHolder *key); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t *keyhash, - const ConnextStaticRawData *instance); - - NDDSUSERDllExport extern RTIBool - ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream *stream, - DDS_KeyHash_t *keyhash, - RTIBool deserialize_encapsulation, - void *endpoint_plugin_qos); - - /* Plugin Functions */ - NDDSUSERDllExport extern struct PRESTypePlugin* - ConnextStaticRawDataPlugin_new(void); - - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); - -} - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* connext_static_raw_dataPlugin_738563504_h */ - diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.cxx b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.cxx deleted file mode 100644 index 83be31bb..00000000 --- a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.cxx +++ /dev/null @@ -1,117 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#include "connext_static_raw_dataSupport.h" -#include "connext_static_raw_dataPlugin.h" - -#ifndef dds_c_log_impl_h -#include "dds_c/dds_c_log_impl.h" -#endif - -/* ========================================================================= */ -/** -<> - -Defines: TData, -TDataWriter, -TDataReader, -TTypeSupport - -Configure and implement 'ConnextStaticRawData' support classes. - -Note: Only the #defined classes get defined -*/ - -/* ----------------------------------------------------------------- */ -/* DDSDataWriter -*/ - -/** -<> - -Defines: TDataWriter, TData -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME - -/* Defines */ -#define TDataWriter ConnextStaticRawDataDataWriter -#define TData ConnextStaticRawData - -#include "dds_cpp/generic/dds_cpp_data_TDataWriter.gen" - -#undef TDataWriter -#undef TData - -#undef TTYPENAME - -/* ----------------------------------------------------------------- */ -/* DDSDataReader -*/ - -/** -<> - -Defines: TDataReader, TDataSeq, TData -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME - -/* Defines */ -#define TDataReader ConnextStaticRawDataDataReader -#define TDataSeq ConnextStaticRawDataSeq -#define TData ConnextStaticRawData - -#include "dds_cpp/generic/dds_cpp_data_TDataReader.gen" - -#undef TDataReader -#undef TDataSeq -#undef TData - -#undef TTYPENAME - -/* ----------------------------------------------------------------- */ -/* TypeSupport - -<> - -Requires: TTYPENAME, -TPlugin_new -TPlugin_delete -Defines: TTypeSupport, TData, TDataReader, TDataWriter -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME -#define TPlugin_new ConnextStaticRawDataPlugin_new -#define TPlugin_delete ConnextStaticRawDataPlugin_delete - -/* Defines */ -#define TTypeSupport ConnextStaticRawDataTypeSupport -#define TData ConnextStaticRawData -#define TDataReader ConnextStaticRawDataDataReader -#define TDataWriter ConnextStaticRawDataDataWriter -#define TGENERATE_SER_CODE -#define TGENERATE_TYPECODE - -#include "dds_cpp/generic/dds_cpp_data_TTypeSupport.gen" - -#undef TTypeSupport -#undef TData -#undef TDataReader -#undef TDataWriter -#undef TGENERATE_TYPECODE -#undef TGENERATE_SER_CODE -#undef TTYPENAME -#undef TPlugin_new -#undef TPlugin_delete - diff --git a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.h b/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.h deleted file mode 100644 index 012a1f61..00000000 --- a/rmw_connext_cpp/resources/raw_generated/connext_static_raw_dataSupport.h +++ /dev/null @@ -1,62 +0,0 @@ - -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from connext_static_raw_data.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef connext_static_raw_dataSupport_738563504_h -#define connext_static_raw_dataSupport_738563504_h - -/* Uses */ -#include "connext_static_raw_data.h" - -#ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" -#endif - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) - -class __declspec(dllimport) DDSTypeSupport; -class __declspec(dllimport) DDSDataWriter; -class __declspec(dllimport) DDSDataReader; - -#endif - -/* ========================================================================= */ -/** -Uses: T - -Defines: TTypeSupport, TDataWriter, TDataReader - -Organized using the well-documented "Generics Pattern" for -implementing generics in C and C++. -*/ - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, start exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport __declspec(dllexport) - -#endif - -DDS_TYPESUPPORT_CPP( - ConnextStaticRawDataTypeSupport, - ConnextStaticRawData); - -DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); -DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); - -#if (defined(RTI_WIN32) || defined (RTI_WINCE)) && defined(NDDS_USER_DLL_EXPORT) -/* If the code is building on Windows, stop exporting symbols. -*/ -#undef NDDSUSERDllExport -#define NDDSUSERDllExport -#endif - -#endif /* connext_static_raw_dataSupport_738563504_h */ - diff --git a/rmw_connext_cpp/src/connext_static_raw_data.cpp b/rmw_connext_cpp/src/connext_static_raw_data.cpp deleted file mode 100644 index 51013d2b..00000000 --- a/rmw_connext_cpp/src/connext_static_raw_data.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#ifndef NDDS_STANDALONE_TYPE -#ifndef ndds_cpp_h -#include "rmw_connext_shared_cpp/ndds_include.hpp" -#endif -#ifndef dds_c_log_impl_h -#include "dds_c/dds_c_log_impl.h" -#endif - -#ifndef cdr_type_h -#include "cdr/cdr_type.h" -#endif - -#include "osapi/osapi_heap.h" -#endif - -#include "rmw_connext_cpp/connext_static_raw_data.hpp" - -#include - -/* ========================================================================= */ -const char * ConnextStaticRawDataTYPENAME = "ConnextStaticRawData"; - -DDS_TypeCode * ConnextStaticRawData_get_typecode() -{ - static RTIBool is_initialized = RTI_FALSE; - - static DDS_TypeCode ConnextStaticRawData_g_tc_key_hash_array = - DDS_INITIALIZE_ARRAY_TYPECODE(1, (KEY_HASH_LENGTH_16), NULL, NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_key_sequence = - DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX, NULL); - static DDS_TypeCode ConnextStaticRawData_g_tc_serialized_data_sequence = - DDS_INITIALIZE_SEQUENCE_TYPECODE(RTI_INT32_MAX, NULL); - static DDS_TypeCode_Member ConnextStaticRawData_g_tc_members[3] = - { - { - (char *)"key_hash", /* Member name */ // NOLINT - { - 0, /* Representation ID */ - DDS_BOOLEAN_FALSE, /* Is a pointer? */ - -1, /* Bitfield bits */ - NULL /* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_KEY_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER, /* Member visibility */ - 1, - NULL /* Ignored */ - }, - { - (char *)"serialized_key", /* Member name */ // NOLINT - { - 1, /* Representation ID */ - DDS_BOOLEAN_FALSE, /* Is a pointer? */ - -1, /* Bitfield bits */ - NULL /* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER, /* Member visibility */ - 1, - NULL /* Ignored */ - }, - { - (char *)"serialized_data", /* Member name */ // NOLINT - { - 2, /* Representation ID */ - DDS_BOOLEAN_FALSE, /* Is a pointer? */ - -1, /* Bitfield bits */ - NULL /* Member type code is assigned later */ - }, - 0, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - RTI_CDR_REQUIRED_MEMBER, /* Is a key? */ - DDS_PUBLIC_MEMBER, /* Member visibility */ - 1, - NULL /* Ignored */ - } - }; - - static DDS_TypeCode ConnextStaticRawData_g_tc = - {{ - DDS_TK_STRUCT, /* Kind */ - DDS_BOOLEAN_FALSE, /* Ignored */ - -1, /*Ignored*/ - (char *)"ConnextStaticRawData", /* Name */ // NOLINT - NULL, /* Ignored */ - 0, /* Ignored */ - 0, /* Ignored */ - NULL, /* Ignored */ - 3, /* Number of members */ - ConnextStaticRawData_g_tc_members, /* Members */ - DDS_VM_NONE /* Ignored */ - }}; /* Type code for ConnextStaticRawData*/ - - if (is_initialized) { - return &ConnextStaticRawData_g_tc; - } - - ConnextStaticRawData_g_tc_key_hash_array._data._typeCode = - (RTICdrTypeCode *)&DDS_g_tc_octet; // NOLINT - - ConnextStaticRawData_g_tc_serialized_key_sequence._data._typeCode = - (RTICdrTypeCode *)&DDS_g_tc_octet; // NOLINT - - ConnextStaticRawData_g_tc_serialized_data_sequence._data._typeCode = - (RTICdrTypeCode *)&DDS_g_tc_octet; // NOLINT - - ConnextStaticRawData_g_tc_members[0]._representation._typeCode = - (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_key_hash_array; // NOLINT - ConnextStaticRawData_g_tc_members[1]._representation._typeCode = - (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_serialized_key_sequence; // NOLINT - ConnextStaticRawData_g_tc_members[2]._representation._typeCode = - (RTICdrTypeCode *)&ConnextStaticRawData_g_tc_serialized_data_sequence; // NOLINT - - is_initialized = RTI_TRUE; - - return &ConnextStaticRawData_g_tc; -} - -RTIBool ConnextStaticRawData_initialize( - ConnextStaticRawData * sample) -{ - return ConnextStaticRawData_initialize_ex(sample, RTI_TRUE, RTI_TRUE); -} - -RTIBool ConnextStaticRawData_initialize_ex( - ConnextStaticRawData * sample, RTIBool allocatePointers, RTIBool allocateMemory) -{ - struct DDS_TypeAllocationParams_t allocParams = - DDS_TYPE_ALLOCATION_PARAMS_DEFAULT; - - allocParams.allocate_pointers = (DDS_Boolean)allocatePointers; - allocParams.allocate_memory = (DDS_Boolean)allocateMemory; - - return ConnextStaticRawData_initialize_w_params( - sample, &allocParams); -} - -RTIBool ConnextStaticRawData_initialize_w_params( - ConnextStaticRawData * sample, const struct DDS_TypeAllocationParams_t * allocParams) -{ - void * buffer = NULL; - if (buffer) {} /* To avoid warnings */ - - if (sample == NULL) { - return RTI_FALSE; - } - if (allocParams == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_initArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) - { - return RTI_FALSE; - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_key); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_key, RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_key, (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - } - if (allocParams->allocate_memory) { - DDS_OctetSeq_initialize(&sample->serialized_data); - DDS_OctetSeq_set_absolute_maximum(&sample->serialized_data, RTI_INT32_MAX); - if (!DDS_OctetSeq_set_maximum(&sample->serialized_data, (0))) { - return RTI_FALSE; - } - } else { - DDS_OctetSeq_set_length(&sample->serialized_data, 0); - } - return RTI_TRUE; -} - -void ConnextStaticRawData_finalize( - ConnextStaticRawData * sample) -{ - ConnextStaticRawData_finalize_ex(sample, RTI_TRUE); -} - -void ConnextStaticRawData_finalize_ex( - ConnextStaticRawData * sample, RTIBool deletePointers) -{ - struct DDS_TypeDeallocationParams_t deallocParams = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - - if (sample == NULL) { - return; - } - - deallocParams.delete_pointers = (DDS_Boolean)deletePointers; - - ConnextStaticRawData_finalize_w_params( - sample, &deallocParams); -} - -void ConnextStaticRawData_finalize_w_params( - ConnextStaticRawData * sample, const struct DDS_TypeDeallocationParams_t * deallocParams) -{ - if (sample == NULL) { - return; - } - - if (deallocParams == NULL) { - return; - } - - DDS_OctetSeq_finalize(&sample->serialized_key); - - DDS_OctetSeq_finalize(&sample->serialized_data); -} - -void ConnextStaticRawData_finalize_optional_members( - ConnextStaticRawData * sample, RTIBool deletePointers) -{ - struct DDS_TypeDeallocationParams_t deallocParamsTmp = - DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT; - struct DDS_TypeDeallocationParams_t * deallocParams = - &deallocParamsTmp; - - if (sample == NULL) { - return; - } - if (deallocParams) {} /* To avoid warnings */ - - deallocParamsTmp.delete_pointers = (DDS_Boolean)deletePointers; - deallocParamsTmp.delete_optional_members = DDS_BOOLEAN_TRUE; -} - -RTIBool ConnextStaticRawData_copy( - ConnextStaticRawData * dst, - const ConnextStaticRawData * src) -{ - try { - if (dst == NULL || src == NULL) { - return RTI_FALSE; - } - - if (!RTICdrType_copyArray( - dst->key_hash, src->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) - { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_key, - &src->serialized_key)) - { - return RTI_FALSE; - } - if (!DDS_OctetSeq_copy(&dst->serialized_data, - &src->serialized_data)) - { - return RTI_FALSE; - } - - return RTI_TRUE; - - } catch (std::bad_alloc &) { - return RTI_FALSE; - } -} - -/** -* <> -* -* Defines: TSeq, T -* -* Configure and implement 'ConnextStaticRawData' sequence class. -*/ -#define T ConnextStaticRawData -#define TSeq ConnextStaticRawDataSeq - -#define T_initialize_w_params ConnextStaticRawData_initialize_w_params - -#define T_finalize_w_params ConnextStaticRawData_finalize_w_params -#define T_copy ConnextStaticRawData_copy - -#ifndef NDDS_STANDALONE_TYPE -#include "dds_c/generic/dds_c_sequence_TSeq.gen" -#include "dds_cpp/generic/dds_cpp_sequence_TSeq.gen" -#else -#include "dds_c_sequence_TSeq.gen" -#include "dds_cpp_sequence_TSeq.gen" -#endif - -#undef T_copy -#undef T_finalize_w_params - -#undef T_initialize_w_params - -#undef TSeq -#undef T diff --git a/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp b/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp deleted file mode 100644 index 9d8f2e8d..00000000 --- a/rmw_connext_cpp/src/connext_static_raw_data_plugin.cpp +++ /dev/null @@ -1,1490 +0,0 @@ -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#include - -#ifndef ndds_cpp_h -#include "rmw_connext_shared_cpp/ndds_include.hpp" -#endif - -#ifndef osapi_type_h -#include "osapi/osapi_type.h" -#endif -#ifndef osapi_heap_h -#include "osapi/osapi_heap.h" -#endif - -#ifndef osapi_utility_h -#include "osapi/osapi_utility.h" -#endif - -#ifndef cdr_type_h -#include "cdr/cdr_type.h" -#endif - -#ifndef cdr_type_object_h -#include "cdr/cdr_typeObject.h" -#endif - -#ifndef cdr_encapsulation_h -#include "cdr/cdr_encapsulation.h" -#endif - -#ifndef cdr_stream_h -#include "cdr/cdr_stream.h" -#endif - -#ifndef cdr_log_h -#include "cdr/cdr_log.h" -#endif - -#ifndef pres_typePlugin_h -#include "pres/pres_typePlugin.h" -#endif - -#define RTI_CDR_CURRENT_SUBMODULE RTI_CDR_SUBMODULE_MASK_STREAM - -#include - -#include "rmw_connext_cpp/connext_static_raw_data_plugin.hpp" - -/* ---------------------------------------------------------------------------- -* Type ConnextStaticRawData -* -------------------------------------------------------------------------- */ - -/* ----------------------------------------------------------------------------- -Support functions: -* -------------------------------------------------------------------------- */ - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data_w_params( - const struct DDS_TypeAllocationParams_t * alloc_params) -{ - ConnextStaticRawData * sample = NULL; - - sample = new (std::nothrow) ConnextStaticRawData; - if (sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_w_params(sample, alloc_params)) { - delete sample; - sample = NULL; - } - return sample; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data_ex(RTIBool allocate_pointers) -{ - ConnextStaticRawData * sample = NULL; - - sample = new (std::nothrow) ConnextStaticRawData; - - if (sample == NULL) { - return NULL; - } - - if (!ConnextStaticRawData_initialize_ex(sample, allocate_pointers, RTI_TRUE)) { - delete sample; - sample = NULL; - } - - return sample; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_data(void) -{ - return ConnextStaticRawDataPluginSupport_create_data_ex(RTI_TRUE); -} - -void -ConnextStaticRawDataPluginSupport_destroy_data_w_params( - ConnextStaticRawData * sample, - const struct DDS_TypeDeallocationParams_t * dealloc_params) -{ - ConnextStaticRawData_finalize_w_params(sample, dealloc_params); - - delete sample; - sample = NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_data_ex( - ConnextStaticRawData * sample, RTIBool deallocate_pointers) -{ - ConnextStaticRawData_finalize_ex(sample, deallocate_pointers); - - delete sample; - sample = NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_data( - ConnextStaticRawData * sample) -{ - ConnextStaticRawDataPluginSupport_destroy_data_ex(sample, RTI_TRUE); -} - -RTIBool -ConnextStaticRawDataPluginSupport_copy_data( - ConnextStaticRawData * dst, - const ConnextStaticRawData * src) -{ - return ConnextStaticRawData_copy(dst, (const ConnextStaticRawData *) src); -} - -void -ConnextStaticRawDataPluginSupport_print_data( - const ConnextStaticRawData * sample, - const char * desc, - unsigned int indent_level) -{ - RTICdrType_printIndent(indent_level); - - if (desc != NULL) { - RTILog_debug("%s:\n", desc); - } else { - RTILog_debug("\n"); - } - - if (sample == NULL) { - RTILog_debug("NULL\n"); - return; - } - - RTICdrType_printArray( - sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "key_hash", indent_level + 1); - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_key), - DDS_OctetSeq_get_length(&sample->serialized_key), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_key", indent_level + 1); - } - - if (DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data) != NULL) { - RTICdrType_printArray( - DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_SIZE, - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } else { - RTICdrType_printPointerArray( - DDS_OctetSeq_get_discontiguous_bufferI(&sample->serialized_data), - DDS_OctetSeq_get_length(&sample->serialized_data), - (RTICdrTypePrintFunction)RTICdrType_printOctet, - "serialized_data", indent_level + 1); - } -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key_ex(RTIBool allocate_pointers) -{ - ConnextStaticRawData * key = NULL; - - key = new (std::nothrow) ConnextStaticRawDataKeyHolder; - - ConnextStaticRawData_initialize_ex(key, allocate_pointers, RTI_TRUE); - - return key; -} - -ConnextStaticRawData * -ConnextStaticRawDataPluginSupport_create_key(void) -{ - return ConnextStaticRawDataPluginSupport_create_key_ex(RTI_TRUE); -} - -void -ConnextStaticRawDataPluginSupport_destroy_key_ex( - ConnextStaticRawDataKeyHolder * key, RTIBool deallocate_pointers) -{ - ConnextStaticRawData_finalize_ex(key, deallocate_pointers); - - delete key; - key = NULL; -} - -void -ConnextStaticRawDataPluginSupport_destroy_key( - ConnextStaticRawDataKeyHolder * key) -{ - ConnextStaticRawDataPluginSupport_destroy_key_ex(key, RTI_TRUE); -} - -/* ---------------------------------------------------------------------------- -Callback functions: -* ---------------------------------------------------------------------------- */ - -PRESTypePluginParticipantData -ConnextStaticRawDataPlugin_on_participant_attached( - void * registration_data, - const struct PRESTypePluginParticipantInfo * participant_info, - RTIBool top_level_registration, - void * container_plugin_context, - RTICdrTypeCode * type_code) -{ - if (registration_data) {} /* To avoid warnings */ - if (participant_info) {} /* To avoid warnings */ - if (top_level_registration) {} /* To avoid warnings */ - if (container_plugin_context) {} /* To avoid warnings */ - if (type_code) {} /* To avoid warnings */ - - return PRESTypePluginDefaultParticipantData_new(participant_info); -} - -void -ConnextStaticRawDataPlugin_on_participant_detached( - PRESTypePluginParticipantData participant_data) -{ - PRESTypePluginDefaultParticipantData_delete(participant_data); -} - -PRESTypePluginEndpointData -ConnextStaticRawDataPlugin_on_endpoint_attached( - PRESTypePluginParticipantData participant_data, - const struct PRESTypePluginEndpointInfo * endpoint_info, - RTIBool top_level_registration, - void * containerPluginContext) -{ - PRESTypePluginEndpointData epd = NULL; - - unsigned int serializedSampleMaxSize; - - unsigned int serializedKeyMaxSize; - - if (top_level_registration) {} /* To avoid warnings */ - if (containerPluginContext) {} /* To avoid warnings */ - - epd = PRESTypePluginDefaultEndpointData_new( - participant_data, - endpoint_info, - (PRESTypePluginDefaultEndpointDataCreateSampleFunction) - ConnextStaticRawDataPluginSupport_create_data, - (PRESTypePluginDefaultEndpointDataDestroySampleFunction) - ConnextStaticRawDataPluginSupport_destroy_data, - (PRESTypePluginDefaultEndpointDataCreateKeyFunction) - ConnextStaticRawDataPluginSupport_create_key, - (PRESTypePluginDefaultEndpointDataDestroyKeyFunction) - ConnextStaticRawDataPluginSupport_destroy_key); - - if (epd == NULL) { - return NULL; - } - serializedKeyMaxSize = ConnextStaticRawDataPlugin_get_serialized_key_max_size( - epd, RTI_FALSE, RTI_CDR_ENCAPSULATION_ID_CDR_BE, 0); - - if (!PRESTypePluginDefaultEndpointData_createMD5StreamWithInfo( - epd, endpoint_info, serializedKeyMaxSize)) - { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - - if (endpoint_info->endpointKind == PRES_TYPEPLUGIN_ENDPOINT_WRITER) { - serializedSampleMaxSize = ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - epd, RTI_FALSE, RTI_CDR_ENCAPSULATION_ID_CDR_BE, 0); - - PRESTypePluginDefaultEndpointData_setMaxSizeSerializedSample(epd, serializedSampleMaxSize); - - if (PRESTypePluginDefaultEndpointData_createWriterPool( - epd, - endpoint_info, - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size, epd, - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size, - epd) == RTI_FALSE) - { - PRESTypePluginDefaultEndpointData_delete(epd); - return NULL; - } - } - - return epd; -} - -void -ConnextStaticRawDataPlugin_on_endpoint_detached( - PRESTypePluginEndpointData endpoint_data) -{ - PRESTypePluginDefaultEndpointData_delete(endpoint_data); -} - -void -ConnextStaticRawDataPlugin_return_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - void * handle) -{ - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - - PRESTypePluginDefaultEndpointData_returnSample( - endpoint_data, sample, handle); -} - -RTIBool -ConnextStaticRawDataPlugin_copy_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * dst, - const ConnextStaticRawData * src) -{ - if (endpoint_data) {} /* To avoid warnings */ - return ConnextStaticRawDataPluginSupport_copy_data(dst, src); -} - -/* ---------------------------------------------------------------------------- -(De)Serialize functions: -* ------------------------------------------------------------------------- */ -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment); - -RTIBool -ConnextStaticRawDataPlugin_serialize( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_sample, - void * endpoint_plugin_qos) -{ - char * position = NULL; - RTIBool retval = RTI_TRUE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - /* This plugin can only be used to publish the top-level DDS Topic-Type - * in which case serialize_encapsulation==TRUE. If that is not - * the case then it is an error. - */ - if (!serialize_encapsulation) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - - if (serialize_sample) { - /* The sample->serialized_data contains the serialized encapsulation followed by the serialized - * data, so we only need to copy that into - * the CDR stream. Not the key_hash, not the length of the data itself - * The SerializedType sample->serialized_data is always a contiguous buffer - */ - DDS_Octet * buffer = DDS_OctetSeq_get_contiguous_bufferI(&sample->serialized_data); - if (buffer == NULL) { - return RTI_FALSE; - } - - /* The encapsulation_id appears in the sample->serialized_data as octet[2] using big-endian - * byte order - */ - if (encapsulation_id != (buffer[0] * 256 + buffer[1]) ) { - return RTI_FALSE; - } - - /* Use RTICdrStream_serializePrimitiveArray so that there is no additional length prepended */ - if (!RTICdrStream_serializePrimitiveArray( - stream, (void *)buffer, - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE)) - { - return RTI_FALSE; - } - } - - RTICdrStream_restoreAlignment(stream, position); - - return retval; -} - -/** - TODO. The code-block below does not belong here. - It should be pushed to the CDR module, perhaps inside - RTICdrStream_deserializeAndSetCdrEncapsulation so that the - stream size is alredy correct when SerializedTypePlugin_deserialize_sample - is called. - - Adjust the size of the CDR stream to not include the alignment - padding. See http://issues.omg.org/browse/DDSXTY12-10 - - @precondition The RTICdrStream *stream has alreadt processed - the encapsulation header and therefore has set the - encapsulation options returned by - RTICdrStream_getEncapsulationOptions() -*/ -void -ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) -{ - /* See http://issues.omg.org/browse/DDSXTY12-10 */ - DDS_UnsignedShort padding_size_mask = 0x0003; - DDS_UnsignedShort padding_size; - int adjustedBufferLength; - - padding_size = RTICdrStream_getEncapsulationOptions(stream) & padding_size_mask; - adjustedBufferLength = RTICdrStream_getBufferLength(stream) - padding_size; - RTICdrStream_setBufferLength(stream, adjustedBufferLength); -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void * endpoint_plugin_qos) -{ - char * position = NULL; - RTIBool done = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - /* This plugin can only be used to publish the top-level DDS Topic-Type - * in which case deserialize_encapsulation==TRUE. If that is not - * the case then it is an error. - */ - if (!deserialize_encapsulation) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - - /* TODO. The call does not belong here. It should be pushed - * inside RTICdrStream_deserializeAndSetCdrEncapsulation - */ - ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); - - if (deserialize_sample) { - /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() - it is done there because SerializedTypePlugin_deserialize_sample does not - have access to the SampleInfo where that information is - */ - - /* We do not set the serialized_key on deserialization */ - DDS_OctetSeq_set_length(&sample->serialized_key, 0); - - /* We copy everything that remains in the CDR stream */ - int bytesLeftInStream = RTICdrStream_getRemainder(stream); - DDS_Octet * cdrBufferPtr = (DDS_Octet *) RTICdrStream_getCurrentPosition(stream); - if (cdrBufferPtr == NULL) { - goto fin; - } - - /* Do not call SerializedType_initialize_ex initialize here - because it would override the key_hash field - SerializedType_initialize_ex(sample, RTI_FALSE, RTI_FALSE); - */ - if (!DDS_OctetSeq_from_array(&sample->serialized_data, cdrBufferPtr, bytesLeftInStream) ) { - goto fin; - } - RTICdrStream_incrementCurrentPosition(stream, bytesLeftInStream); - } - - done = RTI_TRUE; - -fin: - if ( (done != RTI_TRUE) && - (RTICdrStream_getRemainder(stream) >= RTI_CDR_PARAMETER_HEADER_ALIGNMENT) ) - { - return RTI_FALSE; - } - - RTICdrStream_restoreAlignment(stream, position); - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - char * buffer, - unsigned int * length, - const ConnextStaticRawData * sample) -{ - struct RTICdrStream stream; - struct PRESTypePluginDefaultEndpointData epd; - RTIBool result; - - if (length == NULL) { - return RTI_FALSE; - } - - epd._maxSizeSerializedSample = - ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - NULL, RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), 0); - - if (buffer == NULL) { - *length = - ConnextStaticRawDataPlugin_get_serialized_sample_size( - (PRESTypePluginEndpointData) & epd, - RTI_TRUE, - RTICdrEncapsulation_getNativeCdrEncapsulationId(), - 0, - sample); - - if (*length == 0) { - return RTI_FALSE; - } - - return RTI_TRUE; - } - - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, *length); - - result = ConnextStaticRawDataPlugin_serialize( - (PRESTypePluginEndpointData) & epd, sample, &stream, - RTI_TRUE, RTICdrEncapsulation_getNativeCdrEncapsulationId(), - RTI_TRUE, NULL); - - *length = RTICdrStream_getCurrentPositionOffset(&stream); - return result; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize_from_cdr_buffer( - ConnextStaticRawData * sample, - const char * buffer, - unsigned int length) -{ - struct RTICdrStream stream; - - RTICdrStream_init(&stream); - RTICdrStream_set(&stream, (char *)buffer, length); - - ConnextStaticRawData_finalize_optional_members(sample, RTI_TRUE); - return ConnextStaticRawDataPlugin_deserialize_sample( - NULL, sample, - &stream, RTI_TRUE, RTI_TRUE, - NULL); -} - -DDS_ReturnCode_t -ConnextStaticRawDataPlugin_data_to_string( - const ConnextStaticRawData * sample, - char * str, - DDS_UnsignedLong * str_size, - const struct DDS_PrintFormatProperty * property) -{ - DDS_DynamicData * data = NULL; - char * buffer = NULL; - unsigned int length = 0; - struct DDS_PrintFormat printFormat; - DDS_ReturnCode_t retCode = DDS_RETCODE_ERROR; - - if (sample == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (str_size == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (property == NULL) { - return DDS_RETCODE_BAD_PARAMETER; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - NULL, - &length, - sample)) - { - return DDS_RETCODE_ERROR; - } - - RTIOsapiHeap_allocateBuffer(&buffer, length, RTI_OSAPI_ALIGNMENT_DEFAULT); - if (buffer == NULL) { - return DDS_RETCODE_ERROR; - } - - if (!ConnextStaticRawDataPlugin_serialize_to_cdr_buffer( - buffer, - &length, - sample)) - { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - data = DDS_DynamicData_new( - ConnextStaticRawData_get_typecode(), - &DDS_DYNAMIC_DATA_PROPERTY_DEFAULT); - if (data == NULL) { - RTIOsapiHeap_freeBuffer(buffer); - return DDS_RETCODE_ERROR; - } - - retCode = DDS_DynamicData_from_cdr_buffer(data, buffer, length); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - retCode = DDS_PrintFormatProperty_to_print_format( - property, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - retCode = DDS_DynamicDataFormatter_to_string_w_format( - data, - str, - str_size, - &printFormat); - if (retCode != DDS_RETCODE_OK) { - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return retCode; - } - - RTIOsapiHeap_freeBuffer(buffer); - DDS_DynamicData_delete(data); - return DDS_RETCODE_OK; -} - -RTIBool -ConnextStaticRawDataPlugin_deserialize( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData ** sample, - RTIBool * drop_sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_sample, - void * endpoint_plugin_qos) -{ - RTIBool result; - const char * METHOD_NAME = "ConnextStaticRawDataPlugin_deserialize"; - if (drop_sample) {} /* To avoid warnings */ - - stream->_xTypesState.unassignable = RTI_FALSE; - result = ConnextStaticRawDataPlugin_deserialize_sample( - endpoint_data, (sample != NULL) ? *sample : NULL, - stream, deserialize_encapsulation, deserialize_sample, - endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } - } - if (!result && stream->_xTypesState.unassignable) { - RTICdrLog_exception( - METHOD_NAME, - &RTI_CDR_LOG_UNASSIGNABLE_SAMPLE_OF_TYPE_s, - "ConnextStaticRawData"); - } - - return result; -} - -RTIBool ConnextStaticRawDataPlugin_skip( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream * stream, - RTIBool skip_encapsulation, - RTIBool skip_sample, - void * endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if (skip_encapsulation) { - if (!RTICdrStream_skipEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if (skip_sample) { - if (!RTICdrStream_skipPrimitiveArray( - stream, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) - { - goto fin; - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)) - { - goto fin; - } - } - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)) - { - goto fin; - } - } - } - - done = RTI_TRUE; -fin: - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) - { - return RTI_FALSE; - } - if (skip_encapsulation) { - RTICdrStream_restoreAlignment(stream, position); - } - - return RTI_TRUE; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - if (endpoint_data) {} /* To avoid warnings */ - if (include_encapsulation) {} - if (encapsulation_id) {} - if (current_alignment) {} - - if (overflow != NULL) { - *overflow = RTI_TRUE; - } - - return RTI_CDR_MAX_SERIALIZED_SIZE; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int size; - RTIBool overflow = RTI_FALSE; - - size = ConnextStaticRawDataPlugin_get_serialized_sample_max_size_ex( - endpoint_data, &overflow, include_encapsulation, encapsulation_id, current_alignment); - - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } - - return size; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_min_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - - if (endpoint_data) {} /* To avoid warnings */ - - if (include_encapsulation) { - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - } - - current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment, 0, RTI_CDR_OCTET_TYPE); - current_alignment += RTICdrType_getPrimitiveSequenceMaxSizeSerialized( - current_alignment, 0, RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -/* Returns the size of the sample in its serialized form (in bytes). -* It can also be an estimation in excess of the real buffer needed -* during a call to the serialize() function. -* The value reported does not have to include the space for the -* encapsulation flags. -*/ -unsigned int -ConnextStaticRawDataPlugin_get_serialized_sample_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment, - const ConnextStaticRawData * sample) -{ - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - struct PRESTypePluginDefaultEndpointData epd; - - if (sample == NULL) { - return 0; - } - if (endpoint_data == NULL) { - endpoint_data = (PRESTypePluginEndpointData) & epd; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - if (include_encapsulation) { - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - PRESTypePluginDefaultEndpointData_setBaseAlignment( - endpoint_data, - current_alignment); - } - - current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_key), - RTI_CDR_OCTET_TYPE); - - current_alignment += RTICdrType_getPrimitiveSequenceSerializedSize( - PRESTypePluginDefaultEndpointData_getAlignment( - endpoint_data, current_alignment), - DDS_OctetSeq_get_length(&sample->serialized_data), - RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -/* -------------------------------------------------------------------------------------- -Key Management functions: -* -------------------------------------------------------------------------------------- */ - -PRESTypePluginKeyKind -ConnextStaticRawDataPlugin_get_key_kind(void) -{ - return PRES_TYPEPLUGIN_NO_KEY; -} - -RTIBool -ConnextStaticRawDataPlugin_serialize_key( - PRESTypePluginEndpointData endpoint_data, - const ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool serialize_encapsulation, - RTIEncapsulationId encapsulation_id, - RTIBool serialize_key, - void * endpoint_plugin_qos) -{ - char * position = NULL; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if (serialize_encapsulation) { - if (!RTICdrStream_serializeAndSetCdrEncapsulation(stream, encapsulation_id)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - if (serialize_key) { - if (!RTICdrStream_serializePrimitiveArray( - stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) - { - return RTI_FALSE; - } - } - - if (serialize_encapsulation) { - RTICdrStream_restoreAlignment(stream, position); - } - - return RTI_TRUE; -} - -RTIBool ConnextStaticRawDataPlugin_deserialize_key_sample( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void * endpoint_plugin_qos) -{ - try { - char * position = NULL; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if (deserialize_encapsulation) { - - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - if (deserialize_key) { - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) - { - return RTI_FALSE; - } - } - - if (deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream, position); - } - - return RTI_TRUE; - - } catch (std::bad_alloc &) { - return RTI_FALSE; - } -} - -RTIBool ConnextStaticRawDataPlugin_deserialize_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData ** sample, - RTIBool * drop_sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void * endpoint_plugin_qos) -{ - RTIBool result; - if (drop_sample) {} /* To avoid warnings */ - stream->_xTypesState.unassignable = RTI_FALSE; - result = ConnextStaticRawDataPlugin_deserialize_key_sample( - endpoint_data, (sample != NULL) ? *sample : NULL, stream, - deserialize_encapsulation, deserialize_key, endpoint_plugin_qos); - if (result) { - if (stream->_xTypesState.unassignable) { - result = RTI_FALSE; - } - } - - return result; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - PRESTypePluginEndpointData endpoint_data, - RTIBool * overflow, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int initial_alignment = current_alignment; - - unsigned int encapsulation_size = current_alignment; - - if (endpoint_data) {} /* To avoid warnings */ - if (overflow) {} /* To avoid warnings */ - - if (include_encapsulation) { - - if (!RTICdrEncapsulation_validEncapsulationId(encapsulation_id)) { - return 1; - } - RTICdrStream_getEncapsulationSize(encapsulation_size); - encapsulation_size -= current_alignment; - current_alignment = 0; - initial_alignment = 0; - } - - current_alignment += RTICdrType_getPrimitiveArrayMaxSizeSerialized( - current_alignment, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE); - - if (include_encapsulation) { - current_alignment += encapsulation_size; - } - return current_alignment - initial_alignment; -} - -unsigned int -ConnextStaticRawDataPlugin_get_serialized_key_max_size( - PRESTypePluginEndpointData endpoint_data, - RTIBool include_encapsulation, - RTIEncapsulationId encapsulation_id, - unsigned int current_alignment) -{ - unsigned int size; - RTIBool overflow = RTI_FALSE; - - size = ConnextStaticRawDataPlugin_get_serialized_key_max_size_ex( - endpoint_data, &overflow, include_encapsulation, encapsulation_id, current_alignment); - - if (overflow) { - size = RTI_CDR_MAX_SERIALIZED_SIZE; - } - - return size; -} - -RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * sample, - struct RTICdrStream * stream, - RTIBool deserialize_encapsulation, - RTIBool deserialize_key, - void * endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - - if (endpoint_data) {} /* To avoid warnings */ - if (endpoint_plugin_qos) {} /* To avoid warnings */ - - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - if (deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - position = RTICdrStream_resetAlignment(stream); - } - - if (deserialize_key) { - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) - { - return RTI_FALSE; - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)) - { - goto fin; - } - } - - { - RTICdrUnsignedLong sequence_length; - if (!RTICdrStream_skipPrimitiveSequence( - stream, - &sequence_length, - RTI_CDR_OCTET_TYPE)) - { - goto fin; - } - } - - } - - done = RTI_TRUE; -fin: - if (!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) - { - return RTI_FALSE; - } - } else { - return RTI_FALSE; - } - - if (deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream, position); - } - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_instance_to_key( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawDataKeyHolder * dst, - const ConnextStaticRawData * src) -{ - if (endpoint_data) {} /* To avoid warnings */ - - if (!RTICdrType_copyArray( - dst->key_hash, src->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) - { - return RTI_FALSE; - } - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_key_to_instance( - PRESTypePluginEndpointData endpoint_data, - ConnextStaticRawData * dst, const - ConnextStaticRawDataKeyHolder * src) -{ - if (endpoint_data) {} /* To avoid warnings */ - if (!RTICdrType_copyArray( - dst->key_hash, src->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_SIZE)) - { - return RTI_FALSE; - } - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_instance_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - DDS_KeyHash_t * keyhash, - const ConnextStaticRawData * instance) -{ - struct RTICdrStream * md5Stream = NULL; - struct RTICdrStreamState cdrState; - char * buffer = NULL; - - RTICdrStreamState_init(&cdrState); - md5Stream = PRESTypePluginDefaultEndpointData_getMD5Stream(endpoint_data); - - if (md5Stream == NULL) { - return RTI_FALSE; - } - - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - int size; - - RTICdrStream_pushState(md5Stream, &cdrState, -1); - - size = (int)ConnextStaticRawDataPlugin_get_serialized_sample_size( - endpoint_data, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - 0, - instance); - - if (size <= RTICdrStream_getBufferLength(md5Stream)) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTIOsapiHeap_allocateBuffer(&buffer, size, 0); - - if (buffer == NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - return RTI_FALSE; - } - - RTICdrStream_set(md5Stream, buffer, size); - RTIOsapiMemory_zero( - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getBufferLength(md5Stream)); - RTICdrStream_resetPosition(md5Stream); - RTICdrStream_setDirtyBit(md5Stream, RTI_TRUE); - if (!ConnextStaticRawDataPlugin_serialize_key( - endpoint_data, - instance, - md5Stream, - RTI_FALSE, - RTI_CDR_ENCAPSULATION_ID_CDR_BE, - RTI_TRUE, - NULL)) - { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - return RTI_FALSE; - } - } - - if (PRESTypePluginDefaultEndpointData_getMaxSizeSerializedKey(endpoint_data) > - (unsigned int)(MIG_RTPS_KEY_HASH_MAX_LENGTH) || - PRESTypePluginDefaultEndpointData_forceMD5KeyHash(endpoint_data)) - { - RTICdrStream_computeMD5(md5Stream, keyhash->value); - } else { - RTIOsapiMemory_zero(keyhash->value, MIG_RTPS_KEY_HASH_MAX_LENGTH); - RTIOsapiMemory_copy( - keyhash->value, - RTICdrStream_getBuffer(md5Stream), - RTICdrStream_getCurrentPositionOffset(md5Stream)); - } - - keyhash->length = MIG_RTPS_KEY_HASH_MAX_LENGTH; - - if (buffer != NULL) { - RTICdrStream_popState(md5Stream, &cdrState); - RTIOsapiHeap_freeBuffer(buffer); - } - - return RTI_TRUE; -} - -RTIBool -ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( - PRESTypePluginEndpointData endpoint_data, - struct RTICdrStream * stream, - DDS_KeyHash_t * keyhash, - RTIBool deserialize_encapsulation, - void * endpoint_plugin_qos) -{ - char * position = NULL; - - RTIBool done = RTI_FALSE; - RTIBool error = RTI_FALSE; - ConnextStaticRawData * sample = NULL; - - if (endpoint_plugin_qos) {} /* To avoid warnings */ - if (stream == NULL) { - error = RTI_TRUE; - goto fin; - } - - if (deserialize_encapsulation) { - if (!RTICdrStream_deserializeAndSetCdrEncapsulation(stream)) { - return RTI_FALSE; - } - - position = RTICdrStream_resetAlignment(stream); - } - - sample = (ConnextStaticRawData *) - PRESTypePluginDefaultEndpointData_getTempSample(endpoint_data); - - if (sample == NULL) { - return RTI_FALSE; - } - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void *) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) - { - return RTI_FALSE; - } - - done = RTI_TRUE; -fin: - if (!error) { - if (done != RTI_TRUE && - RTICdrStream_getRemainder(stream) >= - RTI_CDR_PARAMETER_HEADER_ALIGNMENT) - { - return RTI_FALSE; - } - } else { - return RTI_FALSE; - } - - if (deserialize_encapsulation) { - RTICdrStream_restoreAlignment(stream, position); - } - - if (!ConnextStaticRawDataPlugin_instance_to_keyhash( - endpoint_data, keyhash, sample)) - { - return RTI_FALSE; - } - - return RTI_TRUE; -} - -/* ------------------------------------------------------------------------ -* Plug-in Installation Methods -* ------------------------------------------------------------------------ */ -struct PRESTypePlugin * ConnextStaticRawDataPlugin_new(void) -{ - return NULL; -} - -struct PRESTypePlugin * -ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) -{ - if (external_type_code == NULL) { - return NULL; - } - - struct PRESTypePlugin * plugin = NULL; - const struct PRESTypePluginVersion PLUGIN_VERSION = - PRES_TYPE_PLUGIN_VERSION_2_0; - - RTIOsapiHeap_allocateStructure( - &plugin, struct PRESTypePlugin); - - if (plugin == NULL) { - return NULL; - } - - plugin->version = PLUGIN_VERSION; - - /* set up parent's function pointers */ - plugin->onParticipantAttached = - (PRESTypePluginOnParticipantAttachedCallback) - ConnextStaticRawDataPlugin_on_participant_attached; - plugin->onParticipantDetached = - (PRESTypePluginOnParticipantDetachedCallback) - ConnextStaticRawDataPlugin_on_participant_detached; - plugin->onEndpointAttached = - (PRESTypePluginOnEndpointAttachedCallback) - ConnextStaticRawDataPlugin_on_endpoint_attached; - plugin->onEndpointDetached = - (PRESTypePluginOnEndpointDetachedCallback) - ConnextStaticRawDataPlugin_on_endpoint_detached; - - plugin->copySampleFnc = - (PRESTypePluginCopySampleFunction) - ConnextStaticRawDataPlugin_copy_sample; - plugin->createSampleFnc = - (PRESTypePluginCreateSampleFunction) - ConnextStaticRawDataPlugin_create_sample; - plugin->destroySampleFnc = - (PRESTypePluginDestroySampleFunction) - ConnextStaticRawDataPlugin_destroy_sample; - - plugin->serializeFnc = - (PRESTypePluginSerializeFunction) - ConnextStaticRawDataPlugin_serialize; - plugin->deserializeFnc = - (PRESTypePluginDeserializeFunction) - ConnextStaticRawDataPlugin_deserialize; - plugin->getSerializedSampleMaxSizeFnc = - (PRESTypePluginGetSerializedSampleMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_max_size; - plugin->getSerializedSampleMinSizeFnc = - (PRESTypePluginGetSerializedSampleMinSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_min_size; - - plugin->getSampleFnc = - (PRESTypePluginGetSampleFunction) - ConnextStaticRawDataPlugin_get_sample; - plugin->returnSampleFnc = - (PRESTypePluginReturnSampleFunction) - ConnextStaticRawDataPlugin_return_sample; - - //plugin->getKeyKindFnc = ConnextStaticRawDataPlugin_get_external_key_kind(external_type_code); - plugin->getKeyKindFnc = (PRESTypePluginGetKeyKindFunction) - ConnextStaticRawDataPlugin_get_key_kind; - - plugin->getSerializedKeyMaxSizeFnc = - (PRESTypePluginGetSerializedKeyMaxSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_key_max_size; - plugin->serializeKeyFnc = - (PRESTypePluginSerializeKeyFunction) - ConnextStaticRawDataPlugin_serialize_key; - plugin->deserializeKeyFnc = - (PRESTypePluginDeserializeKeyFunction) - ConnextStaticRawDataPlugin_deserialize_key; - plugin->deserializeKeySampleFnc = - (PRESTypePluginDeserializeKeySampleFunction) - ConnextStaticRawDataPlugin_deserialize_key_sample; - - plugin->instanceToKeyHashFnc = - (PRESTypePluginInstanceToKeyHashFunction) - ConnextStaticRawDataPlugin_instance_to_keyhash; - plugin->serializedSampleToKeyHashFnc = - (PRESTypePluginSerializedSampleToKeyHashFunction) - ConnextStaticRawDataPlugin_serialized_sample_to_keyhash; - - plugin->getKeyFnc = - (PRESTypePluginGetKeyFunction) - ConnextStaticRawDataPlugin_get_key; - plugin->returnKeyFnc = - (PRESTypePluginReturnKeyFunction) - ConnextStaticRawDataPlugin_return_key; - - plugin->instanceToKeyFnc = - (PRESTypePluginInstanceToKeyFunction) - ConnextStaticRawDataPlugin_instance_to_key; - plugin->keyToInstanceFnc = - (PRESTypePluginKeyToInstanceFunction) - ConnextStaticRawDataPlugin_key_to_instance; - plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ - //plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); - plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; - - plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; - - /* Serialized buffer */ - plugin->getBuffer = - (PRESTypePluginGetBufferFunction) - ConnextStaticRawDataPlugin_get_buffer; - plugin->returnBuffer = - (PRESTypePluginReturnBufferFunction) - ConnextStaticRawDataPlugin_return_buffer; - plugin->getSerializedSampleSizeFnc = - (PRESTypePluginGetSerializedSampleSizeFunction) - ConnextStaticRawDataPlugin_get_serialized_sample_size; - - plugin->endpointTypeName = ConnextStaticRawDataTYPENAME; - - return plugin; -} - -void -ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin * plugin) -{ - RTIOsapiHeap_freeStructure(plugin); -} -#undef RTI_CDR_CURRENT_SUBMODULE diff --git a/rmw_connext_cpp/src/connext_static_raw_data_support.cpp b/rmw_connext_cpp/src/connext_static_raw_data_support.cpp deleted file mode 100644 index e486af45..00000000 --- a/rmw_connext_cpp/src/connext_static_raw_data_support.cpp +++ /dev/null @@ -1,175 +0,0 @@ -/* -WARNING: THIS FILE IS AUTO-GENERATED. DO NOT MODIFY. - -This file was generated from ConnextStaticRawData.idl using "rtiddsgen". -The rtiddsgen tool is part of the RTI Connext distribution. -For more information, type 'rtiddsgen -help' at a command shell -or consult the RTI Connext manual. -*/ - -#include "rmw_connext_cpp/connext_static_raw_data_support.hpp" -#include "rmw_connext_cpp/connext_static_raw_data_plugin.hpp" - -#ifndef dds_c_log_impl_h -#include "dds_c/dds_c_log_impl.h" -#endif - -/* ========================================================================= */ -/** -<> - -Defines: TData, -TDataWriter, -TDataReader, -TTypeSupport - -Configure and implement 'ConnextStaticRawData' support classes. - -Note: Only the #defined classes get defined -*/ - -/* ----------------------------------------------------------------- */ -/* DDSDataWriter -*/ - -/** -<> - -Defines: TDataWriter, TData -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME - -/* Defines */ -#define TDataWriter ConnextStaticRawDataDataWriter -#define TData ConnextStaticRawData - -#include "dds_cpp/generic/dds_cpp_data_TDataWriter.gen" - -#undef TDataWriter -#undef TData - -#undef TTYPENAME - -/* ----------------------------------------------------------------- */ -/* DDSDataReader -*/ - -/** -<> - -Defines: TDataReader, TDataSeq, TData -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME - -/* Defines */ -#define TDataReader ConnextStaticRawDataDataReader -#define TDataSeq ConnextStaticRawDataSeq -#define TData ConnextStaticRawData - -#include "dds_cpp/generic/dds_cpp_data_TDataReader.gen" - -#undef TDataReader -#undef TDataSeq -#undef TData - -#undef TTYPENAME - -/* ----------------------------------------------------------------- */ -/* TypeSupport - -<> - -Requires: TTYPENAME, -TPlugin_new -TPlugin_delete -Defines: TTypeSupport, TData, TDataReader, TDataWriter -*/ - -/* Requires */ -#define TTYPENAME ConnextStaticRawDataTYPENAME -#define TPlugin_new ConnextStaticRawDataPlugin_new -#define TPlugin_delete ConnextStaticRawDataPlugin_delete - -/* Defines */ -#define TTypeSupport ConnextStaticRawDataTypeSupport -#define TData ConnextStaticRawData -#define TDataReader ConnextStaticRawDataDataReader -#define TDataWriter ConnextStaticRawDataDataWriter -#define TGENERATE_SER_CODE -#define TGENERATE_TYPECODE - -#include "dds_cpp/generic/dds_cpp_data_TTypeSupport.gen" - -#undef TTypeSupport -#undef TData -#undef TDataReader -#undef TDataWriter -#undef TGENERATE_TYPECODE -#undef TGENERATE_SER_CODE -#undef TTYPENAME -#undef TPlugin_new -#undef TPlugin_delete - -DDS_ReturnCode_t -ConnextStaticRawDataSupport_register_external_type( - DDSDomainParticipant * participant, - const char * type_name, - struct DDS_TypeCode * type_code) -{ - DDSTypeSupport * dds_data_type = NULL; - struct PRESTypePlugin * presTypePlugin = NULL; - DDS_ReturnCode_t retcode = DDS_RETCODE_ERROR; - DDS_Boolean delete_data_type = DDS_BOOLEAN_FALSE; - RTIBool already_registered = RTI_FALSE; - - if (type_code == NULL) { - goto finError; - } - - if (participant == NULL) { - goto finError; - } - - /* TODO pass the type_code */ - presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); - if (presTypePlugin == NULL) { - goto finError; - } - - dds_data_type = new ConnextStaticRawDataTypeSupport(true); - if (dds_data_type == NULL) { - fprintf(stderr, "Error while registering external type\n"); - goto finError; - } - delete_data_type = RTI_TRUE; - - presTypePlugin->_userBuffer = (PRESWord *)dds_data_type; - already_registered = participant->is_type_registered(type_name); - - retcode = participant->register_type(type_name, presTypePlugin, NULL, !already_registered); - if (retcode != DDS_RETCODE_OK) { - fprintf(stderr, "error while registering external type\n"); - goto finError; - } - - if (!already_registered) { - delete_data_type = DDS_BOOLEAN_FALSE; - } - - retcode = DDS_RETCODE_OK; - -finError: - if (presTypePlugin != NULL) { - ConnextStaticRawDataPlugin_delete(presTypePlugin); - } - if (delete_data_type) { - delete (ConnextStaticRawDataTypeSupport *)dds_data_type; - dds_data_type = NULL; - } - - return retcode; -} From 26f9b4e0cc0558aba704ab31b39a5da6913ae140 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 23 Apr 2018 10:55:52 -0700 Subject: [PATCH 24/34] add rmw_serialize functions --- rmw_connext_cpp/CMakeLists.txt | 1 + rmw_connext_cpp/src/rmw_client.cpp | 2 +- rmw_connext_cpp/src/rmw_publish.cpp | 16 ++-- rmw_connext_cpp/src/rmw_publisher.cpp | 2 +- rmw_connext_cpp/src/rmw_serialize.cpp | 85 +++++++++++++++++++ rmw_connext_cpp/src/rmw_service.cpp | 2 +- rmw_connext_cpp/src/rmw_subscription.cpp | 2 +- rmw_connext_cpp/src/rmw_take.cpp | 14 +-- rmw_connext_cpp/src/type_support_common.hpp | 12 +-- .../resource/msg__type_support_c.cpp.em | 15 ++-- .../connext_static_cdr_stream.hpp | 10 ++- .../resource/msg__type_support.cpp.em | 17 ++-- 12 files changed, 135 insertions(+), 43 deletions(-) create mode 100644 rmw_connext_cpp/src/rmw_serialize.cpp diff --git a/rmw_connext_cpp/CMakeLists.txt b/rmw_connext_cpp/CMakeLists.txt index 52591cd9..8a011e9d 100644 --- a/rmw_connext_cpp/CMakeLists.txt +++ b/rmw_connext_cpp/CMakeLists.txt @@ -143,6 +143,7 @@ add_library( src/rmw_publisher.cpp src/rmw_request.cpp src/rmw_response.cpp + src/rmw_serialize.cpp src/rmw_service.cpp src/rmw_service_names_and_types.cpp src/rmw_service_server_is_available.cpp diff --git a/rmw_connext_cpp/src/rmw_client.cpp b/rmw_connext_cpp/src/rmw_client.cpp index 14d14bec..fdbb4f9c 100644 --- a/rmw_connext_cpp/src/rmw_client.cpp +++ b/rmw_connext_cpp/src/rmw_client.cpp @@ -50,7 +50,7 @@ rmw_create_client( node->implementation_identifier, rti_connext_identifier, return NULL) - RMW_CONNEXT_EXTRACT_SERVICE_TYPESUPPORT(type_supports, type_support) + RMW_CONNEXT_EXTRACT_SERVICE_TYPESUPPORT(type_supports, type_support, NULL) if (!qos_profile) { RMW_SET_ERROR_MSG("qos_profile is null"); diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 2eca3c32..6ba54b93 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -44,8 +44,8 @@ publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) instance->serialized_data.maximum(0); if (!instance->serialized_data.loan_contiguous( - reinterpret_cast(cdr_stream->raw_message), - cdr_stream->message_length, cdr_stream->message_length)) + reinterpret_cast(cdr_stream->buffer), + cdr_stream->buffer_length, cdr_stream->buffer_length)) { RMW_SET_ERROR_MSG("failed to loan memory for message"); goto cleanup; @@ -100,16 +100,18 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) return RMW_RET_ERROR; } + // if calling rmw_publish as is, this will always dynamically allocate memory ConnextStaticCDRStream cdr_stream; + cdr_stream.allocator = rcutils_get_default_allocator(); if (!callbacks->to_cdr_stream(ros_message, &cdr_stream)) { RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); return RMW_RET_ERROR; } - if (cdr_stream.message_length == 0) { + if (cdr_stream.buffer_length == 0) { RMW_SET_ERROR_MSG("no message length set"); return RMW_RET_ERROR; } - if (!cdr_stream.raw_message) { + if (!cdr_stream.buffer) { RMW_SET_ERROR_MSG("no raw message attached"); return RMW_RET_ERROR; } @@ -117,7 +119,6 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) RMW_SET_ERROR_MSG("failed to publish message"); return RMW_RET_ERROR; } - free(cdr_stream.raw_message); return RMW_RET_OK; } @@ -155,8 +156,9 @@ rmw_publish_raw(const rmw_publisher_t * publisher, const rmw_message_raw_t * raw } ConnextStaticCDRStream cdr_stream; - cdr_stream.raw_message = raw_message->buffer; - cdr_stream.message_length = raw_message->buffer_length; + cdr_stream.buffer = raw_message->buffer; + cdr_stream.buffer_length = raw_message->buffer_length; + cdr_stream.buffer_capacity = raw_message->buffer_capacity; bool published = publish(topic_writer, &cdr_stream); if (!published) { RMW_SET_ERROR_MSG("failed to publish message"); diff --git a/rmw_connext_cpp/src/rmw_publisher.cpp b/rmw_connext_cpp/src/rmw_publisher.cpp index 111c9b8d..e1137c07 100644 --- a/rmw_connext_cpp/src/rmw_publisher.cpp +++ b/rmw_connext_cpp/src/rmw_publisher.cpp @@ -55,7 +55,7 @@ rmw_create_publisher( node->implementation_identifier, rti_connext_identifier, return NULL) - RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_supports, type_support) + RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_supports, type_support, NULL) if (!topic_name || strlen(topic_name) == 0) { RMW_SET_ERROR_MSG("publisher topic is null or empty string"); diff --git a/rmw_connext_cpp/src/rmw_serialize.cpp b/rmw_connext_cpp/src/rmw_serialize.cpp new file mode 100644 index 00000000..db6c7678 --- /dev/null +++ b/rmw_connext_cpp/src/rmw_serialize.cpp @@ -0,0 +1,85 @@ +// Copyright 2014-2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "./type_support_common.hpp" + +// include patched generated code from the build folder +#include "connext_static_raw_dataSupport.h" + +extern "C" +{ +rmw_ret_t +rmw_serialize( + const void * ros_message, + const rosidl_message_type_support_t * type_support, + rmw_message_raw_t * raw_message) +{ + RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_support, ts, RMW_RET_ERROR) + + const message_type_support_callbacks_t * callbacks = + static_cast(ts->data); + if (!callbacks) { + RMW_SET_ERROR_MSG("callbacks handle is null"); + return RMW_RET_ERROR; + } + + ConnextStaticCDRStream cdr_stream; + cdr_stream.buffer = raw_message->buffer; + cdr_stream.buffer_length = raw_message->buffer_length; + cdr_stream.buffer_capacity = raw_message->buffer_capacity; + cdr_stream.allocator = raw_message->allocator; + + if (!callbacks->to_cdr_stream(ros_message, &cdr_stream)) { + RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); + return RMW_RET_ERROR; + } + raw_message->buffer = cdr_stream.buffer; // reassgin buffer because it might have been resized + raw_message->buffer_length = cdr_stream.buffer_length; + raw_message->buffer_capacity = cdr_stream.buffer_capacity; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_deserialize( + const rmw_message_raw_t * raw_message, + const rosidl_message_type_support_t * type_support, + void * ros_message) +{ + RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_support, ts, RMW_RET_ERROR) + + const message_type_support_callbacks_t * callbacks = + static_cast(ts->data); + if (!callbacks) { + RMW_SET_ERROR_MSG("callbacks handle is null"); + return RMW_RET_ERROR; + } + + ConnextStaticCDRStream cdr_stream; + cdr_stream.buffer = raw_message->buffer; + cdr_stream.buffer_length = raw_message->buffer_length; + cdr_stream.buffer_capacity = raw_message->buffer_capacity; + cdr_stream.allocator = raw_message->allocator; + // convert the cdr stream to the message + if (!callbacks->to_message(&cdr_stream, ros_message)) { + RMW_SET_ERROR_MSG("can't convert cdr stream to ros message"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} + +} // extern "C" diff --git a/rmw_connext_cpp/src/rmw_service.cpp b/rmw_connext_cpp/src/rmw_service.cpp index 3d552997..ad02328c 100644 --- a/rmw_connext_cpp/src/rmw_service.cpp +++ b/rmw_connext_cpp/src/rmw_service.cpp @@ -49,7 +49,7 @@ rmw_create_service( node->implementation_identifier, rti_connext_identifier, return NULL) - RMW_CONNEXT_EXTRACT_SERVICE_TYPESUPPORT(type_supports, type_support) + RMW_CONNEXT_EXTRACT_SERVICE_TYPESUPPORT(type_supports, type_support, NULL) if (!qos_profile) { RMW_SET_ERROR_MSG("qos_profile is null"); diff --git a/rmw_connext_cpp/src/rmw_subscription.cpp b/rmw_connext_cpp/src/rmw_subscription.cpp index 227aefbb..00e62351 100644 --- a/rmw_connext_cpp/src/rmw_subscription.cpp +++ b/rmw_connext_cpp/src/rmw_subscription.cpp @@ -55,7 +55,7 @@ rmw_create_subscription( node->implementation_identifier, rti_connext_identifier, return NULL) - RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_supports, type_support) + RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_supports, type_support, NULL) if (!qos_profile) { RMW_SET_ERROR_MSG("qos_profile is null"); diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index 4e814e22..7cff8d88 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -101,12 +101,12 @@ take( } if (!ignore_sample) { - cdr_stream->message_length = dds_messages[0].serialized_data.length(); + cdr_stream->buffer_length = dds_messages[0].serialized_data.length(); // TODO(karsten1987): This malloc has to go! - cdr_stream->raw_message = - reinterpret_cast(malloc(cdr_stream->message_length * sizeof(char))); - for (unsigned int i = 0; i < cdr_stream->message_length; ++i) { - cdr_stream->raw_message[i] = dds_messages[0].serialized_data[i]; + cdr_stream->buffer = + reinterpret_cast(malloc(cdr_stream->buffer_length * sizeof(char))); + for (unsigned int i = 0; i < cdr_stream->buffer_length; ++i) { + cdr_stream->buffer[i] = dds_messages[0].serialized_data[i]; } *taken = true; } else { @@ -265,8 +265,8 @@ _take_raw( return RMW_RET_ERROR; } - raw_message->buffer_length = cdr_stream.message_length; - raw_message->buffer = cdr_stream.raw_message; + raw_message->buffer_length = cdr_stream.buffer_length; + raw_message->buffer = cdr_stream.buffer; return RMW_RET_OK; } diff --git a/rmw_connext_cpp/src/type_support_common.hpp b/rmw_connext_cpp/src/type_support_common.hpp index 06d90229..28dfd875 100644 --- a/rmw_connext_cpp/src/type_support_common.hpp +++ b/rmw_connext_cpp/src/type_support_common.hpp @@ -27,10 +27,10 @@ #include "rosidl_typesupport_connext_cpp/message_type_support.h" #include "rosidl_typesupport_connext_cpp/service_type_support.h" -#define RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(TYPE_SUPPORTS, TYPE_SUPPORT) \ +#define RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(TYPE_SUPPORTS, TYPE_SUPPORT, RET_VAL) \ if (!TYPE_SUPPORTS) { \ RMW_SET_ERROR_MSG("type supports handle is null"); \ - return NULL; \ + return RET_VAL; \ } \ const rosidl_message_type_support_t * TYPE_SUPPORT = \ get_message_typesupport_handle( \ @@ -51,14 +51,14 @@ rosidl_typesupport_connext_c__identifier, \ static_cast(rosidl_typesupport_connext_c__identifier)); \ RMW_SET_ERROR_MSG(__msg); \ - return NULL; \ + return RET_VAL; \ } \ } -#define RMW_CONNEXT_EXTRACT_SERVICE_TYPESUPPORT(TYPE_SUPPORTS, TYPE_SUPPORT) \ +#define RMW_CONNEXT_EXTRACT_SERVICE_TYPESUPPORT(TYPE_SUPPORTS, TYPE_SUPPORT, RET_VAL) \ if (!TYPE_SUPPORTS) { \ RMW_SET_ERROR_MSG("type supports handle is null"); \ - return NULL; \ + return RET_VAL; \ } \ const rosidl_service_type_support_t * TYPE_SUPPORT = \ get_service_typesupport_handle( \ @@ -79,7 +79,7 @@ rosidl_typesupport_connext_c__identifier, \ static_cast(rosidl_typesupport_connext_c__identifier)); \ RMW_SET_ERROR_MSG(__msg); \ - return NULL; \ + return RET_VAL; \ } \ } diff --git a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em index 1d9c07f9..e17b066e 100644 --- a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em +++ b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em @@ -341,18 +341,17 @@ to_cdr_stream( // call the serialize function for the first time to get the expected length of the message if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( - NULL, &cdr_stream->message_length, &dds_message) != RTI_TRUE) + NULL, &cdr_stream->buffer_length, &dds_message) != RTI_TRUE) { return false; } - // allocate enough memory for the CDR stream - // TODO(karsten1987): This allocation has to be preallocated - // or at least bring in a custom allocator - cdr_stream->raw_message = - reinterpret_cast(malloc(sizeof(char) * cdr_stream->message_length)); + if (cdr_stream->buffer_capacity < cdr_stream->buffer_length) { + cdr_stream->allocator.deallocate(cdr_stream->buffer, cdr_stream->allocator.state); + cdr_stream->buffer = static_cast(cdr_stream->allocator.allocate(cdr_stream->buffer_length, cdr_stream->allocator.state)); + } // call the function again and fill the buffer this time if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( - cdr_stream->raw_message, &cdr_stream->message_length, &dds_message) != RTI_TRUE) + cdr_stream->buffer, &cdr_stream->buffer_length, &dds_message) != RTI_TRUE) { return false; } @@ -375,7 +374,7 @@ to_message( @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer( - dds_message, cdr_stream->raw_message, cdr_stream->message_length) != RTI_TRUE) + dds_message, cdr_stream->buffer, cdr_stream->buffer_length) != RTI_TRUE) { fprintf(stderr, "deserialize from cdr buffer failed\n"); return false; diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp index 255dacbf..3aa39896 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp @@ -15,12 +15,18 @@ #ifndef ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_CDR_STREAM_HPP_ #define ROSIDL_TYPESUPPORT_CONNEXT_CPP__CONNEXT_STATIC_CDR_STREAM_HPP_ +#include "rcutils/allocator.h" + extern "C" { +// TODO(karsten1987): Join rmw_raw_message_t and this ConnextStaticCDRStream struct +// inside rcutils to have only one implementation of it. typedef struct ConnextStaticCDRStream { - char * raw_message = nullptr; - unsigned int message_length = 0; + char * buffer = NULL; + unsigned int buffer_length = 0; + unsigned int buffer_capacity = 0; + rcutils_allocator_t allocator; } ConnextStaticCDRStream; } // extern "C" diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index 9afa4b07..65cf56a3 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -220,16 +220,15 @@ to_cdr_stream__@(spec.base_type.type)( } // call the serialize function for the first time to get the expected length of the message - if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(NULL, &cdr_stream->message_length, dds_message) != RTI_TRUE) { + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(NULL, &cdr_stream->buffer_length, dds_message) != RTI_TRUE) { return false; } - // allocate enough memory for the CDR stream - // TODO(karsten1987): This allocation has to be preallocated - // or at least bring in a custom allocator - cdr_stream->raw_message = - reinterpret_cast(malloc(sizeof(char) * cdr_stream->message_length)); + if (cdr_stream->buffer_capacity < cdr_stream->buffer_length) { + cdr_stream->allocator.deallocate(cdr_stream->buffer, cdr_stream->allocator.state); + cdr_stream->buffer = static_cast(cdr_stream->allocator.allocate(cdr_stream->buffer_length, cdr_stream->allocator.state)); + } // call the function again and fill the buffer this time - if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->raw_message, &cdr_stream->message_length, dds_message) != RTI_TRUE) { + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->buffer, &cdr_stream->buffer_length, dds_message) != RTI_TRUE) { return false; } if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK) { @@ -246,7 +245,7 @@ to_message__@(spec.base_type.type)( if (!cdr_stream) { return false; } - if (!cdr_stream->raw_message) { + if (!cdr_stream->buffer) { fprintf(stderr, "cdr stream doesn't contain data\n"); } if (!untyped_ros_message) { @@ -254,7 +253,7 @@ to_message__@(spec.base_type.type)( } @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); - if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer(dds_message, cdr_stream->raw_message, cdr_stream->message_length) != RTI_TRUE) { + if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer(dds_message, cdr_stream->buffer, cdr_stream->buffer_length) != RTI_TRUE) { fprintf(stderr, "deserialize from cdr buffer failed\n"); return false; } From 1e5484df04f3892b11aab168ca44d3f17b724718 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 27 Apr 2018 09:27:00 -0700 Subject: [PATCH 25/34] deallocate memory of temporary cdr stream --- rmw_connext_cpp/src/rmw_publish.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 6ba54b93..23bf386c 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -119,6 +119,8 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) RMW_SET_ERROR_MSG("failed to publish message"); return RMW_RET_ERROR; } + + cdr_stream.allocator.deallocate(cdr_stream.buffer, cdr_stream.allocator.state); return RMW_RET_OK; } From 3fac8cfebcddf6222d3cc81978936f4603b740d0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 27 Apr 2018 09:56:25 -0700 Subject: [PATCH 26/34] linters --- rmw_connext_cpp/bin/apply-patch.py | 33 ++++++++++++++++++++++----- rmw_connext_cpp/src/rmw_serialize.cpp | 13 +++++------ rmw_connext_cpp/src/rmw_take.cpp | 4 ++-- 3 files changed, 35 insertions(+), 15 deletions(-) diff --git a/rmw_connext_cpp/bin/apply-patch.py b/rmw_connext_cpp/bin/apply-patch.py index 7ae12150..8db7a22e 100644 --- a/rmw_connext_cpp/bin/apply-patch.py +++ b/rmw_connext_cpp/bin/apply-patch.py @@ -1,10 +1,29 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# originated from the stackoverflow post: +# https://stackoverflow.com/a/40967337 +# TODO(Karsten1987): Rewrite this function for clarity and tests + import argparse import re -_hdr_pat = re.compile("^@@ -(\d+),?(\d+)? \+(\d+),?(\d+)? @@.*$") +# regular expression / pattern for patch header +_hdr_pat = re.compile('^@@ -(\d+),?(\d+)? \+(\d+),?(\d+)? @@.*$') -def apply_patch(s, patch, revert=False): +def apply_patch(s, patch): """ Apply unified diff patch to string s to recover newer string. @@ -14,17 +33,19 @@ def apply_patch(s, patch, revert=False): p = patch.splitlines(True) t = '' i = sl = 0 - (midx, sign) = (1, '+') if not revert else (3, '-') + (midx, sign) = (1, '+') while i < len(p) and p[i].startswith(('---', '+++')): i += 1 # skip header lines while i < len(p): + # find patch header m = _hdr_pat.match(p[i]) if not m: raise Exception('Cannot process diff in line ' + str(i)) + i += 1 - l = int(m.group(midx)) - 1 + (m.group(midx + 1) == '0') - t += ''.join(s[sl:l]) - sl = l + ll = int(m.group(midx)) - 1 + (m.group(midx + 1) == '0') + t += ''.join(s[sl:ll]) + sl = ll while i < len(p) and p[i][0] != '@': if i + 1 < len(p) and p[i + 1][0] == '\\': line = p[i][:-1] diff --git a/rmw_connext_cpp/src/rmw_serialize.cpp b/rmw_connext_cpp/src/rmw_serialize.cpp index db6c7678..dc9536ba 100644 --- a/rmw_connext_cpp/src/rmw_serialize.cpp +++ b/rmw_connext_cpp/src/rmw_serialize.cpp @@ -24,9 +24,9 @@ extern "C" { rmw_ret_t rmw_serialize( - const void * ros_message, - const rosidl_message_type_support_t * type_support, - rmw_message_raw_t * raw_message) + const void * ros_message, + const rosidl_message_type_support_t * type_support, + rmw_message_raw_t * raw_message) { RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_support, ts, RMW_RET_ERROR) @@ -55,9 +55,9 @@ rmw_serialize( rmw_ret_t rmw_deserialize( - const rmw_message_raw_t * raw_message, - const rosidl_message_type_support_t * type_support, - void * ros_message) + const rmw_message_raw_t * raw_message, + const rosidl_message_type_support_t * type_support, + void * ros_message) { RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_support, ts, RMW_RET_ERROR) @@ -81,5 +81,4 @@ rmw_deserialize( return RMW_RET_OK; } - } // extern "C" diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index 7cff8d88..c4d385d5 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -24,8 +24,8 @@ #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" // include patched generated code from the build folder -#include "connext_static_raw_dataSupport.h" -#include "connext_static_raw_data.h" +#include "./connext_static_raw_dataSupport.h" +#include "./connext_static_raw_data.h" static bool take( From a810982d0e2a6f5a1810ca6f93f4b67f0826e7f6 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 7 May 2018 11:25:52 -0700 Subject: [PATCH 27/34] cleanup allocated memory --- rmw_connext_cpp/src/rmw_publish.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 23bf386c..9eab7004 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -100,28 +100,33 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) return RMW_RET_ERROR; } - // if calling rmw_publish as is, this will always dynamically allocate memory + auto ret = RMW_RET_OK; ConnextStaticCDRStream cdr_stream; cdr_stream.allocator = rcutils_get_default_allocator(); if (!callbacks->to_cdr_stream(ros_message, &cdr_stream)) { RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); - return RMW_RET_ERROR; + ret = RMW_RET_ERROR; + goto fail; } if (cdr_stream.buffer_length == 0) { RMW_SET_ERROR_MSG("no message length set"); - return RMW_RET_ERROR; + ret = RMW_RET_ERROR; + goto fail; } if (!cdr_stream.buffer) { RMW_SET_ERROR_MSG("no raw message attached"); - return RMW_RET_ERROR; + ret = RMW_RET_ERROR; + goto fail; } if (!publish(topic_writer, &cdr_stream)) { RMW_SET_ERROR_MSG("failed to publish message"); - return RMW_RET_ERROR; + ret = RMW_RET_ERROR; + goto fail; } +fail: cdr_stream.allocator.deallocate(cdr_stream.buffer, cdr_stream.allocator.state); - return RMW_RET_OK; + return ret; } rmw_ret_t From d5f525f52b4b58ed014681425e338ec1bdc66f9c Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 13 Jun 2018 10:09:17 +0200 Subject: [PATCH 28/34] address comments --- rmw_connext_cpp/bin/apply-patch.py | 4 ++-- rmw_connext_cpp/src/rmw_take.cpp | 6 ++++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/rmw_connext_cpp/bin/apply-patch.py b/rmw_connext_cpp/bin/apply-patch.py index 8db7a22e..8514841e 100644 --- a/rmw_connext_cpp/bin/apply-patch.py +++ b/rmw_connext_cpp/bin/apply-patch.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -# originated from the stackoverflow post: -# https://stackoverflow.com/a/40967337 +# Based on `unifieddiff.py` by @noporpoise which is licensed as `Public domain (CC0)` +# see: https://gist.github.com/noporpoise/16e731849eb1231e86d78f9dfeca3abc # TODO(Karsten1987): Rewrite this function for clarity and tests import argparse diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index c4d385d5..efc27342 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -177,6 +177,10 @@ _take( return RMW_RET_ERROR; } + // the call to take allocates memory for the raw message + // we have to free this here again + free(cdr_stream.buffer); + return RMW_RET_OK; } @@ -266,6 +270,8 @@ _take_raw( } raw_message->buffer_length = cdr_stream.buffer_length; + // we don't free the allocated memory + // and thus can directly set the pointer raw_message->buffer = cdr_stream.buffer; return RMW_RET_OK; From 988371de0d83d10015605458f1d1a5dc679cf9b0 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 14 Jun 2018 02:06:27 +0200 Subject: [PATCH 29/34] raw->serialized --- rmw_connext_cpp/CMakeLists.txt | 42 +++++++-------- ...idl => connext_static_serialized_data.idl} | 2 +- .../connext_static_raw_dataPlugin.h.patch | 11 ---- ... connext_static_serialized_data.cxx.patch} | 4 +- ...=> connext_static_serialized_data.h.patch} | 4 +- ...xt_static_serialized_dataPlugin.cxx.patch} | 28 +++++----- ...nnext_static_serialized_dataPlugin.h.patch | 11 ++++ ...t_static_serialized_dataSupport.cxx.patch} | 14 ++--- ...ext_static_serialized_dataSupport.h.patch} | 54 +++++++++---------- rmw_connext_cpp/src/rmw_publish.cpp | 25 ++++----- rmw_connext_cpp/src/rmw_publisher.cpp | 4 +- rmw_connext_cpp/src/rmw_serialize.cpp | 28 +++++----- rmw_connext_cpp/src/rmw_subscription.cpp | 4 +- rmw_connext_cpp/src/rmw_take.cpp | 35 ++++++------ 14 files changed, 134 insertions(+), 132 deletions(-) rename rmw_connext_cpp/resources/{connext_static_raw_data.idl => connext_static_serialized_data.idl} (80%) delete mode 100644 rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch rename rmw_connext_cpp/resources/patch_files/{connext_static_raw_data.cxx.patch => connext_static_serialized_data.cxx.patch} (64%) rename rmw_connext_cpp/resources/patch_files/{connext_static_raw_data.h.patch => connext_static_serialized_data.h.patch} (59%) rename rmw_connext_cpp/resources/patch_files/{connext_static_raw_dataPlugin.cxx.patch => connext_static_serialized_dataPlugin.cxx.patch} (91%) create mode 100644 rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataPlugin.h.patch rename rmw_connext_cpp/resources/patch_files/{connext_static_raw_dataSupport.cxx.patch => connext_static_serialized_dataSupport.cxx.patch} (73%) rename rmw_connext_cpp/resources/patch_files/{connext_static_raw_dataSupport.h.patch => connext_static_serialized_dataSupport.h.patch} (59%) diff --git a/rmw_connext_cpp/CMakeLists.txt b/rmw_connext_cpp/CMakeLists.txt index 8a011e9d..b127fc26 100644 --- a/rmw_connext_cpp/CMakeLists.txt +++ b/rmw_connext_cpp/CMakeLists.txt @@ -76,45 +76,45 @@ endif() set(generated_directory "${CMAKE_CURRENT_BINARY_DIR}/resources/generated") file(MAKE_DIRECTORY ${generated_directory}) set(generated_files - ${generated_directory}/connext_static_raw_data.cxx - ${generated_directory}/connext_static_raw_data.h - ${generated_directory}/connext_static_raw_dataPlugin.cxx - ${generated_directory}/connext_static_raw_dataPlugin.h - ${generated_directory}/connext_static_raw_dataSupport.cxx - ${generated_directory}/connext_static_raw_dataSupport.h + ${generated_directory}/connext_static_serialized_data.cxx + ${generated_directory}/connext_static_serialized_data.h + ${generated_directory}/connext_static_serialized_dataPlugin.cxx + ${generated_directory}/connext_static_serialized_dataPlugin.h + ${generated_directory}/connext_static_serialized_dataSupport.cxx + ${generated_directory}/connext_static_serialized_dataSupport.h ) add_custom_command( OUTPUT ${generated_files} - COMMAND "${_idl_pp}" -language C++ -unboundedSupport "connext_static_raw_data.idl" -d ${generated_directory} + COMMAND "${_idl_pp}" -language C++ -unboundedSupport "connext_static_serialized_data.idl" -d ${generated_directory} WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/resources" - COMMENT "Generating raw type support for RTI Connext (using '${_idl_pp}')" + COMMENT "Generating serialized type support for RTI Connext (using '${_idl_pp}')" VERBATIM ) # patch the generate code for raw data set(patch_files - ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_data.cxx.patch - ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_data.h.patch - ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch - ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataPlugin.h.patch - ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataSupport.cxx.patch - ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_raw_dataSupport.h.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_data.cxx.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_data.h.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_dataPlugin.cxx.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_dataPlugin.h.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_dataSupport.cxx.patch + ${CMAKE_CURRENT_SOURCE_DIR}/resources/patch_files/connext_static_serialized_dataSupport.h.patch ) set(patched_directory "${CMAKE_CURRENT_BINARY_DIR}/resources/patched") file(MAKE_DIRECTORY ${patched_directory}) set(patched_files - ${patched_directory}/connext_static_raw_data.cxx - ${patched_directory}/connext_static_raw_data.h - ${patched_directory}/connext_static_raw_dataPlugin.cxx - ${patched_directory}/connext_static_raw_dataPlugin.h - ${patched_directory}/connext_static_raw_dataSupport.cxx - ${patched_directory}/connext_static_raw_dataSupport.h + ${patched_directory}/connext_static_serialized_data.cxx + ${patched_directory}/connext_static_serialized_data.h + ${patched_directory}/connext_static_serialized_dataPlugin.cxx + ${patched_directory}/connext_static_serialized_dataPlugin.h + ${patched_directory}/connext_static_serialized_dataSupport.cxx + ${patched_directory}/connext_static_serialized_dataSupport.h ) add_custom_command( OUTPUT ${patched_files} COMMAND ${PYTHON_EXECUTABLE} "${CMAKE_SOURCE_DIR}/bin/apply-patch.py" --input ${generated_files} --patch ${patch_files} --out ${patched_files} DEPENDS ${generated_files} - COMMENT "Patching raw type support for RTI Connext" + COMMENT "Patching serialized type support for RTI Connext" VERBATIM ) diff --git a/rmw_connext_cpp/resources/connext_static_raw_data.idl b/rmw_connext_cpp/resources/connext_static_serialized_data.idl similarity index 80% rename from rmw_connext_cpp/resources/connext_static_raw_data.idl rename to rmw_connext_cpp/resources/connext_static_serialized_data.idl index 44f5704a..90c69dc8 100644 --- a/rmw_connext_cpp/resources/connext_static_raw_data.idl +++ b/rmw_connext_cpp/resources/connext_static_serialized_data.idl @@ -1,6 +1,6 @@ const long KEY_HASH_LENGTH_16 = 16; -struct ConnextStaticRawData { +struct ConnextStaticSerializedData { @key octet key_hash[KEY_HASH_LENGTH_16]; sequence serialized_key; sequence serialized_data; diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch deleted file mode 100644 index 059a86ce..00000000 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.h.patch +++ /dev/null @@ -1,11 +0,0 @@ ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.h -@@ -324,6 +324,9 @@ extern "C" { - NDDSUSERDllExport extern struct PRESTypePlugin* - ConnextStaticRawDataPlugin_new(void); - -+ NDDSUSERDllExport extern struct PRESTypePlugin* -+ ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code); -+ - NDDSUSERDllExport extern void - ConnextStaticRawDataPlugin_delete(struct PRESTypePlugin *); diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_data.cxx.patch similarity index 64% rename from rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch rename to rmw_connext_cpp/resources/patch_files/connext_static_serialized_data.cxx.patch index 03a07378..3b9800ae 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.cxx.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_data.cxx.patch @@ -1,5 +1,5 @@ ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.cxx +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_data.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_data.cxx @@ -11,7 +11,7 @@ or consult the RTI Connext manual. #ifndef NDDS_STANDALONE_TYPE diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_data.h.patch similarity index 59% rename from rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch rename to rmw_connext_cpp/resources/patch_files/connext_static_serialized_data.h.patch index babcf2da..04ffd023 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_data.h.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_data.h.patch @@ -1,5 +1,5 @@ ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_data.h +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_data.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_data.h @@ -14,7 +14,7 @@ or consult the RTI Connext manual. #ifndef NDDS_STANDALONE_TYPE diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataPlugin.cxx.patch similarity index 91% rename from rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch rename to rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataPlugin.cxx.patch index f5634eea..327c4f69 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataPlugin.cxx.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataPlugin.cxx.patch @@ -1,5 +1,5 @@ ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataPlugin.cxx +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_dataPlugin.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_dataPlugin.cxx @@ -11,7 +11,7 @@ or consult the RTI Connext manual. #include @@ -9,7 +9,7 @@ #endif #ifndef osapi_type_h -@@ -379,74 +379,81 @@ ConnextStaticRawDataPlugin_serialize( +@@ -379,74 +379,81 @@ ConnextStaticSerializedDataPlugin_serialize( RTIBool serialize_sample, void *endpoint_plugin_qos) { @@ -138,7 +138,7 @@ + RTICdrStream_getEncapsulationOptions() +*/ +void -+ConnextStaticRawDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) ++ConnextStaticSerializedDataPlugin_remove_padding_from_stream(struct RTICdrStream * stream) +{ + /* See http://issues.omg.org/browse/DDSXTY12-10 */ + DDS_UnsignedShort padding_size_mask = 0x0003; @@ -151,7 +151,7 @@ } RTIBool -@@ -458,114 +465,65 @@ ConnextStaticRawDataPlugin_deserialize_sample( +@@ -458,114 +465,65 @@ ConnextStaticSerializedDataPlugin_deserialize_sample( RTIBool deserialize_sample, void *endpoint_plugin_qos) { @@ -174,7 +174,7 @@ + /* TODO. The call does not belong here. It should be pushed + * inside RTICdrStream_deserializeAndSetCdrEncapsulation + */ -+ ConnextStaticRawDataPlugin_remove_padding_from_stream(stream); ++ ConnextStaticSerializedDataPlugin_remove_padding_from_stream(stream); + + if (deserialize_sample) { + /* Note that sample->key_hash was already set by SerializedTypePlugin_deserialize() @@ -210,7 +210,7 @@ - } - if(deserialize_sample) { - -- ConnextStaticRawData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); +- ConnextStaticSerializedData_initialize_ex(sample, RTI_FALSE, RTI_FALSE); - - if (!RTICdrStream_deserializePrimitiveArray( - stream, (void*) sample->key_hash, ((KEY_HASH_LENGTH_16)), RTI_CDR_OCTET_TYPE)) { @@ -322,7 +322,7 @@ RTIBool @@ -971,7 +929,9 @@ Key Management functions: PRESTypePluginKeyKind - ConnextStaticRawDataPlugin_get_key_kind(void) + ConnextStaticSerializedDataPlugin_get_key_kind(void) { - return PRES_TYPEPLUGIN_USER_KEY; + // TODO(karsten1987): Whenever we introduce keys for our data types @@ -331,23 +331,23 @@ } RTIBool -@@ -1408,6 +1368,11 @@ ConnextStaticRawDataPlugin_serialized_sample_to_keyhash( +@@ -1408,6 +1368,11 @@ ConnextStaticSerializedDataPlugin_serialized_sample_to_keyhash( * ------------------------------------------------------------------------ */ - struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) + struct PRESTypePlugin *ConnextStaticSerializedDataPlugin_new(void) { + return NULL; +} + -+struct PRESTypePlugin *ConnextStaticRawDataPlugin_new_external(struct DDS_TypeCode * external_type_code) ++struct PRESTypePlugin *ConnextStaticSerializedDataPlugin_new_external(struct DDS_TypeCode * external_type_code) +{ struct PRESTypePlugin *plugin = NULL; const struct PRESTypePluginVersion PLUGIN_VERSION = PRES_TYPE_PLUGIN_VERSION_2_0; -@@ -1503,7 +1468,7 @@ struct PRESTypePlugin *ConnextStaticRawDataPlugin_new(void) +@@ -1503,7 +1468,7 @@ struct PRESTypePlugin *ConnextStaticSerializedDataPlugin_new(void) (PRESTypePluginKeyToInstanceFunction) - ConnextStaticRawDataPlugin_key_to_instance; + ConnextStaticSerializedDataPlugin_key_to_instance; plugin->serializedKeyToKeyHashFnc = NULL; /* Not supported yet */ -- plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticRawData_get_typecode(); +- plugin->typeCode = (struct RTICdrTypeCode *)ConnextStaticSerializedData_get_typecode(); + plugin->typeCode = (struct RTICdrTypeCode *)external_type_code; plugin->languageKind = PRES_TYPEPLUGIN_CPP_LANG; diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataPlugin.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataPlugin.h.patch new file mode 100644 index 00000000..c59c6981 --- /dev/null +++ b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataPlugin.h.patch @@ -0,0 +1,11 @@ +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_dataPlugin.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_dataPlugin.h +@@ -324,6 +324,9 @@ extern "C" { + NDDSUSERDllExport extern struct PRESTypePlugin* + ConnextStaticSerializedDataPlugin_new(void); + ++ NDDSUSERDllExport extern struct PRESTypePlugin* ++ ConnextStaticSerializedDataPlugin_new_external(struct DDS_TypeCode * external_type_code); ++ + NDDSUSERDllExport extern void + ConnextStaticSerializedDataPlugin_delete(struct PRESTypePlugin *); diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.cxx.patch b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataSupport.cxx.patch similarity index 73% rename from rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.cxx.patch rename to rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataSupport.cxx.patch index 7c3a3922..0849bf1d 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.cxx.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataSupport.cxx.patch @@ -1,11 +1,11 @@ ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.cxx +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_dataSupport.cxx ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_dataSupport.cxx @@ -115,3 +115,62 @@ Defines: TTypeSupport, TData, TDataReader, TDataWriter #undef TPlugin_new #undef TPlugin_delete +DDS_ReturnCode_t -+ConnextStaticRawDataSupport_register_external_type( ++ConnextStaticSerializedDataSupport_register_external_type( + DDSDomainParticipant * participant, + const char * type_name, + struct DDS_TypeCode * type_code) @@ -25,12 +25,12 @@ + } + + /* TODO pass the type_code */ -+ presTypePlugin = ConnextStaticRawDataPlugin_new_external(type_code); ++ presTypePlugin = ConnextStaticSerializedDataPlugin_new_external(type_code); + if (presTypePlugin == NULL) { + goto finError; + } + -+ dds_data_type = new ConnextStaticRawDataTypeSupport(true); ++ dds_data_type = new ConnextStaticSerializedDataTypeSupport(true); + if (dds_data_type == NULL) { + fprintf(stderr, "Error while registering external type\n"); + goto finError; @@ -54,10 +54,10 @@ + +finError: + if (presTypePlugin != NULL) { -+ ConnextStaticRawDataPlugin_delete(presTypePlugin); ++ ConnextStaticSerializedDataPlugin_delete(presTypePlugin); + } + if (delete_data_type) { -+ delete (ConnextStaticRawDataTypeSupport *)dds_data_type; ++ delete (ConnextStaticSerializedDataTypeSupport *)dds_data_type; + dds_data_type = NULL; + } + diff --git a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataSupport.h.patch similarity index 59% rename from rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch rename to rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataSupport.h.patch index e519078d..285b19ed 100644 --- a/rmw_connext_cpp/resources/patch_files/connext_static_raw_dataSupport.h.patch +++ b/rmw_connext_cpp/resources/patch_files/connext_static_serialized_dataSupport.h.patch @@ -1,7 +1,7 @@ ---- a/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h -+++ b/rmw_connext_cpp/resources/patch_generated/connext_static_raw_dataSupport.h +--- a/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_dataSupport.h ++++ b/rmw_connext_cpp/resources/patch_generated/connext_static_serialized_dataSupport.h @@ -15,7 +15,7 @@ or consult the RTI Connext manual. - #include "connext_static_raw_data.h" + #include "connext_static_serialized_data.h" #ifndef ndds_cpp_h -#include "ndds/ndds_cpp.h" @@ -14,63 +14,63 @@ #endif -DDS_TYPESUPPORT_CPP( -- ConnextStaticRawDataTypeSupport, -- ConnextStaticRawData); -+class NDDSUSERDllExport DDSCPPDllExport ConnextStaticRawDataTypeSupport : public ::DDSTypeSupport +- ConnextStaticSerializedDataTypeSupport, +- ConnextStaticSerializedData); ++class NDDSUSERDllExport DDSCPPDllExport ConnextStaticSerializedDataTypeSupport : public ::DDSTypeSupport +{ +public: -+ ConnextStaticRawDataTypeSupport(bool osrf) ++ ConnextStaticSerializedDataTypeSupport(bool osrf) + { + (void) osrf; + } + -+ ~ConnextStaticRawDataTypeSupport(); ++ ~ConnextStaticSerializedDataTypeSupport(); + + static DDS_ReturnCode_t register_type( + DDSDomainParticipant * participant, -+ const char * type_name = "ConnextStaticRawData"); ++ const char * type_name = "ConnextStaticSerializedData"); + + static DDS_ReturnCode_t unregister_type( + DDSDomainParticipant * participant, -+ const char * type_name = "ConnextStaticRawData"); ++ const char * type_name = "ConnextStaticSerializedData"); + + static const char * get_type_name(); + -+ static ConnextStaticRawData * create_data_ex(DDS_Boolean allocatePointers); ++ static ConnextStaticSerializedData * create_data_ex(DDS_Boolean allocatePointers); + -+ static ConnextStaticRawData * create_data( ++ static ConnextStaticSerializedData * create_data( + const DDS_TypeAllocationParams_t & alloc_params = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + + static DDS_ReturnCode_t delete_data_ex( -+ ConnextStaticRawData * a_data, ++ ConnextStaticSerializedData * a_data, + DDS_Boolean deletePointers); + + static DDS_ReturnCode_t delete_data( -+ ConnextStaticRawData * a_data, ++ ConnextStaticSerializedData * a_data, + const DDS_TypeDeallocationParams_t & dealloc_params = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + -+ static void print_data(const ConnextStaticRawData * a_data); ++ static void print_data(const ConnextStaticSerializedData * a_data); + + static DDS_ReturnCode_t copy_data( -+ ConnextStaticRawData * dst_data, const ConnextStaticRawData * src_data); ++ ConnextStaticSerializedData * dst_data, const ConnextStaticSerializedData * src_data); + + static DDS_ReturnCode_t initialize_data_ex( -+ ConnextStaticRawData * a_data, ++ ConnextStaticSerializedData * a_data, + DDS_Boolean allocatePointers); + + static DDS_ReturnCode_t initialize_data( -+ ConnextStaticRawData * a_data, ++ ConnextStaticSerializedData * a_data, + const DDS_TypeAllocationParams_t & alloc_params = + DDS_TYPE_ALLOCATION_PARAMS_DEFAULT); + + static DDS_ReturnCode_t finalize_data_ex( -+ ConnextStaticRawData * a_data, ++ ConnextStaticSerializedData * a_data, + DDS_Boolean deletePointers); + + static DDS_ReturnCode_t finalize_data( -+ ConnextStaticRawData * a_data, ++ ConnextStaticSerializedData * a_data, + const DDS_TypeDeallocationParams_t & dealloc_params = + DDS_TYPE_DEALLOCATION_PARAMS_DEFAULT); + @@ -87,15 +87,15 @@ + static DDS_ReturnCode_t serialize_data_to_cdr_buffer( + char * buffer, + unsigned int & length, -+ const ConnextStaticRawData * a_data); ++ const ConnextStaticSerializedData * a_data); + + static DDS_ReturnCode_t deserialize_data_from_cdr_buffer( -+ ConnextStaticRawData * a_data, ++ ConnextStaticSerializedData * a_data, + const char * buffer, + unsigned int length); + + static DDS_ReturnCode_t data_to_string( -+ ConnextStaticRawData * sample, ++ ConnextStaticSerializedData * sample, + char * str, + DDS_UnsignedLong & str_size, + const DDS_PrintFormatProperty & property); @@ -103,15 +103,15 @@ + static void finalize(); + +private: -+ ConnextStaticRawDataTypeSupport(); ++ ConnextStaticSerializedDataTypeSupport(); +}; - DDS_DATAWRITER_CPP(ConnextStaticRawDataDataWriter, ConnextStaticRawData); - DDS_DATAREADER_CPP(ConnextStaticRawDataDataReader, ConnextStaticRawDataSeq, ConnextStaticRawData); + DDS_DATAWRITER_CPP(ConnextStaticSerializedDataDataWriter, ConnextStaticSerializedData); + DDS_DATAREADER_CPP(ConnextStaticSerializedDataDataReader, ConnextStaticSerializedDataSeq, ConnextStaticSerializedData); +NDDSUSERDllExport +DDS_ReturnCode_t -+ConnextStaticRawDataSupport_register_external_type( ++ConnextStaticSerializedDataSupport_register_external_type( + DDSDomainParticipant * participant, + const char * type_name, + struct DDS_TypeCode * type_code); diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 9eab7004..75d5fc47 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -22,19 +22,19 @@ #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" // include patched generated code from the build folder -#include "connext_static_raw_dataSupport.h" +#include "connext_static_serialized_dataSupport.h" bool publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) { - ConnextStaticRawDataDataWriter * data_writer = - ConnextStaticRawDataDataWriter::narrow(dds_data_writer); + ConnextStaticSerializedDataDataWriter * data_writer = + ConnextStaticSerializedDataDataWriter::narrow(dds_data_writer); if (!data_writer) { RMW_SET_ERROR_MSG("failed to narrow data writer"); return false; } - ConnextStaticRawData * instance = ConnextStaticRawDataTypeSupport::create_data(); + ConnextStaticSerializedData * instance = ConnextStaticSerializedDataTypeSupport::create_data(); if (!instance) { RMW_SET_ERROR_MSG("failed to create dds message instance"); return false; @@ -59,7 +59,7 @@ publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) fprintf(stderr, "failed to return loaned memory\n"); status = DDS_RETCODE_ERROR; } - ConnextStaticRawDataTypeSupport::delete_data(instance); + ConnextStaticSerializedDataTypeSupport::delete_data(instance); } return status == DDS_RETCODE_OK; @@ -114,7 +114,7 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) goto fail; } if (!cdr_stream.buffer) { - RMW_SET_ERROR_MSG("no raw message attached"); + RMW_SET_ERROR_MSG("no serialized message attached"); ret = RMW_RET_ERROR; goto fail; } @@ -130,7 +130,8 @@ rmw_publish(const rmw_publisher_t * publisher, const void * ros_message) } rmw_ret_t -rmw_publish_raw(const rmw_publisher_t * publisher, const rmw_message_raw_t * raw_message) +rmw_publish_serialized_message( + const rmw_publisher_t * publisher, const rmw_serialized_message_t * serialized_message) { if (!publisher) { RMW_SET_ERROR_MSG("publisher handle is null"); @@ -140,8 +141,8 @@ rmw_publish_raw(const rmw_publisher_t * publisher, const rmw_message_raw_t * raw RMW_SET_ERROR_MSG("publisher handle is not from this rmw implementation"); return RMW_RET_ERROR; } - if (!raw_message) { - RMW_SET_ERROR_MSG("raw message handle is null"); + if (!serialized_message) { + RMW_SET_ERROR_MSG("serialized message handle is null"); return RMW_RET_ERROR; } @@ -163,9 +164,9 @@ rmw_publish_raw(const rmw_publisher_t * publisher, const rmw_message_raw_t * raw } ConnextStaticCDRStream cdr_stream; - cdr_stream.buffer = raw_message->buffer; - cdr_stream.buffer_length = raw_message->buffer_length; - cdr_stream.buffer_capacity = raw_message->buffer_capacity; + cdr_stream.buffer = serialized_message->buffer; + cdr_stream.buffer_length = serialized_message->buffer_length; + cdr_stream.buffer_capacity = serialized_message->buffer_capacity; bool published = publish(topic_writer, &cdr_stream); if (!published) { RMW_SET_ERROR_MSG("failed to publish message"); diff --git a/rmw_connext_cpp/src/rmw_publisher.cpp b/rmw_connext_cpp/src/rmw_publisher.cpp index e1137c07..d10d74f8 100644 --- a/rmw_connext_cpp/src/rmw_publisher.cpp +++ b/rmw_connext_cpp/src/rmw_publisher.cpp @@ -30,7 +30,7 @@ #include "rmw_connext_cpp/connext_static_publisher_info.hpp" // include patched generated code from the build folder -#include "connext_static_raw_dataSupport.h" +#include "connext_static_serialized_dataSupport.h" // Uncomment this to get extra console output about discovery. // This affects code in this file, but there is a similar variable in: @@ -119,7 +119,7 @@ rmw_create_publisher( // which only publishes DDS_Octets // The purpose of this is to send only raw data DDS_Octets over the wire, // advertise the topic however with a type of the message, e.g. std_msgs::msg::dds_::String - status = ConnextStaticRawDataSupport_register_external_type( + status = ConnextStaticSerializedDataSupport_register_external_type( participant, type_name.c_str(), type_code); if (status != DDS_RETCODE_OK) { RMW_SET_ERROR_MSG("failed to register external type"); diff --git a/rmw_connext_cpp/src/rmw_serialize.cpp b/rmw_connext_cpp/src/rmw_serialize.cpp index dc9536ba..8d01a5b0 100644 --- a/rmw_connext_cpp/src/rmw_serialize.cpp +++ b/rmw_connext_cpp/src/rmw_serialize.cpp @@ -18,7 +18,7 @@ #include "./type_support_common.hpp" // include patched generated code from the build folder -#include "connext_static_raw_dataSupport.h" +#include "connext_static_serialized_dataSupport.h" extern "C" { @@ -26,7 +26,7 @@ rmw_ret_t rmw_serialize( const void * ros_message, const rosidl_message_type_support_t * type_support, - rmw_message_raw_t * raw_message) + rmw_serialized_message_t * serialized_message) { RMW_CONNEXT_EXTRACT_MESSAGE_TYPESUPPORT(type_support, ts, RMW_RET_ERROR) @@ -38,24 +38,24 @@ rmw_serialize( } ConnextStaticCDRStream cdr_stream; - cdr_stream.buffer = raw_message->buffer; - cdr_stream.buffer_length = raw_message->buffer_length; - cdr_stream.buffer_capacity = raw_message->buffer_capacity; - cdr_stream.allocator = raw_message->allocator; + cdr_stream.buffer = serialized_message->buffer; + cdr_stream.buffer_length = serialized_message->buffer_length; + cdr_stream.buffer_capacity = serialized_message->buffer_capacity; + cdr_stream.allocator = serialized_message->allocator; if (!callbacks->to_cdr_stream(ros_message, &cdr_stream)) { RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); return RMW_RET_ERROR; } - raw_message->buffer = cdr_stream.buffer; // reassgin buffer because it might have been resized - raw_message->buffer_length = cdr_stream.buffer_length; - raw_message->buffer_capacity = cdr_stream.buffer_capacity; + serialized_message->buffer = cdr_stream.buffer; // reassgin buffer because it might have been resized + serialized_message->buffer_length = cdr_stream.buffer_length; + serialized_message->buffer_capacity = cdr_stream.buffer_capacity; return RMW_RET_OK; } rmw_ret_t rmw_deserialize( - const rmw_message_raw_t * raw_message, + const rmw_serialized_message_t * serialized_message, const rosidl_message_type_support_t * type_support, void * ros_message) { @@ -69,10 +69,10 @@ rmw_deserialize( } ConnextStaticCDRStream cdr_stream; - cdr_stream.buffer = raw_message->buffer; - cdr_stream.buffer_length = raw_message->buffer_length; - cdr_stream.buffer_capacity = raw_message->buffer_capacity; - cdr_stream.allocator = raw_message->allocator; + cdr_stream.buffer = serialized_message->buffer; + cdr_stream.buffer_length = serialized_message->buffer_length; + cdr_stream.buffer_capacity = serialized_message->buffer_capacity; + cdr_stream.allocator = serialized_message->allocator; // convert the cdr stream to the message if (!callbacks->to_message(&cdr_stream, ros_message)) { RMW_SET_ERROR_MSG("can't convert cdr stream to ros message"); diff --git a/rmw_connext_cpp/src/rmw_subscription.cpp b/rmw_connext_cpp/src/rmw_subscription.cpp index 00e62351..b548a9f5 100644 --- a/rmw_connext_cpp/src/rmw_subscription.cpp +++ b/rmw_connext_cpp/src/rmw_subscription.cpp @@ -29,7 +29,7 @@ #include "rmw_connext_cpp/connext_static_subscriber_info.hpp" // include patched generated code from the build folder -#include "connext_static_raw_dataSupport.h" +#include "connext_static_serialized_dataSupport.h" // Uncomment this to get extra console output about discovery. // This affects code in this file, but there is a similar variable in: @@ -115,7 +115,7 @@ rmw_create_subscription( // which only publishes DDS_Octets // The purpose of this is to send only raw data DDS_Octets over the wire, // advertise the topic however with a type of the message, e.g. std_msgs::msg::dds_::String - status = ConnextStaticRawDataSupport_register_external_type( + status = ConnextStaticSerializedDataSupport_register_external_type( participant, type_name.c_str(), type_code); if (status != DDS_RETCODE_OK) { RMW_SET_ERROR_MSG("failed to register external type"); diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index efc27342..bdc9c8ff 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -24,8 +24,8 @@ #include "rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp" // include patched generated code from the build folder -#include "./connext_static_raw_dataSupport.h" -#include "./connext_static_raw_data.h" +#include "./connext_static_serialized_dataSupport.h" +#include "./connext_static_serialized_data.h" static bool take( @@ -48,14 +48,14 @@ take( return false; } - ConnextStaticRawDataDataReader * data_reader = - ConnextStaticRawDataDataReader::narrow(dds_data_reader); + ConnextStaticSerializedDataDataReader * data_reader = + ConnextStaticSerializedDataDataReader::narrow(dds_data_reader); if (!data_reader) { RMW_SET_ERROR_MSG("failed to narrow data reader"); return false; } - ConnextStaticRawDataSeq dds_messages; + ConnextStaticSerializedDataSeq dds_messages; DDS_SampleInfoSeq sample_infos; bool ignore_sample = false; @@ -177,7 +177,7 @@ _take( return RMW_RET_ERROR; } - // the call to take allocates memory for the raw message + // the call to take allocates memory for the serialized message // we have to free this here again free(cdr_stream.buffer); @@ -218,9 +218,9 @@ rmw_take_with_info( } rmw_ret_t -_take_raw( +_take_serialized_message( const rmw_subscription_t * subscription, - rmw_message_raw_t * raw_message, + rmw_serialized_message_t * serialized_message, bool * taken, DDS_InstanceHandle_t * sending_publication_handle) { @@ -233,7 +233,7 @@ _take_raw( subscription->implementation_identifier, rti_connext_identifier, return RMW_RET_ERROR) - if (!raw_message) { + if (!serialized_message) { RMW_SET_ERROR_MSG("ros message handle is null"); return RMW_RET_ERROR; } @@ -269,27 +269,27 @@ _take_raw( return RMW_RET_ERROR; } - raw_message->buffer_length = cdr_stream.buffer_length; + serialized_message->buffer_length = cdr_stream.buffer_length; // we don't free the allocated memory // and thus can directly set the pointer - raw_message->buffer = cdr_stream.buffer; + serialized_message->buffer = cdr_stream.buffer; return RMW_RET_OK; } rmw_ret_t -rmw_take_raw( +rmw_take_serialized_message( const rmw_subscription_t * subscription, - rmw_message_raw_t * raw_message, + rmw_serialized_message_t * serialized_message, bool * taken) { - return _take_raw(subscription, raw_message, taken, nullptr); + return _take_serialized_message(subscription, serialized_message, taken, nullptr); } rmw_ret_t -rmw_take_raw_with_info( +rmw_take_serialized_message_with_info( const rmw_subscription_t * subscription, - rmw_message_raw_t * raw_message, + rmw_serialized_message_t * serialized_message, bool * taken, rmw_message_info_t * message_info) { @@ -298,7 +298,8 @@ rmw_take_raw_with_info( return RMW_RET_ERROR; } DDS_InstanceHandle_t sending_publication_handle; - auto ret = _take_raw(subscription, raw_message, taken, &sending_publication_handle); + auto ret = + _take_serialized_message(subscription, serialized_message, taken, &sending_publication_handle); if (ret != RMW_RET_OK) { // Error string is already set. return RMW_RET_ERROR; From 9cea055a5fa4c818510d1536879fe467d5f38707 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 14 Jun 2018 06:03:15 -0700 Subject: [PATCH 30/34] use size_t (#289) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * use size_t * fix generated code’s support for size_t --- rmw_connext_cpp/src/rmw_take.cpp | 2 +- .../resource/msg__type_support_c.cpp.em | 22 +++++++++++-- .../connext_static_cdr_stream.hpp | 6 ++-- .../resource/msg__type_support.cpp.em | 33 ++++++++++++++++--- 4 files changed, 52 insertions(+), 11 deletions(-) diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index bdc9c8ff..d9be1b5d 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -105,7 +105,7 @@ take( // TODO(karsten1987): This malloc has to go! cdr_stream->buffer = reinterpret_cast(malloc(cdr_stream->buffer_length * sizeof(char))); - for (unsigned int i = 0; i < cdr_stream->buffer_length; ++i) { + for (size_t i = 0; i < cdr_stream->buffer_length; ++i) { cdr_stream->buffer[i] = dds_messages[0].serialized_data[i]; } *taken = true; diff --git a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em index e17b066e..f0d7ad21 100644 --- a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em +++ b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em @@ -340,9 +340,16 @@ to_cdr_stream( } // call the serialize function for the first time to get the expected length of the message + unsigned int expected_length; if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( - NULL, &cdr_stream->buffer_length, &dds_message) != RTI_TRUE) + NULL, &expected_length, &dds_message) != RTI_TRUE) { + fprintf(stderr, "failed to call @(spec.base_type.type)_Plugin_serialize_to_cdr_buffer()\n"); + return false; + } + cdr_stream->buffer_length = expected_length; + if (cdr_stream->buffer_length > std::numeric_limits::max()) { + fprintf(stderr, "cdr_stream->buffer_length, unexpectedly larger than max unsigned int\n"); return false; } if (cdr_stream->buffer_capacity < cdr_stream->buffer_length) { @@ -350,8 +357,11 @@ to_cdr_stream( cdr_stream->buffer = static_cast(cdr_stream->allocator.allocate(cdr_stream->buffer_length, cdr_stream->allocator.state)); } // call the function again and fill the buffer this time + unsigned int buffer_length_uint = static_cast(cdr_stream->buffer_length); if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( - cdr_stream->buffer, &cdr_stream->buffer_length, &dds_message) != RTI_TRUE) + cdr_stream->buffer, + &buffer_length_uint, + &dds_message) != RTI_TRUE) { return false; } @@ -373,8 +383,14 @@ to_message( @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); + if (cdr_stream->buffer_length > std::numeric_limits::max()) { + fprintf(stderr, "cdr_stream->buffer_length, unexpectedly larger than max unsigned int\n"); + return false; + } if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer( - dds_message, cdr_stream->buffer, cdr_stream->buffer_length) != RTI_TRUE) + dds_message, + cdr_stream->buffer, + static_cast(cdr_stream->buffer_length)) != RTI_TRUE) { fprintf(stderr, "deserialize from cdr buffer failed\n"); return false; diff --git a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp index 3aa39896..da719a40 100644 --- a/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp +++ b/rosidl_typesupport_connext_cpp/include/rosidl_typesupport_connext_cpp/connext_static_cdr_stream.hpp @@ -19,13 +19,13 @@ extern "C" { -// TODO(karsten1987): Join rmw_raw_message_t and this ConnextStaticCDRStream struct +// TODO(karsten1987): Join rmw_serialized_message_t and this ConnextStaticCDRStream struct // inside rcutils to have only one implementation of it. typedef struct ConnextStaticCDRStream { char * buffer = NULL; - unsigned int buffer_length = 0; - unsigned int buffer_capacity = 0; + size_t buffer_length = 0; + size_t buffer_capacity = 0; rcutils_allocator_t allocator; } ConnextStaticCDRStream; } // extern "C" diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index 65cf56a3..212886b1 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -220,7 +220,18 @@ to_cdr_stream__@(spec.base_type.type)( } // call the serialize function for the first time to get the expected length of the message - if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(NULL, &cdr_stream->buffer_length, dds_message) != RTI_TRUE) { + unsigned int expected_length; + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( + NULL, + &expected_length, + dds_message) != RTI_TRUE) + { + fprintf(stderr, "failed to call @(spec.base_type.type)_Plugin_serialize_to_cdr_buffer()\n"); + return false; + } + cdr_stream->buffer_length = expected_length; + if (cdr_stream->buffer_length > std::numeric_limits::max()) { + fprintf(stderr, "cdr_stream->buffer_length, unexpectedly larger than max unsigned int\n"); return false; } if (cdr_stream->buffer_capacity < cdr_stream->buffer_length) { @@ -228,7 +239,12 @@ to_cdr_stream__@(spec.base_type.type)( cdr_stream->buffer = static_cast(cdr_stream->allocator.allocate(cdr_stream->buffer_length, cdr_stream->allocator.state)); } // call the function again and fill the buffer this time - if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer(cdr_stream->buffer, &cdr_stream->buffer_length, dds_message) != RTI_TRUE) { + unsigned int buffer_length_uint = static_cast(cdr_stream->buffer_length); + if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( + cdr_stream->buffer, + &buffer_length_uint, + dds_message) != RTI_TRUE) + { return false; } if (@(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::delete_data(dds_message) != DDS_RETCODE_OK) { @@ -252,8 +268,17 @@ to_message__@(spec.base_type.type)( return false; } - @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); - if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer(dds_message, cdr_stream->buffer, cdr_stream->buffer_length) != RTI_TRUE) { + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = + @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); + if (cdr_stream->buffer_length > std::numeric_limits::max()) { + fprintf(stderr, "cdr_stream->buffer_length, unexpectedly larger than max unsigned int\n"); + return false; + } + if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer( + dds_message, + cdr_stream->buffer, + static_cast(cdr_stream->buffer_length)) != RTI_TRUE) + { fprintf(stderr, "deserialize from cdr buffer failed\n"); return false; } From e74a72de3b5e996964459be1f7e212791c3d1a85 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 14 Jun 2018 17:45:52 +0200 Subject: [PATCH 31/34] fix uncrustify --- rmw_connext_cpp/src/rmw_serialize.cpp | 3 ++- .../resource/msg__type_support.cpp.em | 18 +++++++++--------- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/rmw_connext_cpp/src/rmw_serialize.cpp b/rmw_connext_cpp/src/rmw_serialize.cpp index 8d01a5b0..8ffafd38 100644 --- a/rmw_connext_cpp/src/rmw_serialize.cpp +++ b/rmw_connext_cpp/src/rmw_serialize.cpp @@ -47,7 +47,8 @@ rmw_serialize( RMW_SET_ERROR_MSG("failed to convert ros_message to cdr stream"); return RMW_RET_ERROR; } - serialized_message->buffer = cdr_stream.buffer; // reassgin buffer because it might have been resized + // reassgin buffer because it might have been resized + serialized_message->buffer = cdr_stream.buffer; serialized_message->buffer_length = cdr_stream.buffer_length; serialized_message->buffer_capacity = cdr_stream.buffer_capacity; return RMW_RET_OK; diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index 212886b1..b68ce809 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -222,9 +222,9 @@ to_cdr_stream__@(spec.base_type.type)( // call the serialize function for the first time to get the expected length of the message unsigned int expected_length; if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( - NULL, - &expected_length, - dds_message) != RTI_TRUE) + NULL, + &expected_length, + dds_message) != RTI_TRUE) { fprintf(stderr, "failed to call @(spec.base_type.type)_Plugin_serialize_to_cdr_buffer()\n"); return false; @@ -241,9 +241,9 @@ to_cdr_stream__@(spec.base_type.type)( // call the function again and fill the buffer this time unsigned int buffer_length_uint = static_cast(cdr_stream->buffer_length); if (@(spec.base_type.type)_Plugin_serialize_to_cdr_buffer( - cdr_stream->buffer, - &buffer_length_uint, - dds_message) != RTI_TRUE) + cdr_stream->buffer, + &buffer_length_uint, + dds_message) != RTI_TRUE) { return false; } @@ -275,9 +275,9 @@ to_message__@(spec.base_type.type)( return false; } if (@(spec.base_type.type)_Plugin_deserialize_from_cdr_buffer( - dds_message, - cdr_stream->buffer, - static_cast(cdr_stream->buffer_length)) != RTI_TRUE) + dds_message, + cdr_stream->buffer, + static_cast(cdr_stream->buffer_length)) != RTI_TRUE) { fprintf(stderr, "deserialize from cdr buffer failed\n"); return false; From 8eaeb9e7e8899a4797810fbcf8919b1c9d5867a0 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 15 Jun 2018 16:45:05 -0700 Subject: [PATCH 32/34] fix remaining cases where Windows messes up max() --- .../resource/msg__type_support_c.cpp.em | 4 ++-- .../resource/msg__type_support.cpp.em | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em index f0d7ad21..a1ae9ed9 100644 --- a/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em +++ b/rosidl_typesupport_connext_c/resource/msg__type_support_c.cpp.em @@ -348,7 +348,7 @@ to_cdr_stream( return false; } cdr_stream->buffer_length = expected_length; - if (cdr_stream->buffer_length > std::numeric_limits::max()) { + if (cdr_stream->buffer_length > (std::numeric_limits::max)()) { fprintf(stderr, "cdr_stream->buffer_length, unexpectedly larger than max unsigned int\n"); return false; } @@ -383,7 +383,7 @@ to_message( @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); - if (cdr_stream->buffer_length > std::numeric_limits::max()) { + if (cdr_stream->buffer_length > (std::numeric_limits::max)()) { fprintf(stderr, "cdr_stream->buffer_length, unexpectedly larger than max unsigned int\n"); return false; } diff --git a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em index b68ce809..8c86fa4e 100644 --- a/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em +++ b/rosidl_typesupport_connext_cpp/resource/msg__type_support.cpp.em @@ -230,7 +230,7 @@ to_cdr_stream__@(spec.base_type.type)( return false; } cdr_stream->buffer_length = expected_length; - if (cdr_stream->buffer_length > std::numeric_limits::max()) { + if (cdr_stream->buffer_length > (std::numeric_limits::max)()) { fprintf(stderr, "cdr_stream->buffer_length, unexpectedly larger than max unsigned int\n"); return false; } @@ -270,7 +270,7 @@ to_message__@(spec.base_type.type)( @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_ * dds_message = @(spec.base_type.pkg_name)::@(subfolder)::dds_::@(spec.base_type.type)_TypeSupport::create_data(); - if (cdr_stream->buffer_length > std::numeric_limits::max()) { + if (cdr_stream->buffer_length > (std::numeric_limits::max)()) { fprintf(stderr, "cdr_stream->buffer_length, unexpectedly larger than max unsigned int\n"); return false; } From f7786242d8a3f26c19516820d199a035fab734f4 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 15 Jun 2018 18:48:42 -0700 Subject: [PATCH 33/34] fix warnings on Windows --- rmw_connext_cpp/src/rmw_publish.cpp | 8 +++++++- rmw_connext_cpp/src/rmw_take.cpp | 11 ++++++++++- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index 75d5fc47..aa81d432 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rmw/error_handling.h" #include "rmw/rmw.h" #include "rmw/types.h" @@ -43,9 +45,13 @@ publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) DDS_ReturnCode_t status = DDS_RETCODE_ERROR; instance->serialized_data.maximum(0); + if (cdr_stream->buffer_length > (std::numeric_limits::max)()) { + RMW_SET_ERROR_MSG("cdr_stream->buffer_length unexpectedly larger than DDS_Long's max value"); + return false; + } if (!instance->serialized_data.loan_contiguous( reinterpret_cast(cdr_stream->buffer), - cdr_stream->buffer_length, cdr_stream->buffer_length)) + static_cast(cdr_stream->buffer_length), cdr_stream->buffer_length)) { RMW_SET_ERROR_MSG("failed to loan memory for message"); goto cleanup; diff --git a/rmw_connext_cpp/src/rmw_take.cpp b/rmw_connext_cpp/src/rmw_take.cpp index d9be1b5d..e174e85d 100644 --- a/rmw_connext_cpp/src/rmw_take.cpp +++ b/rmw_connext_cpp/src/rmw_take.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rmw/error_handling.h" #include "rmw/impl/cpp/macros.hpp" #include "rmw/types.h" @@ -105,7 +107,14 @@ take( // TODO(karsten1987): This malloc has to go! cdr_stream->buffer = reinterpret_cast(malloc(cdr_stream->buffer_length * sizeof(char))); - for (size_t i = 0; i < cdr_stream->buffer_length; ++i) { + + if (cdr_stream->buffer_length > (std::numeric_limits::max)()) { + RMW_SET_ERROR_MSG("cdr_stream->buffer_length unexpectedly larger than max unsiged int value"); + data_reader->return_loan(dds_messages, sample_infos); + *taken = false; + return DDS_RETCODE_ERROR; + } + for (unsigned int i = 0; i < static_cast(cdr_stream->buffer_length); ++i) { cdr_stream->buffer[i] = dds_messages[0].serialized_data[i]; } *taken = true; From 27cd7636aaa89d76002cd11cffa8bb54082a72d8 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 15 Jun 2018 21:40:18 -0700 Subject: [PATCH 34/34] fix second argument that needs casting --- rmw_connext_cpp/src/rmw_publish.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmw_connext_cpp/src/rmw_publish.cpp b/rmw_connext_cpp/src/rmw_publish.cpp index aa81d432..e5226409 100644 --- a/rmw_connext_cpp/src/rmw_publish.cpp +++ b/rmw_connext_cpp/src/rmw_publish.cpp @@ -51,7 +51,8 @@ publish(DDSDataWriter * dds_data_writer, ConnextStaticCDRStream * cdr_stream) } if (!instance->serialized_data.loan_contiguous( reinterpret_cast(cdr_stream->buffer), - static_cast(cdr_stream->buffer_length), cdr_stream->buffer_length)) + static_cast(cdr_stream->buffer_length), + static_cast(cdr_stream->buffer_length))) { RMW_SET_ERROR_MSG("failed to loan memory for message"); goto cleanup;