Fix guard conditions (#209) (#210)

* Fix guard conditions

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Add regression test

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Minor fixes

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>
(cherry picked from commit 7bffd49)

Co-authored-by: Pablo Garrido <pablogs9@gmail.com>

Author: mergify[bot]

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;
 }

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;

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;
     }
   }

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)

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 <memory>
+#include <string>
+#include <chrono>
+#include <thread>
+#include <vector>
+
+#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);
+}