From 4c6d40a2f46db315cc6f7d67c0547f7583ae406c Mon Sep 17 00:00:00 2001
From: Miguel Company <MiguelCompany@eprosima.com>
Date: Tue, 4 Feb 2020 16:23:52 +0100
Subject: [PATCH] Refs #7534. Fixed submessage_size calculation on last
 data_frag.

---
 src/cpp/rtps/messages/submessages/DataMsg.hpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/cpp/rtps/messages/submessages/DataMsg.hpp b/src/cpp/rtps/messages/submessages/DataMsg.hpp
index d96f0c21833..ad9da9e3037 100644
--- a/src/cpp/rtps/messages/submessages/DataMsg.hpp
+++ b/src/cpp/rtps/messages/submessages/DataMsg.hpp
@@ -400,9 +400,10 @@ bool RTPSMessageCreator::addSubmessageDataFrag(
     // TODO(Ricardo) This should be on cachechange.
     // Align submessage to rtps alignment (4).
     submessage_size = uint16_t(msg->pos - position_size_count_size);
-    uint32_t align = (4 - msg->pos % 4) & 3;
-    for (uint32_t count = 0; count < align; ++count)
+    for (; submessage_size & 3; ++submessage_size)
+    {
         added_no_error &= CDRMessage::addOctet(msg, 0);
+    }
 
     //TODO(Ricardo) Improve.
     octet* o= reinterpret_cast<octet*>(&submessage_size);