diff --git a/src/main/java/us/ihmc/fastddsjava/cdr/idl/IDLObjectSequence.java b/src/main/java/us/ihmc/fastddsjava/cdr/idl/IDLObjectSequence.java index 42dc4485..4cdf4301 100644 --- a/src/main/java/us/ihmc/fastddsjava/cdr/idl/IDLObjectSequence.java +++ b/src/main/java/us/ihmc/fastddsjava/cdr/idl/IDLObjectSequence.java @@ -231,6 +231,25 @@ public int elementSizeBytes(int currentAlignment, int i) return elements[i].calculateSizeBytes(currentAlignment); } + /** + * Object elements already include their own alignment padding in {@link #elementSizeBytes(int, int)}, + * so the base {@link IDLSequence#calculateSizeBytes(int)} loop must not add a second alignment pass. + */ + @Override + public int calculateSizeBytes(int currentAlignment) + { + int initialAlignment = currentAlignment; + + currentAlignment += 4 + CDRBuffer.alignment(currentAlignment, 4); // Length header + + for (int i = 0; i < size(); i++) + { + currentAlignment += elementSizeBytes(currentAlignment, i); + } + + return currentAlignment - initialAlignment; + } + @Override public void readElement(CDRBuffer buffer) { diff --git a/src/main/java/us/ihmc/jros2/ROS2Publisher.java b/src/main/java/us/ihmc/jros2/ROS2Publisher.java index 63cc80e0..314559fa 100644 --- a/src/main/java/us/ihmc/jros2/ROS2Publisher.java +++ b/src/main/java/us/ihmc/jros2/ROS2Publisher.java @@ -205,11 +205,11 @@ private void writeAndPublish(T message, boolean recordStatistics) { writeBuffer.rewind(); - // For fixed-size messages the payload length is stable after construction preallocation. - payloadSizeBytes = CDRBuffer.PAYLOAD_HEADER.length + message.calculateSizeBytes(0); - if (payloadSizeBytes > writeBuffer.getBufferUnsafe().capacity()) + // Presize from calculateSizeBytes; actual payload length is the buffer position after serialize. + int estimatedSizeBytes = CDRBuffer.PAYLOAD_HEADER.length + message.calculateSizeBytes(0); + if (estimatedSizeBytes > writeBuffer.getBufferUnsafe().capacity()) { - writeBuffer.ensureRemainingCapacity(payloadSizeBytes); + writeBuffer.ensureRemainingCapacity(estimatedSizeBytes); } writeBuffer.writePayloadHeader(); diff --git a/src/test/java/us/ihmc/fastddsjava/cdr/idl/IDLObjectSequenceVariableSizeElementTest.java b/src/test/java/us/ihmc/fastddsjava/cdr/idl/IDLObjectSequenceVariableSizeElementTest.java new file mode 100644 index 00000000..3cf97185 --- /dev/null +++ b/src/test/java/us/ihmc/fastddsjava/cdr/idl/IDLObjectSequenceVariableSizeElementTest.java @@ -0,0 +1,94 @@ +package us.ihmc.fastddsjava.cdr.idl; + +import org.junit.jupiter.api.Test; +import us.ihmc.fastddsjava.cdr.CDRBuffer; +import us.ihmc.fastddsjava.cdr.CDRSerializable; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +/** + * Checks {@link IDLObjectSequence#calculateSizeBytes(int)} against serialize() for variable-size elements. + */ +public class IDLObjectSequenceVariableSizeElementTest +{ + /** Element with a string field so encoded size is not a fixed CDR alignment boundary. */ + static class VariableSizeMsg implements CDRSerializable + { + private int id; + private final StringBuilder name = new StringBuilder(); + + @Override + public int calculateSizeBytes(int currentAlignment) + { + int initialAlignment = currentAlignment; + currentAlignment += 4 + CDRBuffer.alignment(currentAlignment, 4); // id + currentAlignment += 4 + CDRBuffer.alignment(currentAlignment, 4) + name.length() + 1; // name + return currentAlignment - initialAlignment; + } + + @Override + public void serialize(CDRBuffer buffer) + { + buffer.writeInt(id); + buffer.writeString(name); + } + + @Override + public void deserialize(CDRBuffer buffer) + { + id = buffer.readInt(); + buffer.readString(name); + } + } + + @Test + public void testCalculateSizeBytesMatchesActualSerializedSize() + { + IDLObjectSequence sequence = new IDLObjectSequence<>(VariableSizeMsg.class); + + VariableSizeMsg first = sequence.add(); + first.id = 1; + first.name.append("pelvis"); // 6 chars -> element size is not a power of two + + VariableSizeMsg second = sequence.add(); + second.id = 2; + second.name.append("thigh"); // 5 chars + + int calculatedSizeBytes = sequence.calculateSizeBytes(0); + + CDRBuffer buffer = new CDRBuffer(); + buffer.ensureRemainingCapacity(CDRBuffer.PAYLOAD_HEADER.length + calculatedSizeBytes); + buffer.writePayloadHeader(); + sequence.serialize(buffer); + + int actualSizeBytes = buffer.getBufferUnsafe().position() - CDRBuffer.PAYLOAD_HEADER.length; + + assertEquals(actualSizeBytes, calculatedSizeBytes, "calculateSizeBytes must match serialize byte count"); + } + + @Test + public void testDeserializeRoundTrip() + { + IDLObjectSequence sequence = new IDLObjectSequence<>(VariableSizeMsg.class); + sequence.add().id = 42; + sequence.get(0).name.append("longer_frame_name_example"); + sequence.add().id = 7; + sequence.get(1).name.append("x"); + + CDRBuffer buffer = new CDRBuffer(); + buffer.ensureRemainingCapacity(CDRBuffer.PAYLOAD_HEADER.length + sequence.calculateSizeBytes(0)); + buffer.writePayloadHeader(); + sequence.serialize(buffer); + buffer.rewind(); + buffer.readPayloadHeader(); + + IDLObjectSequence deserialized = new IDLObjectSequence<>(VariableSizeMsg.class); + deserialized.deserialize(buffer); + + assertEquals(2, deserialized.size()); + assertEquals(42, deserialized.get(0).id); + assertEquals("longer_frame_name_example", deserialized.get(0).name.toString()); + assertEquals(7, deserialized.get(1).id); + assertEquals("x", deserialized.get(1).name.toString()); + } +}