From 11c0e45140c95f8723f2a97c3b654ef35282e85b Mon Sep 17 00:00:00 2001 From: Hao Date: Wed, 4 Nov 2026 00:36:31 +0000 Subject: [PATCH 1/3] Take input buffers in list order instead of by timestamp Samples that arrive within one clock tick get equal timestamps, and the strict comparison then returned the newest one. rcl_take swapped such samples, and KEEP_LAST could overwrite a newer sample than the oldest. The timestamp comes from rmw_uros_epoch_nanos(), so a time sync may also move it. Allocated items are inserted newest first, so the oldest buffer of an entity is its last match. Overwriting used to keep the buffer in place and break that order, so recycle it through the pool to move it to the head. timestamp remains for the lifespan check. Fixes #349 Co-Authored-By: Claude Opus 5.5 Signed-off-by: Hao --- rmw_microxrcedds_c/src/types.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/rmw_microxrcedds_c/src/types.c b/rmw_microxrcedds_c/src/types.c index ff0f0cae..30c63f4d 100644 --- a/rmw_microxrcedds_c/src/types.c +++ b/rmw_microxrcedds_c/src/types.c @@ -252,6 +252,11 @@ rmw_uxrce_mempool_item_t * rmw_uxrce_get_static_input_buffer_for_entity( ret = get_memory(&static_buffer_memory); } else { ret = rmw_uxrce_find_static_input_buffer_by_owner(entity); + if (ret != NULL) { + // Recycle through the pool, so that the new sample moves to the list head + put_memory(&static_buffer_memory, ret); + ret = get_memory(&static_buffer_memory); + } } break; case RMW_QOS_POLICY_HISTORY_KEEP_ALL: @@ -273,19 +278,18 @@ rmw_uxrce_mempool_item_t * rmw_uxrce_find_static_input_buffer_by_owner( void * owner) { rmw_uxrce_mempool_item_t * ret = NULL; - int64_t min_time = INT64_MAX; UXR_LOCK(&static_buffer_memory.mutex); - // Return the oldest + // Return the oldest: allocated items are kept newest first, so it is the last match. + // Timestamps cannot order samples, as several can arrive within one clock tick. rmw_uxrce_mempool_item_t * static_buffer_item = static_buffer_memory.allocateditems; while (static_buffer_item != NULL) { rmw_uxrce_static_input_buffer_t * data = (rmw_uxrce_static_input_buffer_t *)static_buffer_item->data; - if (data->owner == owner && data->timestamp < min_time) { + if (data->owner == owner) { ret = static_buffer_item; - min_time = data->timestamp; } static_buffer_item = static_buffer_item->next; From 46eb564e7164c661c188a1631eaec9ee0cc0f278 Mon Sep 17 00:00:00 2001 From: Hao Date: Tue, 17 Nov 2026 11:25:36 +0000 Subject: [PATCH 2/3] Move the overwritten input buffer with move_to_head Returning the buffer to the pool and getting it back only served to move it to the head of the allocated list, which was hard to read. Add move_to_head to the memory pool and use it instead. Co-Authored-By: Claude Fable 5.2 Signed-off-by: Hao --- rmw_microxrcedds_c/src/memory.c | 23 +++++++++++++++++++ .../src/rmw_microros_internal/memory.h | 8 +++++++ rmw_microxrcedds_c/src/types.c | 6 ++--- 3 files changed, 34 insertions(+), 3 deletions(-) diff --git a/rmw_microxrcedds_c/src/memory.c b/rmw_microxrcedds_c/src/memory.c index 90f94607..a0880e09 100644 --- a/rmw_microxrcedds_c/src/memory.c +++ b/rmw_microxrcedds_c/src/memory.c @@ -108,3 +108,26 @@ void put_memory( UXR_UNLOCK(&mem->mutex); } + +void move_to_head( + rmw_uxrce_mempool_t * mem, + rmw_uxrce_mempool_item_t * item) +{ + UXR_LOCK(&mem->mutex); + + if (mem->allocateditems != item) { + // Unlinks item, which has a previous item as it is not the head + item->prev->next = item->next; + if (item->next) { + item->next->prev = item->prev; + } + + // Puts item at the head + item->prev = NULL; + item->next = mem->allocateditems; + item->next->prev = item; + mem->allocateditems = item; + } + + UXR_UNLOCK(&mem->mutex); +} diff --git a/rmw_microxrcedds_c/src/rmw_microros_internal/memory.h b/rmw_microxrcedds_c/src/rmw_microros_internal/memory.h index 40813664..b3403faf 100644 --- a/rmw_microxrcedds_c/src/rmw_microros_internal/memory.h +++ b/rmw_microxrcedds_c/src/rmw_microros_internal/memory.h @@ -50,4 +50,12 @@ void put_memory( rmw_uxrce_mempool_t * mem, rmw_uxrce_mempool_item_t * item); +/** + * Moves an allocated item to the head of the allocated list. + * The other allocated items keep their relative order. + */ +void move_to_head( + rmw_uxrce_mempool_t * mem, + rmw_uxrce_mempool_item_t * item); + #endif // RMW_MICROROS_INTERNAL__MEMORY_H_ diff --git a/rmw_microxrcedds_c/src/types.c b/rmw_microxrcedds_c/src/types.c index 30c63f4d..87d5aee3 100644 --- a/rmw_microxrcedds_c/src/types.c +++ b/rmw_microxrcedds_c/src/types.c @@ -253,9 +253,9 @@ rmw_uxrce_mempool_item_t * rmw_uxrce_get_static_input_buffer_for_entity( } else { ret = rmw_uxrce_find_static_input_buffer_by_owner(entity); if (ret != NULL) { - // Recycle through the pool, so that the new sample moves to the list head - put_memory(&static_buffer_memory, ret); - ret = get_memory(&static_buffer_memory); + // The oldest buffer takes the new sample, so it becomes the newest one. + // Allocated items are kept newest first, which puts it at the list head. + move_to_head(&static_buffer_memory, ret); } } break; From 39e1631269b0f3506f680c4e5a15dd84a7f79746 Mon Sep 17 00:00:00 2001 From: Hao Date: Tue, 17 Nov 2026 11:25:36 +0000 Subject: [PATCH 3/3] Test the order of the static input buffers Cover equal timestamps, overwrites, clock jumps, interleaved owners and a take between stores, plus move_to_head on a pool of its own. The tests use the buffer pool directly, so they need no Agent. Without the fix, six of the seven order tests fail. Co-Authored-By: Claude Fable 5.2 Signed-off-by: Hao --- rmw_microxrcedds_c/test/CMakeLists.txt | 1 + rmw_microxrcedds_c/test/test_input_buffer.cpp | 262 ++++++++++++++++++ 2 files changed, 263 insertions(+) create mode 100644 rmw_microxrcedds_c/test/test_input_buffer.cpp diff --git a/rmw_microxrcedds_c/test/CMakeLists.txt b/rmw_microxrcedds_c/test/CMakeLists.txt index d7d08c6a..d8c1aae8 100644 --- a/rmw_microxrcedds_c/test/CMakeLists.txt +++ b/rmw_microxrcedds_c/test/CMakeLists.txt @@ -59,3 +59,4 @@ rmw_test(test-rmw test_rmw.cpp) rmw_test(test-sizes test_sizes.cpp) rmw_test(test-guardcond test_guard_condition.cpp) rmw_test(test-wait test_wait.cpp) +rmw_test(test-inputbuffer test_input_buffer.cpp) diff --git a/rmw_microxrcedds_c/test/test_input_buffer.cpp b/rmw_microxrcedds_c/test/test_input_buffer.cpp new file mode 100644 index 00000000..2cef71e9 --- /dev/null +++ b/rmw_microxrcedds_c/test/test_input_buffer.cpp @@ -0,0 +1,262 @@ +// Copyright 2026 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +#include + +#include +#include + +#include + +// The internal headers have no C linkage guards +extern "C" +{ +#include "./rmw_microros_internal/memory.h" +#include "./rmw_microros_internal/types.h" +} + +/* + * Testing the order of the static input buffers. + * These tests use the buffer pool directly, so they need no Agent. + */ + +class InputBufferTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rmw_uxrce_init_static_input_buffer_memory( + &static_buffer_memory, custom_static_buffers, RMW_UXRCE_MAX_HISTORY); + static_buffer_memory.is_dynamic_allowed = false; + } + + void TearDown() override + { + rmw_uxrce_mempool_item_t * item = static_buffer_memory.allocateditems; + while (item != NULL) { + rmw_uxrce_mempool_item_t * next = item->next; + put_memory(&static_buffer_memory, item); + item = next; + } + } + + // Stores a sample as the topic, request and reply callbacks do + void store(void * owner, size_t depth, uint8_t value, int64_t timestamp) + { + rmw_qos_profile_t qos = rmw_qos_profile_default; + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = depth; + + rmw_uxrce_mempool_item_t * item = rmw_uxrce_get_static_input_buffer_for_entity(owner, qos); + ASSERT_NE(item, nullptr); + + rmw_uxrce_static_input_buffer_t * data = + reinterpret_cast(item->data); + data->owner = owner; + data->buffer[0] = value; + data->length = 1; + data->timestamp = timestamp; + } + + // Takes samples as rmw_take does, until the owner has none left + std::vector take_all(void * owner) + { + std::vector values; + rmw_uxrce_mempool_item_t * item; + + while ((item = rmw_uxrce_find_static_input_buffer_by_owner(owner)) != NULL) { + rmw_uxrce_static_input_buffer_t * data = + reinterpret_cast(item->data); + values.push_back(data->buffer[0]); + put_memory(&static_buffer_memory, item); + } + + return values; + } + + uint8_t take(void * owner) + { + rmw_uxrce_mempool_item_t * item = rmw_uxrce_find_static_input_buffer_by_owner(owner); + EXPECT_NE(item, nullptr); + if (item == NULL) { + return 0; + } + + uint8_t value = reinterpret_cast(item->data)->buffer[0]; + put_memory(&static_buffer_memory, item); + return value; + } + + int owner_a; + int owner_b; +}; + +/* + * Samples that arrive within one clock tick get the same timestamp. + */ +TEST_F(InputBufferTest, equal_timestamps_keep_arrival_order) +{ + store(&owner_a, 4, 1, 5); + store(&owner_a, 4, 2, 5); + store(&owner_a, 4, 3, 5); + + ASSERT_EQ(take_all(&owner_a), (std::vector{1, 2, 3})); +} + +TEST_F(InputBufferTest, overwrites_keep_arrival_order) +{ + for (uint8_t i = 1; i <= 6; i++) { + store(&owner_a, 2, i, i); + } + + ASSERT_EQ(take_all(&owner_a), (std::vector{5, 6})); +} + +TEST_F(InputBufferTest, equal_timestamps_after_overwrites_keep_arrival_order) +{ + store(&owner_a, 2, 1, 1); + store(&owner_a, 2, 2, 2); + store(&owner_a, 2, 3, 3); + store(&owner_a, 2, 4, 3); + store(&owner_a, 2, 5, 3); + + ASSERT_EQ(take_all(&owner_a), (std::vector{4, 5})); +} + +TEST_F(InputBufferTest, overwrite_evicts_the_oldest_on_equal_timestamps) +{ + store(&owner_a, 2, 1, 1); + store(&owner_a, 2, 2, 2); + store(&owner_a, 2, 3, 3); + store(&owner_a, 2, 4, 3); + store(&owner_a, 2, 5, 3); + store(&owner_a, 2, 6, 3); + + ASSERT_EQ(take_all(&owner_a), (std::vector{5, 6})); +} + +/* + * The timestamp is an epoch time, which a time synchronization can move. + */ +TEST_F(InputBufferTest, clock_jumps_do_not_reorder) +{ + store(&owner_a, 3, 1, 9); + store(&owner_a, 3, 2, 1); + store(&owner_a, 3, 3, 5); + + ASSERT_EQ(take_all(&owner_a), (std::vector{1, 2, 3})); +} + +/* + * All entities share one list of buffers. + */ +TEST_F(InputBufferTest, interleaved_owners_keep_their_own_order) +{ + store(&owner_a, 2, 1, 5); + store(&owner_b, 2, 11, 5); + store(&owner_a, 2, 2, 5); + store(&owner_b, 2, 12, 5); + store(&owner_a, 2, 3, 5); + store(&owner_b, 2, 13, 5); + + ASSERT_EQ(take_all(&owner_a), (std::vector{2, 3})); + ASSERT_EQ(take_all(&owner_b), (std::vector{12, 13})); +} + +TEST_F(InputBufferTest, take_between_stores_keeps_arrival_order) +{ + store(&owner_a, 4, 1, 5); + store(&owner_a, 4, 2, 5); + ASSERT_EQ(take(&owner_a), 1); + store(&owner_a, 4, 3, 5); + + ASSERT_EQ(take_all(&owner_a), (std::vector{2, 3})); +} + +/* + * Testing move_to_head on a pool of its own. + */ + +class MemoryPoolTest : public ::testing::Test +{ +protected: + void SetUp() override + { + UXR_INIT_LOCK(&pool.mutex); + for (size_t i = 0; i < 3; i++) { + put_memory(&pool, &items[i]); + } + + // Each get_memory inserts at the head + third = get_memory(&pool); + second = get_memory(&pool); + first = get_memory(&pool); + } + + // Checks the allocated list in both directions + void expect_allocated(std::vector expected) + { + std::vector forward; + rmw_uxrce_mempool_item_t * last = NULL; + for (rmw_uxrce_mempool_item_t * item = pool.allocateditems; item != NULL; item = item->next) { + forward.push_back(item); + last = item; + } + EXPECT_EQ(forward, expected); + + std::vector backward; + for (rmw_uxrce_mempool_item_t * item = last; item != NULL; item = item->prev) { + backward.insert(backward.begin(), item); + } + EXPECT_EQ(backward, expected); + } + + rmw_uxrce_mempool_t pool = {}; + rmw_uxrce_mempool_item_t items[3] = {}; + + rmw_uxrce_mempool_item_t * first; + rmw_uxrce_mempool_item_t * second; + rmw_uxrce_mempool_item_t * third; +}; + +TEST_F(MemoryPoolTest, move_to_head_from_the_head) +{ + expect_allocated({first, second, third}); + + move_to_head(&pool, first); + expect_allocated({first, second, third}); +} + +TEST_F(MemoryPoolTest, move_to_head_from_the_middle) +{ + move_to_head(&pool, second); + expect_allocated({second, first, third}); +} + +TEST_F(MemoryPoolTest, move_to_head_from_the_tail) +{ + move_to_head(&pool, third); + expect_allocated({third, first, second}); +} + +TEST_F(MemoryPoolTest, move_to_head_keeps_the_free_list) +{ + put_memory(&pool, second); + move_to_head(&pool, third); + expect_allocated({third, first}); + + ASSERT_EQ(get_memory(&pool), second); + expect_allocated({second, third, first}); + ASSERT_FALSE(has_memory(&pool)); +}