diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index ff5fee7c9..66c1b7799 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -20,6 +20,7 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" #include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" @@ -117,7 +118,6 @@ rmw_create_publisher( CustomPublisherInfo * info = nullptr; rmw_publisher_t * rmw_publisher = nullptr; eprosima::fastrtps::PublisherAttributes publisherParam; - const eprosima::fastrtps::rtps::GUID_t * guid = nullptr; if (!is_valid_qos(*qos_policies)) { return nullptr; @@ -193,12 +193,9 @@ rmw_create_publisher( ); memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); - guid = &info->publisher_->getGuid(); - if (!guid) { - RMW_SET_ERROR_MSG("no guid found for publisher"); - goto fail; - } - memcpy(info->publisher_gid.data, guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + info->publisher_->getGuid(), + info->publisher_gid.data); rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index 3de1ba7bd..9c3d01f51 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -20,6 +20,7 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" #include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" @@ -121,7 +122,6 @@ rmw_create_publisher( CustomPublisherInfo * info = nullptr; rmw_publisher_t * rmw_publisher = nullptr; eprosima::fastrtps::PublisherAttributes publisherParam; - const eprosima::fastrtps::rtps::GUID_t * guid = nullptr; // Load default XML profile. Domain::getDefaultPublisherAttributes(publisherParam); @@ -188,12 +188,9 @@ rmw_create_publisher( ); memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); - guid = &info->publisher_->getGuid(); - if (!guid) { - RMW_SET_ERROR_MSG("no guid found for publisher"); - goto fail; - } - memcpy(info->publisher_gid.data, guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + info->publisher_->getGuid(), + info->publisher_gid.data); rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp new file mode 100644 index 000000000..6b8872fac --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/guid_utils.hpp @@ -0,0 +1,60 @@ +// Copyright 2020 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 RMW_FASTRTPS_SHARED_CPP__GUID_UTILS_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__GUID_UTILS_HPP_ + +#include +#include +#include + +#include "fastrtps/rtps/common/Guid.h" + +namespace rmw_fastrtps_shared_cpp +{ + +template +void +copy_from_byte_array_to_fastrtps_guid( + const ByteT * guid_byte_array, + eprosima::fastrtps::rtps::GUID_t * guid) +{ + static_assert( + std::is_same::value || std::is_same::value, + "ByteT should be either int8_t or uint8_t"); + assert(guid_byte_array); + assert(guid); + constexpr auto prefix_size = sizeof(guid->guidPrefix.value); + memcpy(guid->guidPrefix.value, guid_byte_array, prefix_size); + memcpy(guid->entityId.value, &guid_byte_array[prefix_size], guid->entityId.size); +} + +template +void +copy_from_fastrtps_guid_to_byte_array( + const eprosima::fastrtps::rtps::GUID_t & guid, + ByteT * guid_byte_array) +{ + static_assert( + std::is_same::value || std::is_same::value, + "ByteT should be either int8_t or uint8_t"); + assert(guid_byte_array); + constexpr auto prefix_size = sizeof(guid.guidPrefix.value); + memcpy(guid_byte_array, &guid.guidPrefix, prefix_size); + memcpy(&guid_byte_array[prefix_size], &guid.entityId, guid.entityId.size); +} + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__GUID_UTILS_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp index faec9b365..7ca05d892 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -23,11 +23,13 @@ #include "rmw/topic_endpoint_info_array.h" #include "rmw/topic_endpoint_info.h" -#include "demangle.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "demangle.hpp" + namespace rmw_fastrtps_shared_cpp { @@ -143,9 +145,10 @@ _set_rmw_topic_endpoint_info( return ret; } // set endpoint gid - uint8_t rmw_gid[RMW_GID_STORAGE_SIZE]; - memset(&rmw_gid, 0, RMW_GID_STORAGE_SIZE); - memcpy(&rmw_gid, &topic_data.entity_guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + uint8_t rmw_gid[RMW_GID_STORAGE_SIZE] = {}; + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + topic_data.entity_guid, + rmw_gid); ret = rmw_topic_endpoint_info_set_gid( topic_endpoint_info, rmw_gid, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index ef83f289f..01a30d020 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -23,9 +23,10 @@ #include "rmw/rmw.h" #include "rmw/types.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" namespace rmw_fastrtps_shared_cpp @@ -99,9 +100,9 @@ __rmw_take_request( deser, ros_request, info->request_type_support_impl_); // Get header - memcpy( - request_header->writer_guid, &request.sample_identity_.writer_guid(), - sizeof(eprosima::fastrtps::rtps::GUID_t)); + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + request.sample_identity_.writer_guid(), + request_header->writer_guid); request_header->sequence_number = ((int64_t)request.sample_identity_.sequence_number().high) << 32 | request.sample_identity_.sequence_number().low; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index b04592fa1..0fc3fdb9d 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -21,9 +21,10 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" namespace rmw_fastrtps_shared_cpp @@ -92,9 +93,9 @@ __rmw_send_response( assert(info); eprosima::fastrtps::rtps::WriteParams wparams; - memcpy( - &wparams.related_sample_identity().writer_guid(), request_header->writer_guid, - sizeof(eprosima::fastrtps::rtps::GUID_t)); + rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( + request_header->writer_guid, + &wparams.related_sample_identity().writer_guid()); wparams.related_sample_identity().sequence_number().high = (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); wparams.related_sample_identity().sequence_number().low = diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index cc16809cf..b5c75bf4b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -24,8 +24,9 @@ #include "fastcdr/Cdr.h" #include "fastcdr/FastBuffer.h" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" namespace rmw_fastrtps_shared_cpp @@ -39,9 +40,10 @@ _assign_message_info( rmw_gid_t * sender_gid = &message_info->publisher_gid; sender_gid->implementation_identifier = identifier; memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); - memcpy( - sender_gid->data, &sinfo->sample_identity.writer_guid(), - sizeof(eprosima::fastrtps::rtps::GUID_t)); + + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + sinfo->sample_identity.writer_guid(), + sender_gid->data); } rmw_ret_t