From 2e77742301466e9e728266f650eeef6bbb10d517 Mon Sep 17 00:00:00 2001 From: Pablo Garrido Date: Mon, 29 Nov 2021 10:16:00 +0100 Subject: [PATCH] Fix guard conditions (#209) * Fix guard conditions Signed-off-by: Pablo Garrido * Add regression test Signed-off-by: Pablo Garrido * Minor fixes Signed-off-by: Pablo Garrido (cherry picked from commit 7bffd49aa8f5c1ec987a029c1334de8deff577d0) --- rmw_microxrcedds_c/src/rmw_guard_condition.c | 1 + .../src/rmw_trigger_guard_condition.c | 5 +- rmw_microxrcedds_c/src/rmw_wait.c | 34 ++++---- rmw_microxrcedds_c/test/CMakeLists.txt | 1 + .../test/test_guard_condition.cpp | 79 +++++++++++++++++++ 5 files changed, 100 insertions(+), 20 deletions(-) create mode 100644 rmw_microxrcedds_c/test/test_guard_condition.cpp diff --git a/rmw_microxrcedds_c/src/rmw_guard_condition.c b/rmw_microxrcedds_c/src/rmw_guard_condition.c index cbbb816e..6bd6c007 100644 --- a/rmw_microxrcedds_c/src/rmw_guard_condition.c +++ b/rmw_microxrcedds_c/src/rmw_guard_condition.c @@ -34,6 +34,7 @@ rmw_create_guard_condition( aux_guard_condition->rmw_guard_condition.context = context; aux_guard_condition->rmw_guard_condition.implementation_identifier = rmw_get_implementation_identifier(); + aux_guard_condition->rmw_guard_condition.data = aux_guard_condition; return &aux_guard_condition->rmw_guard_condition; } diff --git a/rmw_microxrcedds_c/src/rmw_trigger_guard_condition.c b/rmw_microxrcedds_c/src/rmw_trigger_guard_condition.c index c403f852..d5e62fcb 100644 --- a/rmw_microxrcedds_c/src/rmw_trigger_guard_condition.c +++ b/rmw_microxrcedds_c/src/rmw_trigger_guard_condition.c @@ -30,8 +30,9 @@ rmw_trigger_guard_condition( RMW_SET_ERROR_MSG("guard condition handle not from this implementation"); ret = RMW_RET_ERROR; } else { - bool * hasTriggered = (bool *)guard_condition->data; - *hasTriggered = true; + rmw_uxrce_guard_condition_t * aux_guard_condition = + (rmw_uxrce_guard_condition_t *)guard_condition->data; + aux_guard_condition->hasTriggered = true; } return ret; diff --git a/rmw_microxrcedds_c/src/rmw_wait.c b/rmw_microxrcedds_c/src/rmw_wait.c index f43a512f..cf8b61f1 100644 --- a/rmw_microxrcedds_c/src/rmw_wait.c +++ b/rmw_microxrcedds_c/src/rmw_wait.c @@ -96,22 +96,19 @@ rmw_wait( } // There is no context that contais any of the wait set entities. Nothing to wait here. - if (available_contexts == 0) { - UXR_UNLOCK(&rmw_uxrce_wait_mutex); - return RMW_RET_OK; - } - - int32_t per_session_timeout = - (timeout.i32 == UXR_TIMEOUT_INF) ? UXR_TIMEOUT_INF : - (int32_t)((float)timeout.i32 / (float)available_contexts); - - item = session_memory.allocateditems; - while (item != NULL) { - rmw_context_impl_t * custom_context = (rmw_context_impl_t *)item->data; - if (custom_context->need_to_be_ran) { - uxr_run_session_until_data(&custom_context->session, per_session_timeout); + if (available_contexts != 0) { + int32_t per_session_timeout = + (timeout.i32 == UXR_TIMEOUT_INF) ? UXR_TIMEOUT_INF : + (int32_t)((float)timeout.i32 / (float)available_contexts); + + item = session_memory.allocateditems; + while (item != NULL) { + rmw_context_impl_t * custom_context = (rmw_context_impl_t *)item->data; + if (custom_context->need_to_be_ran) { + uxr_run_session_until_data(&custom_context->session, per_session_timeout); + } + item = item->next; } - item = item->next; } UXR_UNLOCK(&rmw_uxrce_wait_mutex); @@ -154,11 +151,12 @@ rmw_wait( // Check guard conditions for (size_t i = 0; guard_conditions && i < guard_conditions->guard_condition_count; ++i) { - bool * hasTriggered = (bool *)guard_conditions->guard_conditions[i]; - if ((*hasTriggered) == false) { + rmw_uxrce_guard_condition_t * custom_guard_condition = + (rmw_uxrce_guard_condition_t *)guard_conditions->guard_conditions[i]; + if (custom_guard_condition->hasTriggered == false) { guard_conditions->guard_conditions[i] = NULL; } else { - *hasTriggered = false; + custom_guard_condition->hasTriggered = false; buffered_status = true; } } diff --git a/rmw_microxrcedds_c/test/CMakeLists.txt b/rmw_microxrcedds_c/test/CMakeLists.txt index f6db805a..023a7333 100644 --- a/rmw_microxrcedds_c/test/CMakeLists.txt +++ b/rmw_microxrcedds_c/test/CMakeLists.txt @@ -56,3 +56,4 @@ rmw_test(test-reqres test_reqres.cpp) rmw_test(test-topic test_topic.cpp) rmw_test(test-rmw test_rmw.cpp) rmw_test(test-sizes test_sizes.cpp) +rmw_test(test-guardcond test_guard_condition.cpp) \ No newline at end of file diff --git a/rmw_microxrcedds_c/test/test_guard_condition.cpp b/rmw_microxrcedds_c/test/test_guard_condition.cpp new file mode 100644 index 00000000..3a0ccc70 --- /dev/null +++ b/rmw_microxrcedds_c/test/test_guard_condition.cpp @@ -0,0 +1,79 @@ +// Copyright 2018 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 +#include + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "./rmw_base_test.hpp" +#include "./test_utils.hpp" + +#include "rosidl_runtime_c/string.h" + +#define MICROXRCEDDS_PADDING sizeof(uint32_t) + +class TestGuardCondition : public ::testing::Test +{ +public: + void SetUp() override + { + ASSERT_EQ(rmw_init_options_init(&options, rcutils_get_default_allocator()), RMW_RET_OK); + ASSERT_EQ(rmw_init(&options, &context), RMW_RET_OK); + } + + void TearDown() override + { + ASSERT_EQ(rmw_init_options_fini(&options), RMW_RET_OK); + ASSERT_EQ(rmw_shutdown(&context), RMW_RET_OK); + } + +protected: + rmw_context_t context = rmw_get_zero_initialized_context(); + rmw_init_options_t options = rmw_get_zero_initialized_init_options(); +}; + +TEST_F(TestGuardCondition, guard_condition) +{ + rmw_guard_condition_t * gc = rmw_create_guard_condition(&context); + ASSERT_NE(gc, nullptr); + + rmw_guard_conditions_t guard_conditions; + void * aux[1] = {gc->data}; + guard_conditions.guard_conditions = aux; + guard_conditions.guard_condition_count = 1; + + rmw_time_t wait_timeout = (rmw_time_t) {1LL, 1LL}; + + rmw_ret_t rc = rmw_wait(NULL, &guard_conditions, NULL, NULL, NULL, NULL, &wait_timeout); + ASSERT_EQ(rc, RMW_RET_TIMEOUT); + + rc = rmw_trigger_guard_condition(gc); + ASSERT_EQ(rc, RMW_RET_OK); + + aux[0] = gc->data; + guard_conditions.guard_conditions = aux; + guard_conditions.guard_condition_count = 1; + rc = rmw_wait(NULL, &guard_conditions, NULL, NULL, NULL, NULL, &wait_timeout); + ASSERT_EQ(rc, RMW_RET_OK); + + rc = rmw_destroy_guard_condition(gc); + ASSERT_EQ(rc, RMW_RET_OK); +}