From 08f947ce47c3c2ce9304d086894fc4c3d23d4483 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Tue, 10 Mar 2020 18:06:46 +0100 Subject: [PATCH 01/19] Added timestamp fields. Both the standard wait_set and internal structures got the timestamps. Currently, the fields are not used. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/include/rcl/wait.h | 6 ++++++ rcl/src/rcl/wait.c | 7 +++++++ 2 files changed, 13 insertions(+) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 10b615c12..31b2a1334 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -22,6 +22,7 @@ extern "C" #include #include +#include #include "rcl/client.h" #include "rcl/guard_condition.h" @@ -33,6 +34,7 @@ extern "C" #include "rcl/types.h" #include "rcl/visibility_control.h" + struct rcl_wait_set_impl_t; /// Container for subscription's, guard condition's, etc to be waited on. @@ -40,18 +42,22 @@ typedef struct rcl_wait_set_t { /// Storage for subscription pointers. const rcl_subscription_t ** subscriptions; + rcutils_time_point_value_t * subscriptions_timestamps; size_t size_of_subscriptions; /// Storage for guard condition pointers. const rcl_guard_condition_t ** guard_conditions; size_t size_of_guard_conditions; /// Storage for timer pointers. const rcl_timer_t ** timers; + rcutils_time_point_value_t * timers_timestamps; size_t size_of_timers; /// Storage for client pointers. const rcl_client_t ** clients; + rcutils_time_point_value_t * clients_timestamps; size_t size_of_clients; /// Storage for service pointers. const rcl_service_t ** services; + rcutils_time_point_value_t * services_timestamps; size_t size_of_services; /// Storage for event pointers. const rcl_event_t ** events; diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 79556df9f..51afbdd1b 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -65,14 +65,18 @@ rcl_get_zero_initialized_wait_set() { static rcl_wait_set_t null_wait_set = { .subscriptions = NULL, + .subscriptions_timestamps = NULL, .size_of_subscriptions = 0, .guard_conditions = NULL, .size_of_guard_conditions = 0, .clients = NULL, + .clients_timestamps = NULL, .size_of_clients = 0, .services = NULL, + .services_timestamps = NULL, .size_of_services = 0, .timers = NULL, + .timers_timestamps = NULL, .size_of_timers = 0, .impl = NULL, }; @@ -137,12 +141,15 @@ rcl_wait_set_init( wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); memset(wait_set->impl, 0, sizeof(rcl_wait_set_impl_t)); wait_set->impl->rmw_subscriptions.subscribers = NULL; + wait_set->impl->rmw_subscriptions.timestamps = NULL; wait_set->impl->rmw_subscriptions.subscriber_count = 0; wait_set->impl->rmw_guard_conditions.guard_conditions = NULL; wait_set->impl->rmw_guard_conditions.guard_condition_count = 0; wait_set->impl->rmw_clients.clients = NULL; + wait_set->impl->rmw_clients.timestamps = NULL; wait_set->impl->rmw_clients.client_count = 0; wait_set->impl->rmw_services.services = NULL; + wait_set->impl->rmw_services.timestamps = NULL; wait_set->impl->rmw_services.service_count = 0; wait_set->impl->rmw_events.events = NULL; wait_set->impl->rmw_events.event_count = 0; From 1cb40e970786201bd91954b723886e417e92399f Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Fri, 13 Mar 2020 11:36:12 +0100 Subject: [PATCH 02/19] Style changes Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/include/rcl/wait.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 31b2a1334..46558324e 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -22,6 +22,7 @@ extern "C" #include #include + #include #include "rcl/client.h" @@ -34,7 +35,6 @@ extern "C" #include "rcl/types.h" #include "rcl/visibility_control.h" - struct rcl_wait_set_impl_t; /// Container for subscription's, guard condition's, etc to be waited on. From b112a2705e0c3efeecb9904ba244f73b06535400 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 19 Mar 2020 18:29:32 +0100 Subject: [PATCH 03/19] Add init test with basic checks Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_wait.cpp | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 59b3528e7..aefe0dc4c 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -106,6 +106,32 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_init) { + // zero-initialized wait set should have zero size ;-) + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + EXPECT_EQ(wait_set.size_of_subscriptions, 0ull); + EXPECT_EQ(wait_set.size_of_guard_conditions, 0ull); + EXPECT_EQ(wait_set.size_of_clients, 0ull); + EXPECT_EQ(wait_set.size_of_services, 0ull); + EXPECT_EQ(wait_set.size_of_timers, 0ull); + + // now lets get some fields + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set)); + EXPECT_EQ(wait_set.size_of_subscriptions, 1ull); + EXPECT_EQ(wait_set.size_of_guard_conditions, 1ull); + EXPECT_EQ(wait_set.size_of_clients, 1ull); + EXPECT_EQ(wait_set.size_of_services, 1ull); + EXPECT_EQ(wait_set.size_of_timers, 1ull); + + // finalized wait set is invalid + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(rcl_wait_set_is_valid(&wait_set)); +} + // Test rcl_wait with a positive finite timeout value (1ms) TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), finite_timeout) { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); From f9915e41fe4af76e7ba18de91efe4ff7d9d65507 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Fri, 20 Mar 2020 11:27:40 +0100 Subject: [PATCH 04/19] Initialize timestamps array in sync. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/wait.c | 72 +++++++++++++++++++++++++++++++++----- rcl/test/rcl/test_wait.cpp | 17 ++++++++- 2 files changed, 80 insertions(+), 9 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 51afbdd1b..a2e8c7789 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -257,6 +257,26 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } \ } while (false) +/* This is for the case where the list has an associated timestamp list. + * I'm not checking whether the timestamp list is non-NULL, since it + * must always be in-sync with the main list. + */ +#define SET_CLEAR_TS(Type) \ + do { \ + if (NULL != wait_set->Type ## s) { \ + memset( \ + (void *)wait_set->Type ## s, \ + 0, \ + sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \ + memset( \ + (void *)wait_set->Type ## s_timestamps, \ + 0, \ + sizeof(rcutils_time_point_value_t *) * wait_set->size_of_ ## Type ## s); \ + wait_set->impl->Type ## _index = 0; \ + } \ + } while (false) + + #define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \ do { \ if (NULL != wait_set->impl->RMWStorage) { \ @@ -292,6 +312,42 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } \ } while (false) +#define SET_RESIZE_TS(Type, ExtraDealloc, ExtraRealloc) \ + do { \ + rcl_allocator_t allocator = wait_set->impl->allocator; \ + wait_set->size_of_ ## Type ## s = 0; \ + wait_set->impl->Type ## _index = 0; \ + if (0 == Type ## s_size) { \ + if (wait_set->Type ## s) { \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ + wait_set->Type ## s = NULL; \ + } \ + if (wait_set->Type ## s_timestamps) { \ + allocator.deallocate((void *)wait_set->Type ## s_timestamps, allocator.state); \ + wait_set->Type ## s_timestamps = NULL; \ + } \ + ExtraDealloc \ + } else { \ + wait_set->Type ## s = (const rcl_ ## Type ## _t **)allocator.reallocate( \ + (void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * Type ## s_size, \ + allocator.state); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + wait_set->Type ## s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ + memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * Type ## s_size); \ + wait_set->Type ## s_timestamps = (rcutils_time_point_value_t *)allocator.reallocate( \ + (void *)wait_set->Type ## s_timestamps, \ + sizeof(rcutils_time_point_value_t *) * Type ## s_size, \ + allocator.state); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + wait_set->Type ## s_timestamps, "allocating memory failed", { \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ + wait_set->Type ## s = NULL; return RCL_RET_BAD_ALLOC;}); \ + wait_set->size_of_ ## Type ## s = Type ## s_size; \ + memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * Type ## s_size); \ + ExtraRealloc \ + } \ + } while (false) + #define SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount) \ /* Also deallocate the rmw storage. */ \ if (wait_set->impl->RMWStorage) { \ @@ -340,12 +396,12 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set) RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(wait_set->impl, RCL_RET_WAIT_SET_INVALID); - SET_CLEAR(subscription); + SET_CLEAR_TS(subscription); SET_CLEAR(guard_condition); - SET_CLEAR(client); - SET_CLEAR(service); + SET_CLEAR_TS(client); + SET_CLEAR_TS(service); SET_CLEAR(event); - SET_CLEAR(timer); + SET_CLEAR_TS(timer); SET_CLEAR_RMW( subscription, @@ -388,7 +444,7 @@ rcl_wait_set_resize( { RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(wait_set->impl, RCL_RET_WAIT_SET_INVALID); - SET_RESIZE( + SET_RESIZE_TS( subscription, SET_RESIZE_RMW_DEALLOC( rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count), @@ -428,15 +484,15 @@ rcl_wait_set_resize( memset(rmw_gcs->guard_conditions, 0, sizeof(void *) * num_rmw_gc); } - SET_RESIZE(timer,;,;); // NOLINT - SET_RESIZE( + SET_RESIZE_TS(timer,;,;); // NOLINT + SET_RESIZE_TS( client, SET_RESIZE_RMW_DEALLOC( rmw_clients.clients, rmw_clients.client_count), SET_RESIZE_RMW_REALLOC( client, rmw_clients.clients, rmw_clients.client_count) ); - SET_RESIZE( + SET_RESIZE_TS( service, SET_RESIZE_RMW_DEALLOC( rmw_services.services, rmw_services.service_count), diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index aefe0dc4c..f55be0741 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -117,7 +117,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_init) { // now lets get some fields rcl_ret_t ret = - rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 1, context_ptr, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set)); EXPECT_EQ(wait_set.size_of_subscriptions, 1ull); @@ -125,6 +125,21 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_init) { EXPECT_EQ(wait_set.size_of_clients, 1ull); EXPECT_EQ(wait_set.size_of_services, 1ull); EXPECT_EQ(wait_set.size_of_timers, 1ull); + EXPECT_EQ(wait_set.size_of_events, 1ull); + + // check that we have arrays for return values + EXPECT_TRUE(wait_set.subscriptions != nullptr); + EXPECT_TRUE(wait_set.guard_conditions != nullptr); + EXPECT_TRUE(wait_set.timers != nullptr); + EXPECT_TRUE(wait_set.clients != nullptr); + EXPECT_TRUE(wait_set.services != nullptr); + EXPECT_TRUE(wait_set.events != nullptr); + + // check that we have timestamp arrays as well + EXPECT_TRUE(wait_set.subscriptions_timestamps != nullptr); + EXPECT_TRUE(wait_set.timers_timestamps != nullptr); + EXPECT_TRUE(wait_set.clients_timestamps != nullptr); + EXPECT_TRUE(wait_set.services_timestamps != nullptr); // finalized wait set is invalid ret = rcl_wait_set_fini(&wait_set); From fecea259de2c2a58443983d53f8584725a4b7072 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Fri, 20 Mar 2020 15:37:25 +0100 Subject: [PATCH 05/19] Test de-/allocation of timestamp arrays. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_wait.cpp | 34 ++++++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 10 deletions(-) diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index f55be0741..ccb3c8b06 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -102,6 +102,20 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) { EXPECT_EQ(wait_set.size_of_services, 0ull); EXPECT_EQ(wait_set.size_of_timers, 0ull); + // check that arrays are gone + EXPECT_EQ(wait_set.subscriptions, nullptr); + EXPECT_EQ(wait_set.guard_conditions, nullptr); + EXPECT_EQ(wait_set.timers, nullptr); + EXPECT_EQ(wait_set.clients, nullptr); + EXPECT_EQ(wait_set.services, nullptr); + EXPECT_EQ(wait_set.events, nullptr); + + // check that the timestamp arrays are gone as well + EXPECT_EQ(wait_set.subscriptions_timestamps, nullptr); + EXPECT_EQ(wait_set.timers_timestamps, nullptr); + EXPECT_EQ(wait_set.clients_timestamps, nullptr); + EXPECT_EQ(wait_set.services_timestamps, nullptr); + ret = rcl_wait_set_fini(&wait_set); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } @@ -128,18 +142,18 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_init) { EXPECT_EQ(wait_set.size_of_events, 1ull); // check that we have arrays for return values - EXPECT_TRUE(wait_set.subscriptions != nullptr); - EXPECT_TRUE(wait_set.guard_conditions != nullptr); - EXPECT_TRUE(wait_set.timers != nullptr); - EXPECT_TRUE(wait_set.clients != nullptr); - EXPECT_TRUE(wait_set.services != nullptr); - EXPECT_TRUE(wait_set.events != nullptr); + EXPECT_NE(wait_set.subscriptions, nullptr); + EXPECT_NE(wait_set.guard_conditions, nullptr); + EXPECT_NE(wait_set.timers, nullptr); + EXPECT_NE(wait_set.clients, nullptr); + EXPECT_NE(wait_set.services, nullptr); + EXPECT_NE(wait_set.events, nullptr); // check that we have timestamp arrays as well - EXPECT_TRUE(wait_set.subscriptions_timestamps != nullptr); - EXPECT_TRUE(wait_set.timers_timestamps != nullptr); - EXPECT_TRUE(wait_set.clients_timestamps != nullptr); - EXPECT_TRUE(wait_set.services_timestamps != nullptr); + EXPECT_NE(wait_set.subscriptions_timestamps, nullptr); + EXPECT_NE(wait_set.timers_timestamps, nullptr); + EXPECT_NE(wait_set.clients_timestamps, nullptr); + EXPECT_NE(wait_set.services_timestamps, nullptr); // finalized wait set is invalid ret = rcl_wait_set_fini(&wait_set); From baef36e4310c9e969a7a927b56f4c9c5fdfc5af5 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Fri, 20 Mar 2020 15:58:42 +0100 Subject: [PATCH 06/19] Add internal header for wait_set_impl Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/wait.c | 27 +--------------- rcl/src/rcl/wait_set_impl.h | 61 +++++++++++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+), 26 deletions(-) create mode 100644 rcl/src/rcl/wait_set_impl.h diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index a2e8c7789..313036ea4 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -33,32 +33,7 @@ extern "C" #include "./context_impl.h" -typedef struct rcl_wait_set_impl_t -{ - // number of subscriptions that have been added to the wait set - size_t subscription_index; - rmw_subscriptions_t rmw_subscriptions; - // number of guard_conditions that have been added to the wait set - size_t guard_condition_index; - rmw_guard_conditions_t rmw_guard_conditions; - // number of clients that have been added to the wait set - size_t client_index; - rmw_clients_t rmw_clients; - // number of services that have been added to the wait set - size_t service_index; - rmw_services_t rmw_services; - // number of events that have been added to the wait set - size_t event_index; - rmw_events_t rmw_events; - - rmw_wait_set_t * rmw_wait_set; - // number of timers that have been added to the wait set - size_t timer_index; - // context with which the wait set is associated - rcl_context_t * context; - // allocator used in the wait set - rcl_allocator_t allocator; -} rcl_wait_set_impl_t; +#include "./wait_set_impl.h" rcl_wait_set_t rcl_get_zero_initialized_wait_set() diff --git a/rcl/src/rcl/wait_set_impl.h b/rcl/src/rcl/wait_set_impl.h new file mode 100644 index 000000000..151579ce6 --- /dev/null +++ b/rcl/src/rcl/wait_set_impl.h @@ -0,0 +1,61 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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. + +#ifndef RCL__WAIT_SET_IMPL_H_ +#define RCL__WAIT_SET_IMPL_H_ + +#include + +#include "rcl/context.h" +#include "rcl/allocator.h" +#include "rmw/types.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// \internal +typedef struct rcl_wait_set_impl_t +{ + // number of subscriptions that have been added to the wait set + size_t subscription_index; + rmw_subscriptions_t rmw_subscriptions; + // number of guard_conditions that have been added to the wait set + size_t guard_condition_index; + rmw_guard_conditions_t rmw_guard_conditions; + // number of clients that have been added to the wait set + size_t client_index; + rmw_clients_t rmw_clients; + // number of services that have been added to the wait set + size_t service_index; + rmw_services_t rmw_services; + // number of events that have been added to the wait set + size_t event_index; + rmw_events_t rmw_events; + + rmw_wait_set_t * rmw_wait_set; + // number of timers that have been added to the wait set + size_t timer_index; + // context with which the wait set is associated + rcl_context_t * context; + // allocator used in the wait set + rcl_allocator_t allocator; +} rcl_wait_set_impl_t; + +#ifdef __cplusplus +} +#endif + +#endif // RCL__WAIT_SET_IMPL_H_ From e44c6c87b43df04d525d52f311f78a58005cd150 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Fri, 20 Mar 2020 21:13:24 +0100 Subject: [PATCH 07/19] Note on de-allocation after resize failure. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/include/rcl/wait.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 46558324e..40a8dc4f4 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -272,6 +272,10 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set); * * This can be called on an uninitialized (zero initialized) wait set. * + * If RCL_RET_BAD_ALLOC is returned, i.e. allocation failed, the wait_set + * may be *partially* allocated and therefore, it must still be cleaned up + * with _fini. + * *
* Attribute | Adherence * ------------------ | ------------- From a68da628ce981f35fabed77f12ae41605e8457cc Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Fri, 20 Mar 2020 21:31:20 +0100 Subject: [PATCH 08/19] Allocating timestamp structures plus test. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/wait.c | 80 +++++++++++--- rcl/test/CMakeLists.txt | 9 ++ rcl/test/rcl/test_wait_set_impl.cpp | 157 ++++++++++++++++++++++++++++ 3 files changed, 231 insertions(+), 15 deletions(-) create mode 100644 rcl/test/rcl/test_wait_set_impl.cpp diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 313036ea4..37ec156de 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -264,6 +264,20 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } \ } while (false) +/* The RMW structures with a timestamp are twice as large. */ +#define SET_CLEAR_RMW_TS(Type, RMWStorage, RMWCount) \ + do { \ + if (NULL != wait_set->impl->RMWStorage) { \ + /* Also clear the rmw storage. */ \ + memset( \ + wait_set->impl->RMWStorage, \ + 0, \ + sizeof(void *) * wait_set->impl->RMWCount * 2); \ + wait_set->impl->RMWCount = 0; \ + } \ + } while (false) + + #define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \ do { \ rcl_allocator_t allocator = wait_set->impl->allocator; \ @@ -331,6 +345,16 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al wait_set->impl->RMWCount = 0; \ } +#define SET_RESIZE_RMW_DEALLOC_TS(RMWStorage, RMWTimestamps, RMWCount) \ + /* Also deallocate the rmw storage. */ \ + if (wait_set->impl->RMWStorage) { \ + allocator.deallocate((void *)wait_set->impl->RMWStorage, allocator.state); \ + allocator.deallocate((void *)wait_set->impl->RMWTimestamps, allocator.state); \ + wait_set->impl->RMWStorage = NULL; \ + wait_set->impl->RMWTimestamps = NULL; \ + wait_set->impl->RMWCount = 0; \ + } + #define SET_RESIZE_RMW_REALLOC(Type, RMWStorage, RMWCount) \ /* Also resize the rmw storage. */ \ wait_set->impl->RMWCount = 0; \ @@ -344,6 +368,30 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } \ memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * Type ## s_size); +/* The RMW structures with a timestamp are twice as large. */ +#define SET_RESIZE_RMW_REALLOC_TS(Type, RMWStorage, RMWTimestamps, RMWCount) \ + /* Also resize the rmw storage. */ \ + wait_set->impl->RMWCount = 0; \ + wait_set->impl->RMWStorage = (void **)allocator.reallocate( \ + wait_set->impl->RMWStorage, sizeof(void *) * Type ## s_size, allocator.state); \ + if (!wait_set->impl->RMWStorage) { \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ + wait_set->size_of_ ## Type ## s = 0; \ + RCL_SET_ERROR_MSG("allocating memory failed"); \ + return RCL_RET_BAD_ALLOC; \ + } \ + wait_set->impl->RMWTimestamps = (rcutils_time_point_value_t *)allocator.reallocate( \ + wait_set->impl->RMWTimestamps, sizeof(rcutils_time_point_value_t) * Type ## s_size, \ + allocator.state); \ + if (!wait_set->impl->RMWTimestamps) { \ + allocator.deallocate((void *)wait_set->impl->RMWStorage, allocator.state); \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ + wait_set->size_of_ ## Type ## s = 0; \ + RCL_SET_ERROR_MSG("allocating memory failed"); \ + return RCL_RET_BAD_ALLOC; \ + } \ + memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * Type ## s_size); + /* Implementation-specific notes: * * Add the rmw representation to the underlying rmw array and increment @@ -378,7 +426,7 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set) SET_CLEAR(event); SET_CLEAR_TS(timer); - SET_CLEAR_RMW( + SET_CLEAR_RMW_TS( subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count); @@ -386,11 +434,11 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set) guard_condition, rmw_guard_conditions.guard_conditions, rmw_guard_conditions.guard_condition_count); - SET_CLEAR_RMW( + SET_CLEAR_RMW_TS( clients, rmw_clients.clients, rmw_clients.client_count); - SET_CLEAR_RMW( + SET_CLEAR_RMW_TS( services, rmw_services.services, rmw_services.service_count); @@ -421,10 +469,12 @@ rcl_wait_set_resize( RCL_CHECK_ARGUMENT_FOR_NULL(wait_set->impl, RCL_RET_WAIT_SET_INVALID); SET_RESIZE_TS( subscription, - SET_RESIZE_RMW_DEALLOC( - rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count), - SET_RESIZE_RMW_REALLOC( - subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count) + SET_RESIZE_RMW_DEALLOC_TS( + rmw_subscriptions.subscribers, rmw_subscriptions.timestamps, + rmw_subscriptions.subscriber_count), + SET_RESIZE_RMW_REALLOC_TS( + subscription, rmw_subscriptions.subscribers, rmw_subscriptions.timestamps, + rmw_subscriptions.subscriber_count) ); // Guard condition RCL size is the resize amount given SET_RESIZE(guard_condition,;,;); // NOLINT @@ -462,17 +512,17 @@ rcl_wait_set_resize( SET_RESIZE_TS(timer,;,;); // NOLINT SET_RESIZE_TS( client, - SET_RESIZE_RMW_DEALLOC( - rmw_clients.clients, rmw_clients.client_count), - SET_RESIZE_RMW_REALLOC( - client, rmw_clients.clients, rmw_clients.client_count) + SET_RESIZE_RMW_DEALLOC_TS( + rmw_clients.clients, rmw_clients.timestamps, rmw_clients.client_count), + SET_RESIZE_RMW_REALLOC_TS( + client, rmw_clients.clients, rmw_clients.timestamps, rmw_clients.client_count) ); SET_RESIZE_TS( service, - SET_RESIZE_RMW_DEALLOC( - rmw_services.services, rmw_services.service_count), - SET_RESIZE_RMW_REALLOC( - service, rmw_services.services, rmw_services.service_count) + SET_RESIZE_RMW_DEALLOC_TS( + rmw_services.services, rmw_services.timestamps, rmw_services.service_count), + SET_RESIZE_RMW_REALLOC_TS( + service, rmw_services.services, rmw_services.timestamps, rmw_services.service_count) ); SET_RESIZE( event, diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 77505b150..324cde68a 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -222,6 +222,15 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" ) + rcl_add_custom_gtest(test_wait_set_impl${target_suffix} + SRCS rcl/test_wait_set_impl.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" + ) + + rcl_add_custom_gtest(test_logging_rosout${target_suffix} SRCS rcl/test_logging_rosout.cpp ENV ${rmw_implementation_env_var} diff --git a/rcl/test/rcl/test_wait_set_impl.cpp b/rcl/test/rcl/test_wait_set_impl.cpp new file mode 100644 index 000000000..d41f6d9ac --- /dev/null +++ b/rcl/test/rcl/test_wait_set_impl.cpp @@ -0,0 +1,157 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// Copyright 2020 Robert Bosch GmbH +// +// 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 // for std::max +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/wait.h" + +#include "rcutils/logging_macros.h" + +#include "../../src/rcl/wait_set_impl.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +#define TOLERANCE RCL_MS_TO_NS(6) + +class CLASSNAME (WaitSetImplTestFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + void SetUp() + { + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(this->context_ptr)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(this->context_ptr)) << rcl_get_error_string().str; + delete this->context_ptr; + } +}; + +TEST_F(CLASSNAME(WaitSetImplTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) { + // Initialize a wait set with a subscription and then resize it to zero. + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_wait_set_resize(&wait_set, 0u, 0u, 0u, 0u, 0u, 0u); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(wait_set.size_of_subscriptions, 0ull); + EXPECT_EQ(wait_set.size_of_guard_conditions, 0ull); + EXPECT_EQ(wait_set.size_of_clients, 0ull); + EXPECT_EQ(wait_set.size_of_services, 0ull); + EXPECT_EQ(wait_set.size_of_timers, 0ull); + + // check that arrays are gone + EXPECT_EQ(wait_set.subscriptions, nullptr); + EXPECT_EQ(wait_set.guard_conditions, nullptr); + EXPECT_EQ(wait_set.timers, nullptr); + EXPECT_EQ(wait_set.clients, nullptr); + EXPECT_EQ(wait_set.services, nullptr); + EXPECT_EQ(wait_set.events, nullptr); + + // check that the timestamp arrays are gone as well + EXPECT_EQ(wait_set.subscriptions_timestamps, nullptr); + EXPECT_EQ(wait_set.timers_timestamps, nullptr); + EXPECT_EQ(wait_set.clients_timestamps, nullptr); + EXPECT_EQ(wait_set.services_timestamps, nullptr); + + ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F(CLASSNAME(WaitSetImplTestFixture, RMW_IMPLEMENTATION), test_init) { + // zero-initialized wait set should have zero size ;-) + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + EXPECT_EQ(wait_set.size_of_subscriptions, 0ull); + EXPECT_EQ(wait_set.size_of_guard_conditions, 0ull); + EXPECT_EQ(wait_set.size_of_clients, 0ull); + EXPECT_EQ(wait_set.size_of_services, 0ull); + EXPECT_EQ(wait_set.size_of_timers, 0ull); + + // now lets get some fields + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 1, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set)); + EXPECT_EQ(wait_set.size_of_subscriptions, 1ull); + EXPECT_EQ(wait_set.size_of_guard_conditions, 1ull); + EXPECT_EQ(wait_set.size_of_clients, 1ull); + EXPECT_EQ(wait_set.size_of_services, 1ull); + EXPECT_EQ(wait_set.size_of_timers, 1ull); + EXPECT_EQ(wait_set.size_of_events, 1ull); + + // check that we have arrays for return values + EXPECT_NE(wait_set.subscriptions, nullptr); + EXPECT_NE(wait_set.guard_conditions, nullptr); + EXPECT_NE(wait_set.timers, nullptr); + EXPECT_NE(wait_set.clients, nullptr); + EXPECT_NE(wait_set.services, nullptr); + EXPECT_NE(wait_set.events, nullptr); + + // check that we have timestamp arrays as well + EXPECT_NE(wait_set.subscriptions_timestamps, nullptr); + EXPECT_NE(wait_set.timers_timestamps, nullptr); + EXPECT_NE(wait_set.clients_timestamps, nullptr); + EXPECT_NE(wait_set.services_timestamps, nullptr); + + // look into the implementation + EXPECT_NE(wait_set.impl->rmw_subscriptions.subscribers, nullptr); + EXPECT_NE(wait_set.impl->rmw_guard_conditions.guard_conditions, nullptr); + EXPECT_NE(wait_set.impl->rmw_clients.clients, nullptr); + EXPECT_NE(wait_set.impl->rmw_services.services, nullptr); + EXPECT_NE(wait_set.impl->rmw_events.events, nullptr); + + // and now for the timestamps in the implementation + EXPECT_NE(wait_set.impl->rmw_subscriptions.timestamps, nullptr); + EXPECT_NE(wait_set.impl->rmw_clients.timestamps, nullptr); + EXPECT_NE(wait_set.impl->rmw_services.timestamps, nullptr); + + // finalized wait set is invalid + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(rcl_wait_set_is_valid(&wait_set)); +} From b96c85a00a3a122cb771a82aee3decb5dc9357ac Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Mon, 23 Mar 2020 17:31:00 +0100 Subject: [PATCH 09/19] Changed timestamp macros to call the non-ts ones.Signed-off-by: Luetkebohle Ingo (CR/AEX3) Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/wait.c | 124 +++++++++++++++++++-------------------------- 1 file changed, 51 insertions(+), 73 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 37ec156de..a3d828ea5 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -232,26 +232,18 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } \ } while (false) -/* This is for the case where the list has an associated timestamp list. - * I'm not checking whether the timestamp list is non-NULL, since it - * must always be in-sync with the main list. - */ -#define SET_CLEAR_TS(Type) \ +/* This is for the case where the list has an associated timestamp list. */ +#define SET_CLEAR_TIMESTAMP(Type) \ do { \ - if (NULL != wait_set->Type ## s) { \ - memset( \ - (void *)wait_set->Type ## s, \ - 0, \ - sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \ + if (NULL != wait_set->Type ## s_timestamps) { \ memset( \ (void *)wait_set->Type ## s_timestamps, \ 0, \ sizeof(rcutils_time_point_value_t *) * wait_set->size_of_ ## Type ## s); \ - wait_set->impl->Type ## _index = 0; \ } \ + SET_CLEAR(Type); \ } while (false) - #define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \ do { \ if (NULL != wait_set->impl->RMWStorage) { \ @@ -264,20 +256,19 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } \ } while (false) -/* The RMW structures with a timestamp are twice as large. */ -#define SET_CLEAR_RMW_TS(Type, RMWStorage, RMWCount) \ +#define SET_CLEAR_RMW_TIMESTAMP(Type, RMWStorage, RMWTimestamp, RMWCount) \ do { \ - if (NULL != wait_set->impl->RMWStorage) { \ - /* Also clear the rmw storage. */ \ + if (NULL != wait_set->impl->RMWTimestamp) { \ + /* Also clear the rmw timestamp. */ \ memset( \ - wait_set->impl->RMWStorage, \ + wait_set->impl->RMWTimestamp, \ 0, \ - sizeof(void *) * wait_set->impl->RMWCount * 2); \ - wait_set->impl->RMWCount = 0; \ + sizeof(rcutils_time_point_value_t) * wait_set->impl->RMWCount); \ } \ + /* Clear the rest. */ \ + SET_CLEAR_RMW(Type, RMWStorage, RMWCount); \ } while (false) - #define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \ do { \ rcl_allocator_t allocator = wait_set->impl->allocator; \ @@ -301,38 +292,32 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } \ } while (false) -#define SET_RESIZE_TS(Type, ExtraDealloc, ExtraRealloc) \ +/* First resizes the common fields that all types have, then resizes the timestamp array, + then calls the "extra" functions. */ +#define SET_RESIZE_TIMESTAMP(Type, ExtraDealloc, ExtraRealloc) \ do { \ + SET_RESIZE(Type,;,;); /* NOLINT */ \ rcl_allocator_t allocator = wait_set->impl->allocator; \ - wait_set->size_of_ ## Type ## s = 0; \ - wait_set->impl->Type ## _index = 0; \ if (0 == Type ## s_size) { \ - if (wait_set->Type ## s) { \ - allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ - wait_set->Type ## s = NULL; \ - } \ if (wait_set->Type ## s_timestamps) { \ allocator.deallocate((void *)wait_set->Type ## s_timestamps, allocator.state); \ wait_set->Type ## s_timestamps = NULL; \ } \ ExtraDealloc \ } else { \ - wait_set->Type ## s = (const rcl_ ## Type ## _t **)allocator.reallocate( \ - (void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * Type ## s_size, \ - allocator.state); \ - RCL_CHECK_FOR_NULL_WITH_MSG( \ - wait_set->Type ## s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ - memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * Type ## s_size); \ - wait_set->Type ## s_timestamps = (rcutils_time_point_value_t *)allocator.reallocate( \ + rcutils_time_point_value_t * timestamps = \ + (rcutils_time_point_value_t *)allocator.reallocate( \ (void *)wait_set->Type ## s_timestamps, \ sizeof(rcutils_time_point_value_t *) * Type ## s_size, \ allocator.state); \ RCL_CHECK_FOR_NULL_WITH_MSG( \ - wait_set->Type ## s_timestamps, "allocating memory failed", { \ - allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ - wait_set->Type ## s = NULL; return RCL_RET_BAD_ALLOC;}); \ - wait_set->size_of_ ## Type ## s = Type ## s_size; \ - memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * Type ## s_size); \ + timestamps, "allocating memory failed", { \ + allocator.deallocate((void *)wait_set->Type ## s_timestamps, allocator.state); \ + wait_set->Type ## s_timestamps = NULL; return RCL_RET_BAD_ALLOC;}); \ + wait_set->Type ## s_timestamps = timestamps; \ + memset( \ + (void *)wait_set->Type ## s_timestamps, 0, \ + sizeof(rcutils_time_point_value_t) * Type ## s_size); \ ExtraRealloc \ } \ } while (false) @@ -345,14 +330,12 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al wait_set->impl->RMWCount = 0; \ } -#define SET_RESIZE_RMW_DEALLOC_TS(RMWStorage, RMWTimestamps, RMWCount) \ - /* Also deallocate the rmw storage. */ \ - if (wait_set->impl->RMWStorage) { \ - allocator.deallocate((void *)wait_set->impl->RMWStorage, allocator.state); \ +#define SET_RESIZE_RMW_DEALLOC_TIMESTAMP(RMWStorage, RMWTimestamps, RMWCount) \ + SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount); \ + /* Also deallocate the timestamp storage. */ \ + if (wait_set->impl->RMWTimestamps) { \ allocator.deallocate((void *)wait_set->impl->RMWTimestamps, allocator.state); \ - wait_set->impl->RMWStorage = NULL; \ wait_set->impl->RMWTimestamps = NULL; \ - wait_set->impl->RMWCount = 0; \ } #define SET_RESIZE_RMW_REALLOC(Type, RMWStorage, RMWCount) \ @@ -369,17 +352,9 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * Type ## s_size); /* The RMW structures with a timestamp are twice as large. */ -#define SET_RESIZE_RMW_REALLOC_TS(Type, RMWStorage, RMWTimestamps, RMWCount) \ - /* Also resize the rmw storage. */ \ - wait_set->impl->RMWCount = 0; \ - wait_set->impl->RMWStorage = (void **)allocator.reallocate( \ - wait_set->impl->RMWStorage, sizeof(void *) * Type ## s_size, allocator.state); \ - if (!wait_set->impl->RMWStorage) { \ - allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ - wait_set->size_of_ ## Type ## s = 0; \ - RCL_SET_ERROR_MSG("allocating memory failed"); \ - return RCL_RET_BAD_ALLOC; \ - } \ +#define SET_RESIZE_RMW_REALLOC_TIMESTAMP(Type, RMWStorage, RMWTimestamps, RMWCount) \ + SET_RESIZE_RMW_REALLOC(Type, RMWStorage, RMWCount); \ + /* Also resize the rmw timestamp storage. */ \ wait_set->impl->RMWTimestamps = (rcutils_time_point_value_t *)allocator.reallocate( \ wait_set->impl->RMWTimestamps, sizeof(rcutils_time_point_value_t) * Type ## s_size, \ allocator.state); \ @@ -419,28 +394,31 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set) RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(wait_set->impl, RCL_RET_WAIT_SET_INVALID); - SET_CLEAR_TS(subscription); + SET_CLEAR_TIMESTAMP(subscription); SET_CLEAR(guard_condition); - SET_CLEAR_TS(client); - SET_CLEAR_TS(service); + SET_CLEAR_TIMESTAMP(client); + SET_CLEAR_TIMESTAMP(service); SET_CLEAR(event); - SET_CLEAR_TS(timer); + SET_CLEAR_TIMESTAMP(timer); - SET_CLEAR_RMW_TS( + SET_CLEAR_RMW_TIMESTAMP( subscription, rmw_subscriptions.subscribers, + rmw_subscriptions.timestamps, rmw_subscriptions.subscriber_count); SET_CLEAR_RMW( guard_condition, rmw_guard_conditions.guard_conditions, rmw_guard_conditions.guard_condition_count); - SET_CLEAR_RMW_TS( + SET_CLEAR_RMW_TIMESTAMP( clients, rmw_clients.clients, + rmw_clients.timestamps, rmw_clients.client_count); - SET_CLEAR_RMW_TS( + SET_CLEAR_RMW_TIMESTAMP( services, rmw_services.services, + rmw_services.timestamps, rmw_services.service_count); SET_CLEAR_RMW( events, @@ -467,12 +445,12 @@ rcl_wait_set_resize( { RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(wait_set->impl, RCL_RET_WAIT_SET_INVALID); - SET_RESIZE_TS( + SET_RESIZE_TIMESTAMP( subscription, - SET_RESIZE_RMW_DEALLOC_TS( + SET_RESIZE_RMW_DEALLOC_TIMESTAMP( rmw_subscriptions.subscribers, rmw_subscriptions.timestamps, rmw_subscriptions.subscriber_count), - SET_RESIZE_RMW_REALLOC_TS( + SET_RESIZE_RMW_REALLOC_TIMESTAMP( subscription, rmw_subscriptions.subscribers, rmw_subscriptions.timestamps, rmw_subscriptions.subscriber_count) ); @@ -509,19 +487,19 @@ rcl_wait_set_resize( memset(rmw_gcs->guard_conditions, 0, sizeof(void *) * num_rmw_gc); } - SET_RESIZE_TS(timer,;,;); // NOLINT - SET_RESIZE_TS( + SET_RESIZE_TIMESTAMP(timer,;,;); // NOLINT + SET_RESIZE_TIMESTAMP( client, - SET_RESIZE_RMW_DEALLOC_TS( + SET_RESIZE_RMW_DEALLOC_TIMESTAMP( rmw_clients.clients, rmw_clients.timestamps, rmw_clients.client_count), - SET_RESIZE_RMW_REALLOC_TS( + SET_RESIZE_RMW_REALLOC_TIMESTAMP( client, rmw_clients.clients, rmw_clients.timestamps, rmw_clients.client_count) ); - SET_RESIZE_TS( + SET_RESIZE_TIMESTAMP( service, - SET_RESIZE_RMW_DEALLOC_TS( + SET_RESIZE_RMW_DEALLOC_TIMESTAMP( rmw_services.services, rmw_services.timestamps, rmw_services.service_count), - SET_RESIZE_RMW_REALLOC_TS( + SET_RESIZE_RMW_REALLOC_TIMESTAMP( service, rmw_services.services, rmw_services.timestamps, rmw_services.service_count) ); SET_RESIZE( From 25d41062779388bbf6d5a12c5519e7c0dfc10a29 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Wed, 1 Apr 2020 19:57:26 +0200 Subject: [PATCH 10/19] Add test for timestamp presence. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/CMakeLists.txt | 18 +++ rcl/test/rcl/test_wait.cpp | 2 + rcl/test/rcl/test_wait_timestamp.cpp | 172 +++++++++++++++++++++++++++ 3 files changed, 192 insertions(+) create mode 100644 rcl/test/rcl/test_wait_timestamp.cpp diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 324cde68a..4951b52d5 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -222,6 +222,24 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" ) + # This condition runs the timestmap test only when we know the RMW implementation + # supports timestamping + # TODO(iluetkeb): remove when timestamping is considered required + if(rmw_implementation STREQUAL "rmw_fastrtps_cpp") + set(AMENT_GTEST_ARGS "") + else() + message(STATUS "Skipping test_wait_timestamp${target_suffix} test, timestamps not implemented.") + set(AMENT_GTEST_ARGS "SKIP_TEST") + endif() + rcl_add_custom_gtest(test_wait_timestamp${target_suffix} + SRCS rcl/test_wait_timestamp.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ${AMENT_GTEST_ARGS} + ) + rcl_add_custom_gtest(test_wait_set_impl${target_suffix} SRCS rcl/test_wait_set_impl.cpp ENV ${rmw_implementation_env_var} diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index ccb3c8b06..d8eb2906f 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -29,6 +29,8 @@ #include "rcutils/logging_macros.h" +#include "test_msgs/msg/empty.h" + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) diff --git a/rcl/test/rcl/test_wait_timestamp.cpp b/rcl/test/rcl/test_wait_timestamp.cpp new file mode 100644 index 000000000..e4cf3590c --- /dev/null +++ b/rcl/test/rcl/test_wait_timestamp.cpp @@ -0,0 +1,172 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// 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 // for std::max +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/wait.h" + +#include "rcutils/logging_macros.h" + +#include "test_msgs/msg/empty.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (WaitSetTimestampTestFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr {nullptr}; + const rosidl_message_type_support_t * ts {nullptr}; + rcl_node_t * send_node_ptr {nullptr}; + rcl_node_t * receive_node_ptr {nullptr}; + rcl_publisher_t * pub_ptr {nullptr}; + rcl_subscription_t * sub_ptr {nullptr}; + test_msgs__msg__Empty msg; + + void SetUp() + { + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // type support for all + const char * TOPIC = "test_wait_timestamp_pub_sub"; + ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Empty); + EXPECT_NE(nullptr, ts); + + // create send node + send_node_ptr = new rcl_node_t; + *send_node_ptr = rcl_get_zero_initialized_node(); + rcl_node_options_t send_node_options = rcl_node_get_default_options(); + ret = rcl_node_init(send_node_ptr, "talker", "", context_ptr, &send_node_options); + + // receive node + receive_node_ptr = new rcl_node_t; + *receive_node_ptr = rcl_get_zero_initialized_node(); + rcl_node_options_t receive_node_options = rcl_node_get_default_options(); + ret = rcl_node_init(receive_node_ptr, "listener", "", this->context_ptr, &receive_node_options); + + // publisher + rcl_publisher_options_t pub_opt = rcl_publisher_get_default_options(); + pub_ptr = new rcl_publisher_t; + *pub_ptr = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init(pub_ptr, send_node_ptr, ts, TOPIC, &pub_opt); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_subscription_options_t sub_opt = rcl_subscription_get_default_options(); + sub_ptr = new rcl_subscription_t; + *sub_ptr = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(sub_ptr, receive_node_ptr, ts, TOPIC, &sub_opt); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + test_msgs__msg__Empty__init(&msg); + } + + void TearDown() + { + test_msgs__msg__Empty__fini(&msg); + + EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(pub_ptr, send_node_ptr)) << + rcl_get_error_string().str; + delete pub_ptr; + pub_ptr = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(sub_ptr, receive_node_ptr)) << + rcl_get_error_string().str; + delete sub_ptr; + sub_ptr = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(this->receive_node_ptr)) << rcl_get_error_string().str; + delete this->receive_node_ptr; + this->receive_node_ptr = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(this->send_node_ptr)) << rcl_get_error_string().str; + delete this->send_node_ptr; + this->send_node_ptr = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(this->context_ptr)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(this->context_ptr)) << rcl_get_error_string().str; + delete this->context_ptr; + this->context_ptr = nullptr; + } + + void wait_for_communication_ready() + { + // TODO(iluetkeb): check events to determine when the connection is there, + // instead of blocking the test for 1s... + std::this_thread::sleep_for(std::chrono::seconds(1)); + } +}; + +TEST_F(CLASSNAME(WaitSetTimestampTestFixture, RMW_IMPLEMENTATION), test_pub_sub) { + rcl_ret_t ret = RCL_RET_OK; + + // wait for setup to complete, then send two messages, a little time apart + wait_for_communication_ready(); + { + ret = rcl_publish(pub_ptr, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + ret = rcl_publish(pub_ptr, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + // wait for middleware + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = + rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_clear(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_subscription(&wait_set, sub_ptr, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1000)); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // check for presence indicator + EXPECT_NE(nullptr, wait_set.subscriptions[0]); + + // check for timestamp presence + EXPECT_NE(0LL, wait_set.subscriptions_timestamps[0]); + + // now take the message to clear it from the queue + ret = rcl_take(sub_ptr, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} From 2f93b493f878504cdb5c2c011d696a8b04ab1152 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Wed, 1 Apr 2020 19:59:19 +0200 Subject: [PATCH 11/19] Remove WS as requested. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 4951b52d5..fb31b27e6 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -248,7 +248,6 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" ) - rcl_add_custom_gtest(test_logging_rosout${target_suffix} SRCS rcl/test_logging_rosout.cpp ENV ${rmw_implementation_env_var} From 5920131135a6b63d33884e8c585b2508986bf293 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Wed, 1 Apr 2020 20:15:33 +0200 Subject: [PATCH 12/19] Copy over timestamp from rmw. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/wait.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index a3d828ea5..e08161a2e 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -710,6 +710,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) is_ready, ROS_PACKAGE_NAME, "Subscription in wait set is ready"); if (!is_ready) { wait_set->subscriptions[i] = NULL; + } else { + wait_set->subscriptions_timestamps[i] = wait_set->impl->rmw_subscriptions.timestamps[i]; } } // Set corresponding rcl guard_condition handles NULL. From c29bb2a9df826d94c067c6b1c2caa1d5a96deff2 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Wed, 1 Apr 2020 22:29:54 +0200 Subject: [PATCH 13/19] Timestamps for clients and services. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/wait.c | 4 ++ rcl/test/rcl/test_wait_timestamp.cpp | 84 ++++++++++++++++++++++++++++ 2 files changed, 88 insertions(+) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index e08161a2e..7637629a2 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -729,6 +729,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Client in wait set is ready"); if (!is_ready) { wait_set->clients[i] = NULL; + } else { + wait_set->clients_timestamps[i] = wait_set->impl->rmw_clients.timestamps[i]; } } // Set corresponding rcl service handles NULL. @@ -737,6 +739,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Service in wait set is ready"); if (!is_ready) { wait_set->services[i] = NULL; + } else { + wait_set->services_timestamps[i] = wait_set->impl->rmw_services.timestamps[i]; } } // Set corresponding rcl event handles NULL. diff --git a/rcl/test/rcl/test_wait_timestamp.cpp b/rcl/test/rcl/test_wait_timestamp.cpp index e4cf3590c..d780adbe1 100644 --- a/rcl/test/rcl/test_wait_timestamp.cpp +++ b/rcl/test/rcl/test_wait_timestamp.cpp @@ -27,10 +27,12 @@ #include "rcl/error_handling.h" #include "rcl/rcl.h" #include "rcl/wait.h" +#include "rcl/graph.h" #include "rcutils/logging_macros.h" #include "test_msgs/msg/empty.h" +#include "test_msgs/srv/basic_types.h" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -170,3 +172,85 @@ TEST_F(CLASSNAME(WaitSetTimestampTestFixture, RMW_IMPLEMENTATION), test_pub_sub) ret = rcl_take(sub_ptr, &msg, nullptr, nullptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } + +TEST_F(CLASSNAME(WaitSetTimestampTestFixture, RMW_IMPLEMENTATION), test_client_service) { + rcl_ret_t ret = RCL_RET_OK; + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * topic = "primitives"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init(&service, send_node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, send_node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // query the graph until the service becomes available + rcl_names_and_types_t tnat {}; + rcl_allocator_t allocator = rcl_get_default_allocator(); + do { + ret = rcl_get_service_names_and_types(receive_node_ptr, + &allocator, &tnat); + EXPECT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_names_and_types_fini(&tnat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + } while(tnat.names.size == 0); + + + // create the client + rcl_client_t client = rcl_get_zero_initialized_client(); + + // Initialize the client. + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init(&client, send_node_ptr, ts, topic, &client_options); + EXPECT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, receive_node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // send request + test_msgs__srv__BasicTypes_Request client_request; + test_msgs__srv__BasicTypes_Request__init(&client_request); + int64_t sequence_number; + ret = rcl_send_request(&client, &client_request, &sequence_number); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(sequence_number, 1); + + // wait for it + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = + rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, context_ptr, allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // first the service + ret = rcl_wait_set_add_service(&wait_set, &service, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1000)); + EXPECT_EQ(RCL_RET_OK, ret); + + bool request_received = false; + rcutils_time_point_value_t timestamp = 0; + for (size_t i = 0; i < wait_set.size_of_services; ++i) { + if (wait_set.services[i] && wait_set.services[i] == &service) { + request_received = true; + timestamp = wait_set.services_timestamps[i]; + } + } + EXPECT_EQ(true, request_received); + EXPECT_NE(0, timestamp); + +} From 43a458feb9a91f789da66a0690ded07ba23816d9 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 2 Apr 2020 20:38:38 +0200 Subject: [PATCH 14/19] Uncrustify Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_wait_timestamp.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rcl/test/rcl/test_wait_timestamp.cpp b/rcl/test/rcl/test_wait_timestamp.cpp index d780adbe1..38bd93f83 100644 --- a/rcl/test/rcl/test_wait_timestamp.cpp +++ b/rcl/test/rcl/test_wait_timestamp.cpp @@ -174,7 +174,7 @@ TEST_F(CLASSNAME(WaitSetTimestampTestFixture, RMW_IMPLEMENTATION), test_pub_sub) } TEST_F(CLASSNAME(WaitSetTimestampTestFixture, RMW_IMPLEMENTATION), test_client_service) { - rcl_ret_t ret = RCL_RET_OK; + rcl_ret_t ret = RCL_RET_OK; const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( test_msgs, srv, BasicTypes); const char * topic = "primitives"; @@ -185,23 +185,23 @@ TEST_F(CLASSNAME(WaitSetTimestampTestFixture, RMW_IMPLEMENTATION), test_client_s ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - rcl_ret_t ret = rcl_service_fini(&service, send_node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_ret_t ret = rcl_service_fini(&service, send_node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); // query the graph until the service becomes available rcl_names_and_types_t tnat {}; rcl_allocator_t allocator = rcl_get_default_allocator(); do { - ret = rcl_get_service_names_and_types(receive_node_ptr, - &allocator, &tnat); + ret = rcl_get_service_names_and_types( + receive_node_ptr, &allocator, &tnat); EXPECT_EQ(RCL_RET_OK, ret); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { rcl_ret_t ret = rcl_names_and_types_fini(&tnat); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); - } while(tnat.names.size == 0); + } while (tnat.names.size == 0); // create the client @@ -252,5 +252,4 @@ TEST_F(CLASSNAME(WaitSetTimestampTestFixture, RMW_IMPLEMENTATION), test_client_s } EXPECT_EQ(true, request_received); EXPECT_NE(0, timestamp); - } From 43986c248e4dc8df3c8a344bdd71c9cb95b158ab Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 2 Apr 2020 20:50:37 +0200 Subject: [PATCH 15/19] Add test for client timestamp. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_wait_timestamp.cpp | 48 ++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/rcl/test/rcl/test_wait_timestamp.cpp b/rcl/test/rcl/test_wait_timestamp.cpp index 38bd93f83..4ba809360 100644 --- a/rcl/test/rcl/test_wait_timestamp.cpp +++ b/rcl/test/rcl/test_wait_timestamp.cpp @@ -1,4 +1,5 @@ // Copyright 2016 Open Source Robotics Foundation, Inc. +// Copyright 2020 Robert Bosch GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -252,4 +253,51 @@ TEST_F(CLASSNAME(WaitSetTimestampTestFixture, RMW_IMPLEMENTATION), test_client_s } EXPECT_EQ(true, request_received); EXPECT_NE(0, timestamp); + + // now reply and check the client + { + test_msgs__srv__BasicTypes_Response service_response; + test_msgs__srv__BasicTypes_Response__init(&service_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Response__fini(&service_response); + }); + + // Initialize a separate instance of the request and take the pending request. + test_msgs__srv__BasicTypes_Request service_request; + test_msgs__srv__BasicTypes_Request__init(&service_request); + rmw_request_id_t header; + ret = rcl_take_request(&service, &header, &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_send_response(&service, &header, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + test_msgs__srv__BasicTypes_Request__fini(&service_request); + } + + // wait for it + ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + wait_set = rcl_get_zero_initialized_wait_set(); + ret = + rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, 0, context_ptr, allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // we have a scope exit set up above already + + // now wait for the client only + ret = rcl_wait_set_add_client(&wait_set, &client, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1000)); + EXPECT_EQ(RCL_RET_OK, ret); + + bool response_received = false; + rcutils_time_point_value_t response_timestamp = 0; + for (size_t i = 0; i < wait_set.size_of_clients; ++i) { + if (wait_set.clients[i] && wait_set.clients[i] == &client) { + response_received = true; + response_timestamp = wait_set.clients_timestamps[i]; + } + } + EXPECT_EQ(true, response_received); + EXPECT_NE(0, response_timestamp); } From 7769ae2922603c1b19d0d14bc5ea3d908f05a33e Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 2 Apr 2020 20:50:41 +0200 Subject: [PATCH 16/19] Copyright. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_wait.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index d8eb2906f..2cc7f737b 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -1,4 +1,5 @@ // Copyright 2016 Open Source Robotics Foundation, Inc. +// Copyright 2020 Robert Bosch GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From f1497020cecfa47c91d898bbd3de2205849a130e Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 2 Apr 2020 21:14:10 +0200 Subject: [PATCH 17/19] Set timer timestamps. Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/wait.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 7637629a2..93583a3e7 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -696,6 +696,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) RCUTILS_LOG_DEBUG_EXPRESSION_NAMED(is_ready, ROS_PACKAGE_NAME, "Timer in wait set is ready"); if (!is_ready) { wait_set->timers[i] = NULL; + wait_set->timers_timestamps[i] = 0; + } else if(wait_set->timers_timestamps[i] == 0) { + ret = rcutils_system_time_now(&(wait_set->timers_timestamps[i])); + if(ret != RCL_RET_OK) { + wait_set->timers_timestamps[i] = 0; + // TODO(iluetkeb) should we report this as an error? + } } } // Check for timeout, return RCL_RET_TIMEOUT only if it wasn't a timer. From 9a55f0eafc608d768d5baf473691a7594e3b1231 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Wed, 8 Apr 2020 22:29:43 +0200 Subject: [PATCH 18/19] space typo Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/src/rcl/wait.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 93583a3e7..9f7d8f9a9 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -365,7 +365,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al RCL_SET_ERROR_MSG("allocating memory failed"); \ return RCL_RET_BAD_ALLOC; \ } \ - memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * Type ## s_size); + memset(wait_set->impl->RMWTimestamps, 0, sizeof(rcutils_time_point_value_t) * Type ## s_size); /* Implementation-specific notes: * @@ -699,7 +699,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) wait_set->timers_timestamps[i] = 0; } else if(wait_set->timers_timestamps[i] == 0) { ret = rcutils_system_time_now(&(wait_set->timers_timestamps[i])); - if(ret != RCL_RET_OK) { + if (ret != RCL_RET_OK) { wait_set->timers_timestamps[i] = 0; // TODO(iluetkeb) should we report this as an error? } From 721dbdd30e801f23348ff92d3bd55c197289cb50 Mon Sep 17 00:00:00 2001 From: "Luetkebohle Ingo (CR/AEX3)" Date: Thu, 9 Apr 2020 09:14:02 +0200 Subject: [PATCH 19/19] Remove unnecessary include Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rcl/test/rcl/test_wait.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 2cc7f737b..98c2b34b7 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -30,8 +30,6 @@ #include "rcutils/logging_macros.h" -#include "test_msgs/msg/empty.h" - #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX)