From 4155dc04ffc1f8d9ee7b210a39edcb6151ce7a3f Mon Sep 17 00:00:00 2001 From: Aleksandar Brakmic Date: Wed, 7 Oct 2020 12:58:13 +0200 Subject: [PATCH 1/2] Version 0.4.1 of eCAL RMW. * Performance improvements for messages containing primitive type arrays * Bug fixes --- Dockerfile | 22 --- Jenkinsfile | 51 ------- rmw_ecal_dynamic_cpp/CHANGELOG.rst | 5 + rmw_ecal_dynamic_cpp/package.xml | 2 +- .../src/internal/ros_array_iterator.hpp | 19 +++ .../serialization/custom/deserializer_c.cpp | 87 ++++++------ .../serialization/custom/deserializer_cpp.cpp | 37 +++-- .../serialization/custom/serializer_c.cpp | 53 ++++--- .../serialization/custom/serializer_cpp.cpp | 59 ++++---- .../serialization/protobuf/deserializer_c.cpp | 132 +++++++++--------- 10 files changed, 214 insertions(+), 253 deletions(-) delete mode 100644 Dockerfile delete mode 100644 Jenkinsfile diff --git a/Dockerfile b/Dockerfile deleted file mode 100644 index 6b151f4..0000000 --- a/Dockerfile +++ /dev/null @@ -1,22 +0,0 @@ -FROM eu.artifactory.conti.de:7007/artifactory/c_chs_ctzs_docker_l/ecal-bionic-amd64:latest - -ARG ros_distro - -RUN apt update -RUN DEBIAN_FRONTEND=noninteractive -RUN apt -f install -y --allow-unauthenticated curl gnupg2 lsb-release - -RUN echo "deb https://eu.artifactory.conti.de/artifactory/i_adas_ubuntu_ros_r `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list -RUN curl -O -s -S -k -tlsv1 https://eu-test.artifactory.conti.de/artifactory/c_chs_ctzs_generic_l/external/ros/ros.asc -RUN apt-key add ros.asc - -RUN apt update - -RUN apt install -y apt-transport-https -RUN apt install -y ros-${ros_distro}-ros-base -RUN apt install -y python3-colcon-common-extensions - -RUN apt -y upgrade - -RUN curl -s -S -k -tlsv1 https://github.conti.de/storage/releases/13433/files/10987 -o ecal.deb -RUN dpkg -i ecal.deb \ No newline at end of file diff --git a/Jenkinsfile b/Jenkinsfile deleted file mode 100644 index 3382a13..0000000 --- a/Jenkinsfile +++ /dev/null @@ -1,51 +0,0 @@ -pipeline { - agent none - - stages { - stage('Build') { - parallel { - stage('Linux - Eloquent') { - agent { - dockerfile { - additionalBuildArgs "--pull --build-arg ros_distro=eloquent" - label 'DockerHost' - } - } - - stages { - stage('Build') { - steps { - checkout scm - sh ''' - . /opt/ros/eloquent/setup.sh - colcon build - ''' - } - } - } - } - - stage('Linux - Dashing') { - agent { - dockerfile { - additionalBuildArgs "--pull --build-arg ros_distro=dashing" - label 'DockerHost' - } - } - - stages { - stage('Build') { - steps { - checkout scm - sh ''' - . /opt/ros/dashing/setup.sh - colcon build - ''' - } - } - } - } - } - } - } -} \ No newline at end of file diff --git a/rmw_ecal_dynamic_cpp/CHANGELOG.rst b/rmw_ecal_dynamic_cpp/CHANGELOG.rst index 5433cac..532d3e3 100644 --- a/rmw_ecal_dynamic_cpp/CHANGELOG.rst +++ b/rmw_ecal_dynamic_cpp/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package rmw_ecal_dynamic_cpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.4.1 (2020-10-05) +------------------ +* Performance improvements for messages containing primitive type arrays +* Bug fixes + 0.4.0 (2020-08-13) ------------------ * Subscribers will now receive messages from publishers that are running in same process diff --git a/rmw_ecal_dynamic_cpp/package.xml b/rmw_ecal_dynamic_cpp/package.xml index 945bed3..7c8b810 100644 --- a/rmw_ecal_dynamic_cpp/package.xml +++ b/rmw_ecal_dynamic_cpp/package.xml @@ -2,7 +2,7 @@ rmw_ecal_dynamic_cpp - 0.4.0 + 0.4.1 RMW implementation based on eCAL. Aleksandar Brakmić Apache License 2.0 diff --git a/rmw_ecal_dynamic_cpp/src/internal/ros_array_iterator.hpp b/rmw_ecal_dynamic_cpp/src/internal/ros_array_iterator.hpp index 9159c26..6de89bb 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/ros_array_iterator.hpp +++ b/rmw_ecal_dynamic_cpp/src/internal/ros_array_iterator.hpp @@ -1,3 +1,22 @@ +/* ========================= RMW eCAL LICENSE ================================= +* +* Copyright (C) 2019 - 2020 Continental Corporation +* +* 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. +* +* ========================= RMW eCAL LICENSE ================================= +*/ + #pragma once #include diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_c.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_c.cpp index 28e3ed2..76657e3 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_c.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_c.cpp @@ -24,6 +24,9 @@ #include #include +#include +#include +#include #include #include "internal/common.hpp" @@ -40,45 +43,24 @@ void CDeserializer::DeserializeSingle(char *member, const char **serialized_data *serialized_data += sizeof(T); } -template -void CDeserializer::DeserializeArray(char *member, size_t size, const char **serialized_data) -{ - for (size_t i = 0; i < size; i++) - { - DeserializeSingle(member, serialized_data); - member += sizeof(T); - } -} - -template -void CDeserializer::DeserializeDynamicArray(char *member, const char **serialized_data) -{ - auto arr_size = *reinterpret_cast(*serialized_data); - *serialized_data += sizeof(array_size_t); - - auto sequence = reinterpret_cast(member); - sequence->size = arr_size; - sequence->capacity = arr_size; - if(arr_size > 0) - { - sequence->data = new signed char[arr_size * sizeof(T)]; - DeserializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); - } -} - template <> void CDeserializer::DeserializeSingle(char *member, const char **serialized_data) { - auto arr_size = *reinterpret_cast(*serialized_data); + auto size = *reinterpret_cast(*serialized_data); *serialized_data += sizeof(array_size_t); - auto sequence = reinterpret_cast(member); - sequence->size = arr_size; - sequence->capacity = arr_size + 1; - sequence->data = new signed char[arr_size + 1]; + auto sequence = reinterpret_cast(member); + rosidl_generator_c__String__init(sequence); + rosidl_generator_c__String__assignn(sequence, *serialized_data, size); + + *serialized_data += size; +} - DeserializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); - sequence->data[arr_size] = '\0'; +template +void CDeserializer::DeserializeArray(char *member, size_t size, const char **serialized_data) +{ + std::copy_n(*serialized_data, sizeof(T) * size, member); + *serialized_data += sizeof(T) * size; } template <> @@ -87,36 +69,52 @@ void CDeserializer::DeserializeArray(char *member, size_t size, con for (size_t i = 0; i < size; i++) { DeserializeSingle(member, serialized_data); - member += sizeof(rosidl_generator_c__char__Sequence); + member += sizeof(rosidl_generator_c__String__Sequence); } } template <> -void CDeserializer::DeserializeDynamicArray(char *member, const char **serialized_data) +void CDeserializer::DeserializeArray(char *message, + const rosidl_typesupport_introspection_c__MessageMember *member, + const char **serialized_data) +{ + auto sub_members = GetMembers(member); + for (size_t i = 0; i < member->array_size_; i++) + { + DeserializeMessage(serialized_data, sub_members, message); + message += sub_members->size_of_; + } +} + +template +void CDeserializer::DeserializeDynamicArray(char *member, const char **serialized_data) { auto arr_size = *reinterpret_cast(*serialized_data); *serialized_data += sizeof(array_size_t); auto sequence = reinterpret_cast(member); + sequence->data = nullptr; sequence->size = arr_size; sequence->capacity = arr_size; if(arr_size > 0) { - sequence->data = new signed char[arr_size * sizeof(rosidl_generator_c__char__Sequence)]; - DeserializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); + sequence->data = new signed char[arr_size * sizeof(T)]; + DeserializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); } } template <> -void CDeserializer::DeserializeArray(char *message, - const rosidl_typesupport_introspection_c__MessageMember *member, - const char **serialized_data) +void CDeserializer::DeserializeDynamicArray(char *member, const char **serialized_data) { - auto sub_members = GetMembers(member); - for (size_t i = 0; i < member->array_size_; i++) + auto arr_size = *reinterpret_cast(*serialized_data); + *serialized_data += sizeof(array_size_t); + + auto sequence = reinterpret_cast(member); + rosidl_generator_c__String__Sequence__init(sequence, arr_size); + + if(arr_size > 0) { - DeserializeMessage(serialized_data, sub_members, message); - message += sub_members->size_of_; + DeserializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); } } @@ -129,6 +127,7 @@ void CDeserializer::DeserializeDynamicArray(char *message, *serialized_data += sizeof(array_size_t); auto sequence = reinterpret_cast(message); + sequence->data = nullptr; sequence->size = arr_size; sequence->capacity = arr_size; diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_cpp.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_cpp.cpp index 43847ed..69dbb7b 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_cpp.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_cpp.cpp @@ -66,11 +66,31 @@ array_size_t CppDeserializer::DeserializeArraySize(const char **serialized_data) template void CppDeserializer::DeserializeArray(char *member, size_t size, const char **serialized_data) +{ + std::copy_n(*serialized_data, sizeof(T) * size, member); + *serialized_data += sizeof(T) * size; +} + +template <> +void CppDeserializer::DeserializeArray(char *member, size_t size, const char **serialized_data) { for (size_t i = 0; i < size; i++) { - DeserializeSingle(member, serialized_data); - member += sizeof(T); + DeserializeSingle(member, serialized_data); + member += sizeof(std::string); + } +} + +template <> +void CppDeserializer::DeserializeArray(char *message, + const ts_introspection::MessageMember *member, + const char **serialized_data) +{ + auto sub_members = GetMembers(member); + for (size_t i = 0; i < member->array_size_; i++) + { + DeserializeMessage(serialized_data, sub_members, message); + message += sub_members->size_of_; } } @@ -98,19 +118,6 @@ void CppDeserializer::DeserializeDynamicArray(char *member, const char **s *serialized_data += arr_size; } -template <> -void CppDeserializer::DeserializeArray(char *message, - const ts_introspection::MessageMember *member, - const char **serialized_data) -{ - auto sub_members = GetMembers(member); - for (size_t i = 0; i < member->array_size_; i++) - { - DeserializeMessage(serialized_data, sub_members, message); - message += sub_members->size_of_; - } -} - template <> void CppDeserializer::DeserializeDynamicArray(char *message, const ts_introspection::MessageMember *member, diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_c.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_c.cpp index 737f693..2040995 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_c.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_c.cpp @@ -46,30 +46,17 @@ void CSerializer::SerializeSingle(const T &data, std::string &serialized_data) c SerializeSingle(data_bytes, serialized_data); } -template -void CSerializer::SerializeArray(const char *data, size_t count, std::string &serialized_data) const +template <> +void CSerializer::SerializeSingle(const char *data, std::string &serialized_data) const { - for (size_t i = 0; i < count; i++) - { - SerializeSingle(data, serialized_data); - data += sizeof(T); - } + SerializeDynamicArray(data, serialized_data); } template -void CSerializer::SerializeDynamicArray(const char *data, std::string &serialized_data) const -{ - auto sequence = reinterpret_cast(data); - - serialized_data.reserve(serialized_data.size() + sequence->size * sizeof(T) + sizeof(array_size_t)); - SerializeSingle(sequence->size, serialized_data); - SerializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); -} - -template <> -void CSerializer::SerializeSingle(const char *data, std::string &serialized_data) const +void CSerializer::SerializeArray(const char *data, size_t count, std::string &serialized_data) const { - SerializeDynamicArray(data, serialized_data); + serialized_data.insert(serialized_data.end(), data, data + count * sizeof(T)); + data += sizeof(T) * count; } template <> @@ -82,15 +69,6 @@ void CSerializer::SerializeArray(const char *data, size_t count, st } } -template <> -void CSerializer::SerializeDynamicArray(const char *data, std::string &serialized_data) const -{ - auto sequence = reinterpret_cast(data); - - SerializeSingle(sequence->size, serialized_data); - SerializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); -} - template <> void CSerializer::SerializeArray(const char *data, const rosidl_typesupport_introspection_c__MessageMember *member, @@ -104,6 +82,25 @@ void CSerializer::SerializeArray(const char *data, } } +template +void CSerializer::SerializeDynamicArray(const char *data, std::string &serialized_data) const +{ + auto sequence = reinterpret_cast(data); + + serialized_data.reserve(serialized_data.size() + sequence->size * sizeof(T) + sizeof(array_size_t)); + SerializeSingle(sequence->size, serialized_data); + SerializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); +} + +template <> +void CSerializer::SerializeDynamicArray(const char *data, std::string &serialized_data) const +{ + auto sequence = reinterpret_cast(data); + + SerializeSingle(sequence->size, serialized_data); + SerializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); +} + template <> void CSerializer::SerializeDynamicArray(const char *data, const rosidl_typesupport_introspection_c__MessageMember *member, diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_cpp.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_cpp.cpp index da5dfd0..d7cbd59 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_cpp.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_cpp.cpp @@ -49,6 +49,17 @@ void CppSerializer::SerializeSingle(const T &data, std::string &serialized_data) SerializeSingle(data_bytes, serialized_data); } +template <> +void CppSerializer::SerializeSingle(const char *data, std::string &serialized_data) const +{ + auto str = reinterpret_cast(data); + auto str_data = str->c_str(); + auto str_size = str->size(); + + SerializeArraySize(*str, serialized_data); + SerializeArray(str_data, str_size, serialized_data); +} + template void CppSerializer::SerializeArraySize(const ARR &array, std::string &serialized_data) const { @@ -58,11 +69,31 @@ void CppSerializer::SerializeArraySize(const ARR &array, std::string &serialized template void CppSerializer::SerializeArray(const char *data, size_t count, std::string &serialized_data) const +{ + serialized_data.insert(serialized_data.end(), data, data + count * sizeof(T)); + data += sizeof(T) * count; +} + +template <> +void CppSerializer::SerializeArray(const char *data, size_t count, std::string &serialized_data) const { for (size_t i = 0; i < count; i++) { - SerializeSingle(data, serialized_data); - data += sizeof(T); + SerializeSingle(data, serialized_data); + data += sizeof(std::string); + } +} + +template <> +void CppSerializer::SerializeArray(const char *data, + const ts_introspection::MessageMember *member, + std::string &serialized_data) const +{ + auto sub_members = GetMembers(member); + for (size_t i = 0; i < member->array_size_; i++) + { + SerializeMessage(data, sub_members, serialized_data); + data += sub_members->size_of_; } } @@ -80,17 +111,6 @@ void CppSerializer::SerializeDynamicArray(const char *data, std::string &seriali SerializeArray(array_data, array_size, serialized_data); } -template <> -void CppSerializer::SerializeSingle(const char *data, std::string &serialized_data) const -{ - auto str = reinterpret_cast(data); - auto str_data = str->c_str(); - auto str_size = str->size(); - - SerializeArraySize(*str, serialized_data); - SerializeArray(str_data, str_size, serialized_data); -} - template <> void CppSerializer::SerializeDynamicArray(const char *data, std::string &serialized_data) const { @@ -101,19 +121,6 @@ void CppSerializer::SerializeDynamicArray(const char *data, std::string &s serialized_data.insert(serialized_data.end(), array.begin(), array.end()); } -template <> -void CppSerializer::SerializeArray(const char *data, - const ts_introspection::MessageMember *member, - std::string &serialized_data) const -{ - auto sub_members = GetMembers(member); - for (size_t i = 0; i < member->array_size_; i++) - { - SerializeMessage(data, sub_members, serialized_data); - data += sub_members->size_of_; - } -} - template <> void CppSerializer::SerializeDynamicArray(const char *data, const ts_introspection::MessageMember *member, diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/deserializer_c.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/deserializer_c.cpp index 5eace3c..a5afb0c 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/deserializer_c.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/deserializer_c.cpp @@ -24,8 +24,11 @@ #include #include -#include #include +#include +#include +#include +#include #include "internal/common.hpp" #include "internal/serialization/protobuf/helpers.hpp" @@ -42,50 +45,50 @@ namespace rmw namespace pb = google::protobuf; -#define DEFINE_SET_METHODS(PB_NAME, TYPE) \ -template <> \ -void CProtobufDeserializer::SetSingle(char *ros_member, \ - const pb::Message *msg, \ - const pb::FieldDescriptor *field) const \ -{ \ - auto data = reinterpret_cast(ros_member); \ - auto ref = msg->GetReflection(); \ - *data = ref->Get##PB_NAME(*msg, field); \ -} \ - \ -template <> \ -void CProtobufDeserializer::SetArray(char *ros_member, \ - int array_size, \ - const pb::Message *msg, \ - const pb::FieldDescriptor *field) const \ -{ \ - auto data = reinterpret_cast(ros_member); \ - auto ref = msg->GetReflection(); \ - for (int i = 0; i < array_size; i++, data++) \ - { \ - *data = ref->GetRepeated##PB_NAME(*msg, field, i); \ - } \ -} \ - \ -template <> \ -void CProtobufDeserializer::SetDynamicArray(char *ros_member, \ - const pb::Message *msg, \ - const pb::FieldDescriptor *field) const \ -{ \ - auto sequence = reinterpret_cast(ros_member); \ - auto ref = msg->GetReflection(); \ - auto size = ref->FieldSize(*msg, field); \ - \ - sequence->size = size; \ - sequence->capacity = size; \ - if(size > 0) \ - { \ - sequence->data = new uint8_t[size * sizeof(TYPE)]; \ - SetArray(reinterpret_cast(sequence->data), \ - sequence->size, msg, field); \ - } \ - \ -} +#define DEFINE_SET_METHODS(PB_NAME, TYPE) \ + template <> \ + void CProtobufDeserializer::SetSingle(char *ros_member, \ + const pb::Message *msg, \ + const pb::FieldDescriptor *field) const \ + { \ + auto data = reinterpret_cast(ros_member); \ + auto ref = msg->GetReflection(); \ + *data = ref->Get##PB_NAME(*msg, field); \ + } \ + \ + template <> \ + void CProtobufDeserializer::SetArray(char *ros_member, \ + int array_size, \ + const pb::Message *msg, \ + const pb::FieldDescriptor *field) const \ + { \ + auto data = reinterpret_cast(ros_member); \ + auto ref = msg->GetReflection(); \ + for (int i = 0; i < array_size; i++, data++) \ + { \ + *data = ref->GetRepeated##PB_NAME(*msg, field, i); \ + } \ + } \ + \ + template <> \ + void CProtobufDeserializer::SetDynamicArray(char *ros_member, \ + const pb::Message *msg, \ + const pb::FieldDescriptor *field) const \ + { \ + auto sequence = reinterpret_cast(ros_member); \ + auto ref = msg->GetReflection(); \ + auto size = ref->FieldSize(*msg, field); \ + \ + sequence->data = nullptr; \ + sequence->size = size; \ + sequence->capacity = size; \ + if (size > 0) \ + { \ + sequence->data = new uint8_t[size * sizeof(TYPE)]; \ + SetArray(reinterpret_cast(sequence->data), \ + sequence->size, msg, field); \ + } \ + } DEFINE_SET_METHODS(Bool, bool) DEFINE_SET_METHODS(Int32, char) @@ -105,17 +108,15 @@ void CProtobufDeserializer::SetSingle(char *ros_member, const pb::Message *msg, const pb::FieldDescriptor *field) const { - auto data = reinterpret_cast(ros_member); + auto data = reinterpret_cast(ros_member); auto ref = msg->GetReflection(); const std::string &str = ref->GetStringReference(*msg, field, nullptr); + auto str_data = str.c_str(); auto size = str.size(); - data->size = size; - data->capacity = size + 1; - data->data = new signed char[data->capacity]; - auto str_data = reinterpret_cast(str.c_str()); - std::copy_n(str_data, size + 1, data->data); + rosidl_generator_c__String__init(data); + rosidl_generator_c__String__assignn(data, str_data, size); } template <> @@ -124,20 +125,19 @@ void CProtobufDeserializer::SetArray(char *ros_member, const pb::Message *msg, const pb::FieldDescriptor *field) const { - auto sequence = reinterpret_cast(ros_member); + auto sequence = reinterpret_cast(ros_member); auto ref = msg->GetReflection(); for (int i = 0; i < array_size; i++) { - auto data = reinterpret_cast(&sequence[i]); + auto data = &sequence[i]; + const std::string &str = ref->GetRepeatedStringReference(*msg, field, i, nullptr); auto size = str.size(); - data->size = size; - data->capacity = size + 1; - data->data = new signed char[data->capacity]; - auto str_data = reinterpret_cast(str.c_str()); + auto str_data = str.c_str(); - std::copy_n(str_data, size + 1, data->data); + rosidl_generator_c__String__init(data); + rosidl_generator_c__String__assignn(data, str_data, size); } } @@ -146,17 +146,15 @@ void CProtobufDeserializer::SetDynamicArray(char *ros_member, const pb::Message *msg, const pb::FieldDescriptor *field) const { - auto sequence = reinterpret_cast(ros_member); + auto sequence = reinterpret_cast(ros_member); auto ref = msg->GetReflection(); - auto array_size = ref->FieldSize(*msg, field); - sequence->size = array_size; - sequence->capacity = array_size; - if(array_size > 0) + auto size = ref->FieldSize(*msg, field); + rosidl_generator_c__String__Sequence__init(sequence, size); + + if(size > 0) { - auto sequence_data = new char[array_size * sizeof(rosidl_generator_c__char__Sequence)]; - SetArray(sequence_data, array_size, msg, field); - sequence->data = reinterpret_cast(sequence_data); + SetArray(reinterpret_cast(sequence->data), size, msg, field); } } @@ -197,8 +195,10 @@ void CProtobufDeserializer::SetDynamicArray(char *ros_member, auto ref = msg->GetReflection(); auto array_size = ref->FieldSize(*msg, field); + sequence->data = nullptr; sequence->size = array_size; sequence->capacity = array_size; + if(array_size > 0) { auto sequence_data = new char[array_size * members->size_of_]; From c7e57ca89f3911f5e758542de6a82d798c61d7b8 Mon Sep 17 00:00:00 2001 From: Aleksandar Brakmic Date: Mon, 19 Oct 2020 13:31:09 +0200 Subject: [PATCH 2/2] Version 0.5.0 of eCAL RMW. * Added Foxy support --- README.md | 5 +- rmw_ecal_dynamic_cpp/CHANGELOG.rst | 4 + rmw_ecal_dynamic_cpp/CMakeLists.txt | 10 +- rmw_ecal_dynamic_cpp/package.xml | 2 +- rmw_ecal_dynamic_cpp/src/event.cpp | 6 +- .../src/get_topic_endpoint_info.cpp | 47 ++++++ rmw_ecal_dynamic_cpp/src/internal/qos.hpp | 1 + .../rosidl_generator_c_pkg_adapter.hpp | 129 ++++++++++++++++ .../src/internal/serialization.hpp | 1 - .../serialization/custom/deserializer_c.cpp | 33 ++-- .../serialization/custom/serializer_c.cpp | 11 +- .../serialization/protobuf/deserializer_c.cpp | 133 ++++++++-------- .../serialization/protobuf/serializer_c.cpp | 16 +- .../src/internal/typesupport.hpp | 3 +- rmw_ecal_dynamic_cpp/src/rmw.cpp | 143 ++++++++++++++---- 15 files changed, 407 insertions(+), 137 deletions(-) create mode 100644 rmw_ecal_dynamic_cpp/src/get_topic_endpoint_info.cpp create mode 100644 rmw_ecal_dynamic_cpp/src/internal/rosidl_generator_c_pkg_adapter.hpp diff --git a/README.md b/README.md index fb8e9f6..20fb0a2 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ eCAL RMW can offer: ### Linux -* Install [eCAL](https://github.com/continental/ecal) +* Install [eCAL](https://github.conti.de/CTZS/ecal) * Clone latest release of this repository into your [ROS2 workspace](https://index.ros.org/doc/ros2/Tutorials/Workspace/Creating-A-Workspace/) * Source ROS2 `source /path/to/your/ros/distro/folder/setup.bash` * Run `colcon build` from your workspace folder @@ -21,7 +21,7 @@ eCAL RMW can offer: ### Windows -* Install [eCAL](https://github.com/continental/ecal) +* Install [eCAL](https://github.conti.de/CTZS/ecal) * Clone latest release of this repository into your [ROS2 workspace](https://index.ros.org/doc/ros2/Tutorials/Workspace/Creating-A-Workspace/) * Source ROS2 `call C:/path/to/your/ros/distro/folder/setup.bat` * Run `colcon build` from your workspace folder @@ -52,3 +52,4 @@ RMW can currently work with only one serialization method at the time. * Dashing Diademata * Eloquent Elusor +* Foxy Fitzroy \ No newline at end of file diff --git a/rmw_ecal_dynamic_cpp/CHANGELOG.rst b/rmw_ecal_dynamic_cpp/CHANGELOG.rst index 532d3e3..c1cd274 100644 --- a/rmw_ecal_dynamic_cpp/CHANGELOG.rst +++ b/rmw_ecal_dynamic_cpp/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package rmw_ecal_dynamic_cpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.5.0 (2020-10-19) +------------------ +* Added support for Foxy Fitzroy + 0.4.1 (2020-10-05) ------------------ * Performance improvements for messages containing primitive type arrays diff --git a/rmw_ecal_dynamic_cpp/CMakeLists.txt b/rmw_ecal_dynamic_cpp/CMakeLists.txt index fff1c91..743d3b3 100644 --- a/rmw_ecal_dynamic_cpp/CMakeLists.txt +++ b/rmw_ecal_dynamic_cpp/CMakeLists.txt @@ -33,10 +33,15 @@ if (CMAKE_CXX_COMPILER_ID MATCHES "MSVC") add_compile_definitions(_CRT_SECURE_NO_WARNINGS) endif() -if(NOT $ENV{ROS_DISTRO} MATCHES "eloquent" AND NOT $ENV{ROS_DISTRO} MATCHES "dashing") +if(NOT $ENV{ROS_DISTRO} MATCHES "dashing" AND NOT $ENV{ROS_DISTRO} MATCHES "eloquent" AND NOT $ENV{ROS_DISTRO} MATCHES "foxy") message(FATAL_ERROR "'$ENV{ROS_DISTRO}' is unsupported ros2 distro.") endif() -add_compile_definitions($) + +add_compile_definitions("DASHING=0") +add_compile_definitions("ELOQUENT=1") +add_compile_definitions("FOXY=2") + +add_compile_definitions("ROS_DISTRO=$") option(USE_PROTOBUF_SERIALIZATION "Use google protobuf as serialization format." OFF) if(USE_PROTOBUF_SERIALIZATION) @@ -69,6 +74,7 @@ add_library(${PROJECT_NAME} SHARED src/get_node_info_and_types.cpp src/get_service_names_and_types.cpp src/get_topic_names_and_types.cpp + src/get_topic_endpoint_info.cpp src/init_options.cpp src/init.cpp src/rmw.cpp diff --git a/rmw_ecal_dynamic_cpp/package.xml b/rmw_ecal_dynamic_cpp/package.xml index 7c8b810..f0862c2 100644 --- a/rmw_ecal_dynamic_cpp/package.xml +++ b/rmw_ecal_dynamic_cpp/package.xml @@ -2,7 +2,7 @@ rmw_ecal_dynamic_cpp - 0.4.1 + 0.5.0 RMW implementation based on eCAL. Aleksandar Brakmić Apache License 2.0 diff --git a/rmw_ecal_dynamic_cpp/src/event.cpp b/rmw_ecal_dynamic_cpp/src/event.cpp index df218c4..d17bd09 100644 --- a/rmw_ecal_dynamic_cpp/src/event.cpp +++ b/rmw_ecal_dynamic_cpp/src/event.cpp @@ -37,11 +37,11 @@ rmw_ret_t rmw_publisher_event_init(rmw_event_t *rmw_event, RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); CHECK_RMW_IMPLEMENTATION(publisher); - + if (event_type != rmw_event_type_t::RMW_EVENT_OFFERED_DEADLINE_MISSED) { RMW_SET_ERROR_MSG("Unsupported publisher event type."); - return RMW_RET_INVALID_ARGUMENT; + return RMW_RET_UNSUPPORTED; } rmw_event->event_type = event_type; @@ -62,7 +62,7 @@ rmw_ret_t rmw_subscription_event_init(rmw_event_t *rmw_event, if (event_type != rmw_event_type_t::RMW_EVENT_REQUESTED_DEADLINE_MISSED) { RMW_SET_ERROR_MSG("Unsupported publisher event type."); - return RMW_RET_INVALID_ARGUMENT; + return RMW_RET_UNSUPPORTED; } rmw_event->event_type = event_type; diff --git a/rmw_ecal_dynamic_cpp/src/get_topic_endpoint_info.cpp b/rmw_ecal_dynamic_cpp/src/get_topic_endpoint_info.cpp new file mode 100644 index 0000000..70f0738 --- /dev/null +++ b/rmw_ecal_dynamic_cpp/src/get_topic_endpoint_info.cpp @@ -0,0 +1,47 @@ +/* ========================= RMW eCAL LICENSE ================================= + * + * Copyright (C) 2019 - 2020 Continental Corporation + * + * 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. + * + * ========================= RMW eCAL LICENSE ================================= +*/ + +#if ROS_DISTRO >= FOXY + +#include + +#include + +#include "internal/graph.hpp" +#include "internal/common.hpp" + +rmw_ret_t rmw_get_publishers_info_by_topic(const rmw_node_t * /* node */, + rcutils_allocator_t * /* allocator */, + const char * /* topic_name */, + bool /* no_mangle */, + rmw_topic_endpoint_info_array_t * /* publishers_info */) +{ + UNSUPPORTED; +} + +rmw_ret_t rmw_get_subscriptions_info_by_topic(const rmw_node_t * /* node */, + rcutils_allocator_t * /* allocator */, + const char * /* topic_name */, + bool /* no_mangle */, + rmw_topic_endpoint_info_array_t * /* subscriptions_info */) +{ + UNSUPPORTED; +} + +#endif \ No newline at end of file diff --git a/rmw_ecal_dynamic_cpp/src/internal/qos.hpp b/rmw_ecal_dynamic_cpp/src/internal/qos.hpp index 7a077b2..585f008 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/qos.hpp +++ b/rmw_ecal_dynamic_cpp/src/internal/qos.hpp @@ -20,6 +20,7 @@ #pragma once #include +#include #include diff --git a/rmw_ecal_dynamic_cpp/src/internal/rosidl_generator_c_pkg_adapter.hpp b/rmw_ecal_dynamic_cpp/src/internal/rosidl_generator_c_pkg_adapter.hpp new file mode 100644 index 0000000..30f54e9 --- /dev/null +++ b/rmw_ecal_dynamic_cpp/src/internal/rosidl_generator_c_pkg_adapter.hpp @@ -0,0 +1,129 @@ +/* ========================= RMW eCAL LICENSE ================================= +* +* Copyright (C) 2019 - 2020 Continental Corporation +* +* 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. +* +* ========================= RMW eCAL LICENSE ================================= +*/ + +//From Foxy distro rosidl_generator_c has been renamed to rosidl_generator_c, +//purpose of this header file is to alias old C type names to new ones, that way the +//rest of the code doesn't have to care about old package name regardless of distro. +//any rosidl_generator_c to rosidl_runtime_c adaptions should be added here + +#if ROS_DISTRO >= FOXY + +#include +#include +#include +#include + +#else + +#include +#include +#include +#include + +#define ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(STRUCT_NAME) \ + using rosidl_runtime_c__##STRUCT_NAME##__Sequence = rosidl_generator_c__##STRUCT_NAME##__Sequence; + +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(float) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(double) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(long_double) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(char) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(wchar) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(boolean) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(octet) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(uint8) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(int8) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(uint16) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(int16) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(uint32) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(int32) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(uint64) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(int64) +ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE(String) + +#undef ADAPT_PRIMITIVE_SEQUENCE_NAMESPACE + +using rosidl_runtime_c__String = rosidl_generator_c__String; + +#define rosidl_runtime_c__float__Sequence__fini(sequence) rosidl_generator_c__float__Sequence__fini(sequence) +#define rosidl_runtime_c__float__Sequence__init(sequence, size) rosidl_generator_c__float__Sequence__init(sequence, size) + +#define rosidl_runtime_c__double__Sequence__fini(sequence) rosidl_generator_c__double__Sequence__fini(sequence) +#define rosidl_runtime_c__double__Sequence__init(sequence, size) rosidl_generator_c__double__Sequence__init(sequence, size) + +#define rosidl_runtime_c__long_double__Sequence__fini(sequence) rosidl_generator_c__long_double__Sequence__fini(sequence) +#define rosidl_runtime_c__long_double__Sequence__init(sequence, size) rosidl_generator_c__long_double__Sequence__init(sequence, size) + +#define rosidl_runtime_c__char__Sequence__fini(sequence) rosidl_generator_c__char__Sequence__fini(sequence) +#define rosidl_runtime_c__char__Sequence__init(sequence, size) rosidl_generator_c__char__Sequence__init(sequence, size) + +#define rosidl_runtime_c__wchar__Sequence__fini(sequence) rosidl_generator_c__wchar__Sequence__fini(sequence) +#define rosidl_runtime_c__wchar__Sequence__init(sequence, size) rosidl_generator_c__wchar__Sequence__init(sequence, size) + +#define rosidl_runtime_c__boolean__Sequence__fini(sequence) rosidl_generator_c__boolean__Sequence__fini(sequence) +#define rosidl_runtime_c__boolean__Sequence__init(sequence, size) rosidl_generator_c__boolean__Sequence__init(sequence, size) + +#define rosidl_runtime_c__octet__Sequence__fini(sequence) rosidl_generator_c__octet__Sequence__fini(sequence) +#define rosidl_runtime_c__octet__Sequence__init(sequence, size) rosidl_generator_c__octet__Sequence__init(sequence, size) + +#define rosidl_runtime_c__uint8__Sequence__fini(sequence) rosidl_generator_c__uint8__Sequence__fini(sequence) +#define rosidl_runtime_c__uint8__Sequence__init(sequence, size) rosidl_generator_c__uint8__Sequence__init(sequence, size) + +#define rosidl_runtime_c__int8__Sequence__fini(sequence) rosidl_generator_c__int8__Sequence__fini(sequence) +#define rosidl_runtime_c__int8__Sequence__init(sequence, size) rosidl_generator_c__int8__Sequence__init(sequence, size) + +#define rosidl_runtime_c__uint16__Sequence__fini(sequence) rosidl_generator_c__uint16__Sequence__fini(sequence) +#define rosidl_runtime_c__uint16__Sequence__init(sequence, size) rosidl_generator_c__uint16__Sequence__init(sequence, size) + +#define rosidl_runtime_c__int16__Sequence__fini(sequence) rosidl_generator_c__int16__Sequence__fini(sequence) +#define rosidl_runtime_c__int16__Sequence__init(sequence, size) rosidl_generator_c__int16__Sequence__init(sequence, size) + +#define rosidl_runtime_c__uint32__Sequence__fini(sequence) rosidl_generator_c__uint32__Sequence__fini(sequence) +#define rosidl_runtime_c__uint32__Sequence__init(sequence, size) rosidl_generator_c__uint32__Sequence__init(sequence, size) + +#define rosidl_runtime_c__int32__Sequence__fini(sequence) rosidl_generator_c__int32__Sequence__fini(sequence) +#define rosidl_runtime_c__int32__Sequence__init(sequence, size) rosidl_generator_c__int32__Sequence__init(sequence, size) + +#define rosidl_runtime_c__uint64__Sequence__fini(sequence) rosidl_generator_c__uint64__Sequence__fini(sequence) +#define rosidl_runtime_c__uint64__Sequence__init(sequence, size) rosidl_generator_c__uint64__Sequence__init(sequence, size) + +#define rosidl_runtime_c__int64__Sequence__fini(sequence) rosidl_generator_c__int64__Sequence__fini(sequence) +#define rosidl_runtime_c__int64__Sequence__init(sequence, size) rosidl_generator_c__int64__Sequence__init(sequence, size) + +#define rosidl_runtime_c__bool__Sequence__init(sequence, size) rosidl_generator_c__bool__Sequence__init(sequence, size) +#define rosidl_runtime_c__bool__Sequence__fini(sequence) rosidl_generator_c__bool__Sequence__init(sequence) + +#define rosidl_runtime_c__byte__Sequence__init(sequence, size) rosidl_generator_c__byte__Sequence__init(sequence, size) +#define rosidl_runtime_c__byte__Sequence__fini(sequence) rosidl_generator_c__byte__Sequence__fini(sequence) + +#define rosidl_runtime_c__float32__Sequence__init(sequence, size) rosidl_generator_c__float32__Sequence__init(sequence, size) +#define rosidl_runtime_c__float32__Sequence__fini(sequence) rosidl_generator_c__float32__Sequence__fini(sequence) + +#define rosidl_runtime_c__float64__Sequence__init(sequence, size) rosidl_generator_c__float64__Sequence__init(sequence, size) +#define rosidl_runtime_c__float64__Sequence__fini(sequence) rosidl_generator_c__float64__Sequence__fini(sequence) + +#define rosidl_runtime_c__String__init(str) rosidl_generator_c__String__init(str) +#define rosidl_runtime_c__String__fini(str) rosidl_generator_c__String__fini(str) +#define rosidl_runtime_c__String__assignn(str, value, n) rosidl_generator_c__String__assignn(str, value, n) +#define rosidl_runtime_c__String__assign(str, value) rosidl_generator_c__String__assign(str, value) +#define rosidl_runtime_c__String__Sequence__init(sequence, size) rosidl_generator_c__String__Sequence__init(sequence, size) +#define rosidl_runtime_c__String__Sequence__fini(sequence) rosidl_generator_c__String__Sequence__fini(sequence) +#define rosidl_runtime_c__String__Sequence__create(size) rosidl_generator_c__String__Sequence__create(size) +#define rosidl_runtime_c__String__Sequence__destroy(sequence) rosidl_generator_c__String__Sequence__destroy(sequence) + +#endif \ No newline at end of file diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization.hpp b/rmw_ecal_dynamic_cpp/src/internal/serialization.hpp index 4d66274..57d0932 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization.hpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization.hpp @@ -19,7 +19,6 @@ #pragma once -#include #include #include diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_c.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_c.cpp index 76657e3..ae7daae 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_c.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/deserializer_c.cpp @@ -23,13 +23,10 @@ #include #include -#include -#include -#include -#include #include #include "internal/common.hpp" +#include "internal/rosidl_generator_c_pkg_adapter.hpp" namespace eCAL { @@ -49,9 +46,9 @@ void CDeserializer::DeserializeSingle(char *member, const char **se auto size = *reinterpret_cast(*serialized_data); *serialized_data += sizeof(array_size_t); - auto sequence = reinterpret_cast(member); - rosidl_generator_c__String__init(sequence); - rosidl_generator_c__String__assignn(sequence, *serialized_data, size); + auto sequence = reinterpret_cast(member); + rosidl_runtime_c__String__init(sequence); + rosidl_runtime_c__String__assignn(sequence, *serialized_data, size); *serialized_data += size; } @@ -69,7 +66,7 @@ void CDeserializer::DeserializeArray(char *member, size_t size, con for (size_t i = 0; i < size; i++) { DeserializeSingle(member, serialized_data); - member += sizeof(rosidl_generator_c__String__Sequence); + member += sizeof(rosidl_runtime_c__String__Sequence); } } @@ -92,11 +89,10 @@ void CDeserializer::DeserializeDynamicArray(char *member, const char **serialize auto arr_size = *reinterpret_cast(*serialized_data); *serialized_data += sizeof(array_size_t); - auto sequence = reinterpret_cast(member); - sequence->data = nullptr; + auto sequence = reinterpret_cast(member); sequence->size = arr_size; sequence->capacity = arr_size; - if(arr_size > 0) + if (arr_size > 0) { sequence->data = new signed char[arr_size * sizeof(T)]; DeserializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); @@ -109,10 +105,10 @@ void CDeserializer::DeserializeDynamicArray(char *member, const cha auto arr_size = *reinterpret_cast(*serialized_data); *serialized_data += sizeof(array_size_t); - auto sequence = reinterpret_cast(member); - rosidl_generator_c__String__Sequence__init(sequence, arr_size); - - if(arr_size > 0) + auto sequence = reinterpret_cast(member); + rosidl_runtime_c__String__Sequence__init(sequence, arr_size); + + if (arr_size > 0) { DeserializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); } @@ -126,15 +122,14 @@ void CDeserializer::DeserializeDynamicArray(char *message, auto arr_size = *reinterpret_cast(*serialized_data); *serialized_data += sizeof(array_size_t); - auto sequence = reinterpret_cast(message); - sequence->data = nullptr; + auto sequence = reinterpret_cast(message); sequence->size = arr_size; sequence->capacity = arr_size; - if(arr_size > 0) + if (arr_size > 0) { auto sub_members = GetMembers(member); - + sequence->data = new signed char[arr_size * sub_members->size_of_]; auto data = sequence->data; diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_c.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_c.cpp index 2040995..9fceb17 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_c.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/custom/serializer_c.cpp @@ -22,10 +22,9 @@ #include #include -#include -#include #include +#include "internal/rosidl_generator_c_pkg_adapter.hpp" #include "internal/common.hpp" namespace eCAL @@ -65,7 +64,7 @@ void CSerializer::SerializeArray(const char *data, size_t count, st for (size_t i = 0; i < count; i++) { SerializeSingle(data, serialized_data); - data += sizeof(rosidl_generator_c__char__Sequence); + data += sizeof(rosidl_runtime_c__char__Sequence); } } @@ -85,7 +84,7 @@ void CSerializer::SerializeArray(const char *data, template void CSerializer::SerializeDynamicArray(const char *data, std::string &serialized_data) const { - auto sequence = reinterpret_cast(data); + auto sequence = reinterpret_cast(data); serialized_data.reserve(serialized_data.size() + sequence->size * sizeof(T) + sizeof(array_size_t)); SerializeSingle(sequence->size, serialized_data); @@ -95,7 +94,7 @@ void CSerializer::SerializeDynamicArray(const char *data, std::string &serialize template <> void CSerializer::SerializeDynamicArray(const char *data, std::string &serialized_data) const { - auto sequence = reinterpret_cast(data); + auto sequence = reinterpret_cast(data); SerializeSingle(sequence->size, serialized_data); SerializeArray(reinterpret_cast(sequence->data), sequence->size, serialized_data); @@ -106,7 +105,7 @@ void CSerializer::SerializeDynamicArray(const char *data, const rosidl_typesupport_introspection_c__MessageMember *member, std::string &serialized_data) const { - auto sequence = reinterpret_cast(data); + auto sequence = reinterpret_cast(data); auto sequence_data = reinterpret_cast(sequence->data); auto sub_members = GetMembers(member); diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/deserializer_c.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/deserializer_c.cpp index a5afb0c..9428337 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/deserializer_c.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/deserializer_c.cpp @@ -24,12 +24,9 @@ #include #include -#include -#include -#include -#include #include +#include "internal/rosidl_generator_c_pkg_adapter.hpp" #include "internal/common.hpp" #include "internal/serialization/protobuf/helpers.hpp" @@ -46,49 +43,48 @@ namespace rmw namespace pb = google::protobuf; #define DEFINE_SET_METHODS(PB_NAME, TYPE) \ - template <> \ - void CProtobufDeserializer::SetSingle(char *ros_member, \ - const pb::Message *msg, \ - const pb::FieldDescriptor *field) const \ - { \ - auto data = reinterpret_cast(ros_member); \ - auto ref = msg->GetReflection(); \ - *data = ref->Get##PB_NAME(*msg, field); \ - } \ - \ - template <> \ - void CProtobufDeserializer::SetArray(char *ros_member, \ - int array_size, \ - const pb::Message *msg, \ - const pb::FieldDescriptor *field) const \ - { \ - auto data = reinterpret_cast(ros_member); \ - auto ref = msg->GetReflection(); \ - for (int i = 0; i < array_size; i++, data++) \ - { \ - *data = ref->GetRepeated##PB_NAME(*msg, field, i); \ - } \ - } \ - \ - template <> \ - void CProtobufDeserializer::SetDynamicArray(char *ros_member, \ - const pb::Message *msg, \ - const pb::FieldDescriptor *field) const \ - { \ - auto sequence = reinterpret_cast(ros_member); \ - auto ref = msg->GetReflection(); \ - auto size = ref->FieldSize(*msg, field); \ - \ - sequence->data = nullptr; \ - sequence->size = size; \ - sequence->capacity = size; \ - if (size > 0) \ - { \ - sequence->data = new uint8_t[size * sizeof(TYPE)]; \ - SetArray(reinterpret_cast(sequence->data), \ - sequence->size, msg, field); \ - } \ - } +template <> \ +void CProtobufDeserializer::SetSingle(char *ros_member, \ + const pb::Message *msg, \ + const pb::FieldDescriptor *field) const \ +{ \ +auto data = reinterpret_cast(ros_member); \ +auto ref = msg->GetReflection(); \ +*data = ref->Get##PB_NAME(*msg, field); \ +} \ + \ +template <> \ +void CProtobufDeserializer::SetArray(char *ros_member, \ + int array_size, \ + const pb::Message *msg, \ + const pb::FieldDescriptor *field) const \ +{ \ +auto data = reinterpret_cast(ros_member); \ +auto ref = msg->GetReflection(); \ +for (int i = 0; i < array_size; i++, data++) \ +{ \ + *data = ref->GetRepeated##PB_NAME(*msg, field, i); \ +} \ +} \ + \ +template <> \ +void CProtobufDeserializer::SetDynamicArray(char *ros_member, \ + const pb::Message *msg, \ + const pb::FieldDescriptor *field) const \ +{ \ +auto sequence = reinterpret_cast(ros_member); \ +auto ref = msg->GetReflection(); \ +auto size = ref->FieldSize(*msg, field); \ + \ +sequence->size = size; \ +sequence->capacity = size; \ +if (size > 0) \ +{ \ + sequence->data = new uint8_t[size * sizeof(TYPE)]; \ + SetArray(reinterpret_cast(sequence->data), \ + sequence->size, msg, field); \ +} \ +} DEFINE_SET_METHODS(Bool, bool) DEFINE_SET_METHODS(Int32, char) @@ -108,15 +104,17 @@ void CProtobufDeserializer::SetSingle(char *ros_member, const pb::Message *msg, const pb::FieldDescriptor *field) const { - auto data = reinterpret_cast(ros_member); + auto data = reinterpret_cast(ros_member); auto ref = msg->GetReflection(); const std::string &str = ref->GetStringReference(*msg, field, nullptr); - auto str_data = str.c_str(); auto size = str.size(); + data->size = size; + data->capacity = size + 1; + data->data = new signed char[data->capacity]; + auto str_data = reinterpret_cast(str.c_str()); - rosidl_generator_c__String__init(data); - rosidl_generator_c__String__assignn(data, str_data, size); + std::copy_n(str_data, size + 1, data->data); } template <> @@ -125,19 +123,20 @@ void CProtobufDeserializer::SetArray(char *ros_member, const pb::Message *msg, const pb::FieldDescriptor *field) const { - auto sequence = reinterpret_cast(ros_member); + auto sequence = reinterpret_cast(ros_member); auto ref = msg->GetReflection(); for (int i = 0; i < array_size; i++) { - auto data = &sequence[i]; - + auto data = reinterpret_cast(&sequence[i]); const std::string &str = ref->GetRepeatedStringReference(*msg, field, i, nullptr); auto size = str.size(); - auto str_data = str.c_str(); + data->size = size; + data->capacity = size + 1; + data->data = new signed char[data->capacity]; + auto str_data = reinterpret_cast(str.c_str()); - rosidl_generator_c__String__init(data); - rosidl_generator_c__String__assignn(data, str_data, size); + std::copy_n(str_data, size + 1, data->data); } } @@ -146,15 +145,17 @@ void CProtobufDeserializer::SetDynamicArray(char *ros_member, const pb::Message *msg, const pb::FieldDescriptor *field) const { - auto sequence = reinterpret_cast(ros_member); + auto sequence = reinterpret_cast(ros_member); auto ref = msg->GetReflection(); - auto size = ref->FieldSize(*msg, field); - rosidl_generator_c__String__Sequence__init(sequence, size); - - if(size > 0) + auto array_size = ref->FieldSize(*msg, field); + sequence->size = array_size; + sequence->capacity = array_size; + if (array_size > 0) { - SetArray(reinterpret_cast(sequence->data), size, msg, field); + auto sequence_data = new char[array_size * sizeof(rosidl_runtime_c__char__Sequence)]; + SetArray(sequence_data, array_size, msg, field); + sequence->data = reinterpret_cast(sequence_data); } } @@ -191,15 +192,13 @@ void CProtobufDeserializer::SetDynamicArray(char *ros_member, const pb::Message *msg, const pb::FieldDescriptor *field) const { - auto sequence = reinterpret_cast(ros_member); + auto sequence = reinterpret_cast(ros_member); auto ref = msg->GetReflection(); auto array_size = ref->FieldSize(*msg, field); - sequence->data = nullptr; sequence->size = array_size; sequence->capacity = array_size; - - if(array_size > 0) + if (array_size > 0) { auto sequence_data = new char[array_size * members->size_of_]; SetArray(sequence_data, members, array_size, msg, field); diff --git a/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/serializer_c.cpp b/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/serializer_c.cpp index dbfe9ac..5efb0f1 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/serializer_c.cpp +++ b/rmw_ecal_dynamic_cpp/src/internal/serialization/protobuf/serializer_c.cpp @@ -23,9 +23,9 @@ #include #include -#include #include +#include "internal/rosidl_generator_c_pkg_adapter.hpp" #include "internal/common.hpp" #include "internal/serialization/protobuf/helpers.hpp" @@ -71,7 +71,7 @@ using MessageMembers = rosidl_typesupport_introspection_c__MessageMembers; const pb::FieldDescriptor *field) const \ { \ auto ref = msg->GetReflection(); \ - auto sequence = reinterpret_cast(data); \ + auto sequence = reinterpret_cast(data); \ \ ThrowIfInvalidProtobufArraySize(sequence->size); \ for (size_t i = 0; i < sequence->size; i++) \ @@ -99,7 +99,7 @@ void CProtobufSerializer::SetSingle(const char *data, pb::Message * const pb::FieldDescriptor *field) const { auto ref = msg->GetReflection(); - auto sequence = reinterpret_cast(data); + auto sequence = reinterpret_cast(data); auto str_data = reinterpret_cast(sequence->data); ref->SetString(msg, field, str_data); @@ -110,7 +110,7 @@ void CProtobufSerializer::SetArray(const char *data, int size, pb:: const pb::FieldDescriptor *field) const { auto ref = msg->GetReflection(); - auto array = reinterpret_cast(data); + auto array = reinterpret_cast(data); for (int i = 0; i < size; i++) { @@ -125,15 +125,15 @@ void CProtobufSerializer::SetDynamicArray(const char *data, pb::Mes const pb::FieldDescriptor *field) const { auto ref = msg->GetReflection(); - auto sequence = reinterpret_cast(data); + auto sequence = reinterpret_cast(data); auto size = sequence->size; auto strings = sequence->data; ThrowIfInvalidProtobufArraySize(size); - for (size_t i = 0; i < size; i++, strings += sizeof(rosidl_generator_c__char__Sequence)) + for (size_t i = 0; i < size; i++, strings += sizeof(rosidl_runtime_c__char__Sequence)) { - auto string = reinterpret_cast(strings); + auto string = reinterpret_cast(strings); auto str_data = reinterpret_cast(string->data); ref->AddString(msg, field, str_data); @@ -175,7 +175,7 @@ template <> void CProtobufSerializer::SetDynamicArray(const char *data, const MessageMembers *members, pb::Message *msg, const pb::FieldDescriptor *field) const { - auto sequence = reinterpret_cast(data); + auto sequence = reinterpret_cast(data); auto arr_data = reinterpret_cast(sequence->data); auto arr_size = sequence->size; diff --git a/rmw_ecal_dynamic_cpp/src/internal/typesupport.hpp b/rmw_ecal_dynamic_cpp/src/internal/typesupport.hpp index 63926dd..b12fd88 100644 --- a/rmw_ecal_dynamic_cpp/src/internal/typesupport.hpp +++ b/rmw_ecal_dynamic_cpp/src/internal/typesupport.hpp @@ -23,9 +23,8 @@ #include #include -#include -#include +#include "internal/rosidl_generator_c_pkg_adapter.hpp" #include "internal/typesupport/message_typesupport_c.hpp" #include "internal/typesupport/message_typesupport_cpp.hpp" #include "internal/typesupport/service_typesupport_c.hpp" diff --git a/rmw_ecal_dynamic_cpp/src/rmw.cpp b/rmw_ecal_dynamic_cpp/src/rmw.cpp index 152fbb3..bedbbb0 100644 --- a/rmw_ecal_dynamic_cpp/src/rmw.cpp +++ b/rmw_ecal_dynamic_cpp/src/rmw.cpp @@ -52,26 +52,33 @@ const char *rmw_get_serialization_format(void) return eCAL::rmw::serialization_format; } -#ifdef DASHING +#if ROS_DISTRO >= FOXY rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, const char *namespace_, size_t /* domain_id */, - const rmw_node_security_options_t *security_options) -#endif -#ifdef ELOQUENT + bool /* localhost_only */) +#elif ROS_DISTRO == ELOQUENT rmw_node_t *rmw_create_node(rmw_context_t *context, const char *name, const char *namespace_, size_t /* domain_id */, const rmw_node_security_options_t *security_options, bool /* local_host_only */) +#else +rmw_node_t *rmw_create_node(rmw_context_t *context, + const char *name, + const char *namespace_, + size_t /* domain_id */, + const rmw_node_security_options_t *security_options) #endif { RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(name, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(namespace_, nullptr); +#if ROS_DISTRO < FOXY RMW_CHECK_ARGUMENT_FOR_NULL(security_options, nullptr); +#endif CHECK_RMW_IMPLEMENTATION_RET_VALUE(context, nullptr); auto rmw_node = rmw_node_allocate(); @@ -116,25 +123,24 @@ const rmw_guard_condition_t *rmw_node_get_graph_guard_condition(const rmw_node_t return eCAL::rmw::GetImplementation(node)->guard_condition; } -#ifdef DASHING +#if ROS_DISTRO >= ELOQUENT rmw_publisher_t *rmw_create_publisher(const rmw_node_t *node, const rosidl_message_type_support_t *type_support, const char *topic_name, - const rmw_qos_profile_t *qos_policies) -#endif -#ifdef ELOQUENT + const rmw_qos_profile_t *qos_policies, + const rmw_publisher_options_t *publisher_options) +#else rmw_publisher_t *rmw_create_publisher(const rmw_node_t *node, const rosidl_message_type_support_t *type_support, const char *topic_name, - const rmw_qos_profile_t *qos_policies, - const rmw_publisher_options_t *publisher_options) + const rmw_qos_profile_t *qos_policies) #endif { RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_support, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); -#ifdef ELOQUENT +#if ROS_DISTRO >= ELOQUENT RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); #endif CHECK_RMW_IMPLEMENTATION_RET_VALUE(node, nullptr); @@ -148,7 +154,7 @@ rmw_publisher_t *rmw_create_publisher(const rmw_node_t *node, rmw_pub->implementation_identifier = rmw_get_implementation_identifier(); rmw_pub->topic_name = eCAL::rmw::ConstructCString(topic_name); rmw_pub->data = ecal_pub; -#ifdef ELOQUENT +#if ROS_DISTRO >= ELOQUENT rmw_pub->options = *publisher_options; rmw_pub->can_loan_messages = false; #endif @@ -223,9 +229,15 @@ rmw_ret_t rmw_publish_serialized_message(const rmw_publisher_t *publisher, return RMW_RET_OK; } +#if ROS_DISTRO >= FOXY +rmw_ret_t rmw_get_serialized_message_size(const rosidl_message_type_support_t * /* type_support */, + const rosidl_runtime_c__Sequence__bound * /* message_bounds */, + size_t * /* size */) +#else rmw_ret_t rmw_get_serialized_message_size(const rosidl_message_type_support_t * /* type_support */, const rosidl_message_bounds_t * /* message_bounds */, size_t * /* size */) +#endif { UNSUPPORTED; } @@ -261,26 +273,25 @@ rmw_ret_t rmw_deserialize(const rmw_serialized_message_t *serialized_message, return RMW_RET_OK; } -#ifdef DASHING +#if ROS_DISTRO >= ELOQUENT rmw_subscription_t *rmw_create_subscription(const rmw_node_t *node, const rosidl_message_type_support_t *type_support, const char *topic_name, const rmw_qos_profile_t *qos_policies, - bool /* ignore_local_publications */) -#endif -#ifdef ELOQUENT + const rmw_subscription_options_t *subscription_options) +#else rmw_subscription_t *rmw_create_subscription(const rmw_node_t *node, const rosidl_message_type_support_t *type_support, const char *topic_name, const rmw_qos_profile_t *qos_policies, - const rmw_subscription_options_t *subscription_options) + bool /* ignore_local_publications */) #endif { RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(type_support, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); -#ifdef ELOQUENT +#if ROS_DISTRO >= ELOQUENT RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); #endif CHECK_RMW_IMPLEMENTATION_RET_VALUE(node, nullptr); @@ -294,7 +305,7 @@ rmw_subscription_t *rmw_create_subscription(const rmw_node_t *node, rmw_sub->implementation_identifier = rmw_get_implementation_identifier(); rmw_sub->topic_name = eCAL::rmw::ConstructCString(topic_name); rmw_sub->data = ecal_sub; -#ifdef ELOQUENT +#if ROS_DISTRO >= ELOQUENT rmw_sub->can_loan_messages = false; #endif @@ -357,6 +368,41 @@ rmw_ret_t rmw_take_with_info(const rmw_subscription_t *subscription, return rmw_take(subscription, ros_message, taken, allocation); } +#if ROS_DISTRO >= FOXY +rmw_ret_t rmw_take_sequence(const rmw_subscription_t *subscription, + size_t count, + rmw_message_sequence_t *message_sequence, + rmw_message_info_sequence_t *message_info_sequence, + size_t *taken, + rmw_subscription_allocation_t * /* allocation */) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(message_sequence, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(message_info_sequence, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + CHECK_RMW_IMPLEMENTATION(subscription); + if(message_sequence->capacity < count) + { + RMW_SET_ERROR_MSG("message_sequence capacity is smaller then count."); + return RMW_RET_INVALID_ARGUMENT; + } + if(message_info_sequence->capacity < count) + { + RMW_SET_ERROR_MSG("message_info_sequence capacity is smaller then count."); + return RMW_RET_INVALID_ARGUMENT; + } + + *taken = 0; + auto ecal_sub = eCAL::rmw::GetImplementation(subscription); + while (ecal_sub->HasData() && *taken != count) + { + ecal_sub->TakeLatestData(message_sequence->data[*taken]); + (*taken)++; + } + return RMW_RET_OK; +} +#endif + rmw_ret_t rmw_take_serialized_message(const rmw_subscription_t *subscription, rmw_serialized_message_t *serialized_message, bool *taken, @@ -445,13 +491,25 @@ rmw_ret_t rmw_send_request(const rmw_client_t *client, return RMW_RET_OK; } +#if ROS_DISTRO >= FOXY +rmw_ret_t +rmw_take_response(const rmw_client_t *client, + rmw_service_info_t *request_header, + void *ros_response, + bool *taken) +#else rmw_ret_t rmw_take_response(const rmw_client_t *client, rmw_request_id_t *rmw_request_id, void *ros_response, bool *taken) +#endif { RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); +#if ROS_DISTRO >= FOXY + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); +#else RMW_CHECK_ARGUMENT_FOR_NULL(rmw_request_id, RMW_RET_INVALID_ARGUMENT); +#endif RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); CHECK_RMW_IMPLEMENTATION(client); @@ -459,7 +517,11 @@ rmw_ret_t rmw_take_response(const rmw_client_t *client, auto ecal_client = eCAL::rmw::GetImplementation(client); if (ecal_client->HasResponse()) { +#if ROS_DISTRO >= FOXY + request_header->request_id.sequence_number = ecal_client->TakeResponse(ros_response); +#else rmw_request_id->sequence_number = ecal_client->TakeResponse(ros_response); +#endif *taken = true; } @@ -505,10 +567,17 @@ rmw_ret_t rmw_destroy_service(rmw_node_t *node, rmw_service_t *service) return RMW_RET_OK; } +#if ROS_DISTRO >= FOXY +rmw_ret_t rmw_take_request(const rmw_service_t *service, + rmw_service_info_t *request_header, + void *ros_request, + bool *taken) +#else rmw_ret_t rmw_take_request(const rmw_service_t *service, rmw_request_id_t *request_header, void *ros_request, bool *taken) +#endif { RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); @@ -519,7 +588,11 @@ rmw_ret_t rmw_take_request(const rmw_service_t *service, auto ecal_service = eCAL::rmw::GetImplementation(service); if (ecal_service->HasRequest()) { +#if ROS_DISTRO >= FOXY + request_header->request_id.sequence_number = ecal_service->TakeRequest(ros_request); +#else request_header->sequence_number = ecal_service->TakeRequest(ros_request); +#endif *taken = true; } @@ -819,6 +892,16 @@ rmw_ret_t rmw_get_node_names(const rmw_node_t *node, return RMW_RET_OK; } +#if ROS_DISTRO >= FOXY +rmw_ret_t rmw_get_node_names_with_enclaves(const rmw_node_t * /* node */, + rcutils_string_array_t * /* node_names */, + rcutils_string_array_t * /* node_namespaces */, + rcutils_string_array_t * /* enclaves */) +{ + UNSUPPORTED; +} +#endif + rmw_ret_t rmw_count_publishers(const rmw_node_t *node, const char *topic_name, size_t *count) @@ -854,10 +937,7 @@ rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t *publisher, rmw_gid_t CHECK_RMW_IMPLEMENTATION(publisher); gid->implementation_identifier = rmw_get_implementation_identifier(); - for (int i = 0; i < RMW_GID_STORAGE_SIZE; i++) - { - gid->data[i] = 0; - } + std::fill(std::begin(gid->data), std::end(gid->data), uint8_t{0}); return RMW_RET_OK; } @@ -924,9 +1004,15 @@ rmw_ret_t rmw_subscription_get_actual_qos(const rmw_subscription_t *subscription return RMW_RET_OK; } +#if ROS_DISTRO >= FOXY +rmw_ret_t rmw_init_publisher_allocation(const rosidl_message_type_support_t * /* type_support */, + const rosidl_runtime_c__Sequence__bound * /* message_bounds */, + rmw_publisher_allocation_t * /* allocation */) +#else rmw_ret_t rmw_init_publisher_allocation(const rosidl_message_type_support_t * /* type_support */, const rosidl_message_bounds_t * /* message_bounds */, rmw_publisher_allocation_t * /* allocation */) +#endif { UNSUPPORTED; } @@ -941,9 +1027,15 @@ rmw_ret_t rmw_publisher_assert_liveliness(const rmw_publisher_t * /* publisher * UNSUPPORTED; } +#if ROS_DISTRO >= FOXY +rmw_ret_t rmw_init_subscription_allocation(const rosidl_message_type_support_t * /* type_support */, + const rosidl_runtime_c__Sequence__bound * /* message_bounds */, + rmw_subscription_allocation_t * /* allocation */) +#else rmw_ret_t rmw_init_subscription_allocation(const rosidl_message_type_support_t * /* type_support */, const rosidl_message_bounds_t * /* message_bounds */, rmw_subscription_allocation_t * /* allocation */) +#endif { UNSUPPORTED; } @@ -970,9 +1062,8 @@ rmw_ret_t rmw_take_loaned_message_with_info(const rmw_subscription_t * /* subscr UNSUPPORTED; } -rmw_ret_t rmw_return_loaned_message_from_subscription(const rmw_subscription_t * /* subscription */ - , - void * /* loaned_message */) +rmw_ret_t rmw_return_loaned_message_from_subscription(const rmw_subscription_t * /* subscription */, + void * /* loaned_message */) { UNSUPPORTED; }