From f2d26b2ba87c8777d4a6e65cd5054e647eed181f Mon Sep 17 00:00:00 2001
From: Alberto Soragna <alberto.soragna@gmail.com>
Date: Thu, 30 Jun 2022 13:26:15 +0100
Subject: [PATCH] cherry-pick https://github.com/ros2/rmw_fastrtps/pull/550

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>
---
 rmw_fastrtps_cpp/src/type_support_common.cpp                  | 2 ++
 .../rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp      | 2 ++
 .../rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp      | 4 ++++
 3 files changed, 8 insertions(+)

diff --git a/rmw_fastrtps_cpp/src/type_support_common.cpp b/rmw_fastrtps_cpp/src/type_support_common.cpp
index ef5567641..25bdb86a4 100644
--- a/rmw_fastrtps_cpp/src/type_support_common.cpp
+++ b/rmw_fastrtps_cpp/src/type_support_common.cpp
@@ -54,6 +54,8 @@ void TypeSupport::set_members(const message_type_support_callbacks_t * members)
 
   // Total size is encapsulation size + data size
   m_typeSize = 4 + data_size;
+  // Account for RTPS submessage alignment
+  m_typeSize = (m_typeSize + 3) & ~3;
 }
 
 size_t TypeSupport::getEstimatedSerializedSize(const void * ros_message, const void * impl) const
diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp
index a21e20721..95f48e996 100644
--- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp
+++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp
@@ -60,6 +60,8 @@ MessageTypeSupport<MembersType>::MessageTypeSupport(
   } else {
     this->m_typeSize++;
   }
+  // Account for RTPS submessage alignment
+  this->m_typeSize = (this->m_typeSize + 3) & ~3;
 }
 
 }  // namespace rmw_fastrtps_dynamic_cpp
diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp
index 467703712..45e3fc602 100644
--- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp
+++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp
@@ -59,6 +59,8 @@ RequestTypeSupport<ServiceMembersType, MessageMembersType>::RequestTypeSupport(
   } else {
     this->m_typeSize++;
   }
+  // Account for RTPS submessage alignment
+  this->m_typeSize = (this->m_typeSize + 3) & ~3;
 }
 
 template<typename ServiceMembersType, typename MessageMembersType>
@@ -90,6 +92,8 @@ ResponseTypeSupport<ServiceMembersType, MessageMembersType>::ResponseTypeSupport
   } else {
     this->m_typeSize++;
   }
+  // Account for RTPS submessage alignment
+  this->m_typeSize = (this->m_typeSize + 3) & ~3;
 }
 
 }  // namespace rmw_fastrtps_dynamic_cpp