From b5b78cd95356b08c15187ab1a1316ccfea293d6f Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Thu, 4 Apr 2024 21:07:55 +0200 Subject: [PATCH] Added a new implementation of the RealtimeBox with added best effort behaviour (#139) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Lennart Nachtigall Co-authored-by: Lennart Nachtigall Co-authored-by: Christoph Froehlich Co-authored-by: Christoph Fröhlich --- CMakeLists.txt | 4 + .../realtime_tools/realtime_box_best_effort.h | 232 ++++++++++++++++++ test/realtime_box_best_effort_tests.cpp | 179 ++++++++++++++ 3 files changed, 415 insertions(+) create mode 100644 include/realtime_tools/realtime_box_best_effort.h create mode 100644 test/realtime_box_best_effort_tests.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4743d984..21f2e96e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_action Threads + rcpputils ) find_package(ament_cmake REQUIRED) @@ -45,6 +46,9 @@ if(BUILD_TESTING) ament_add_gmock(realtime_box_tests test/realtime_box_tests.cpp) target_link_libraries(realtime_box_tests realtime_tools) + ament_add_gmock(realtime_box_best_effort_tests test/realtime_box_best_effort_tests.cpp) + target_link_libraries(realtime_box_best_effort_tests realtime_tools) + ament_add_gmock(realtime_buffer_tests test/realtime_buffer_tests.cpp) target_link_libraries(realtime_buffer_tests realtime_tools) diff --git a/include/realtime_tools/realtime_box_best_effort.h b/include/realtime_tools/realtime_box_best_effort.h new file mode 100644 index 00000000..4763c5a0 --- /dev/null +++ b/include/realtime_tools/realtime_box_best_effort.h @@ -0,0 +1,232 @@ +// Copyright (c) 2024, Lennart Nachtigall +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: Lennart Nachtigall + +#ifndef REALTIME_TOOLS__REALTIME_BOX_BEST_EFFORT_H_ +#define REALTIME_TOOLS__REALTIME_BOX_BEST_EFFORT_H_ + +#include +#include +#include +#include + +#include + +namespace realtime_tools +{ + +template +constexpr auto is_ptr_or_smart_ptr = rcpputils::is_pointer::value; + +/*! + A Box that ensures thread safe access to the boxed contents. + Access is best effort. If it can not lock it will return. + + NOTE about pointers: + You can use pointers with this box but the access will be different. + Only use the get/set methods that take function pointer for accessing the internal value. +*/ +template +class RealtimeBoxBestEffort +{ + static_assert( + std::is_same_v || std::is_same_v); + static_assert(std::is_copy_constructible_v, "Passed type must be copy constructible"); + +public: + using mutex_t = mutex_type; + using type = T; + // Provide various constructors + constexpr explicit RealtimeBoxBestEffort(const T & init = T{}) : value_(init) {} + constexpr explicit RealtimeBoxBestEffort(const T && init) : value_(std::move(init)) {} + + // Only enabled for types that can be constructed from an initializer list + template + constexpr RealtimeBoxBestEffort( + const std::initializer_list & init, + std::enable_if_t>) + : value_(init) + { + } + + /** + * @brief set a new content with best effort + * @return false if mutex could not be locked + * @note disabled for pointer types + */ + template + typename std::enable_if_t, bool> trySet(const T & value) + { + std::unique_lock guard(lock_, std::defer_lock); + if (!guard.try_lock()) { + return false; + } + value_ = value; + return true; + } + /** + * @brief access the content readable with best effort + * @return false if the mutex could not be locked + * @note only safe way to access pointer type content (rw) + */ + bool trySet(const std::function & func) + { + std::unique_lock guard(lock_, std::defer_lock); + if (!guard.try_lock()) { + return false; + } + + func(value_); + return true; + } + /** + * @brief get the content with best effort + * @return std::nullopt if content could not be access, otherwise the content is returned + */ + template + [[nodiscard]] typename std::enable_if_t, std::optional> tryGet() const + { + std::unique_lock guard(lock_, std::defer_lock); + if (!guard.try_lock()) { + return std::nullopt; + } + return value_; + } + /** + * @brief access the content (r) with best effort + * @return false if the mutex could not be locked + * @note only safe way to access pointer type content (r) + */ + bool tryGet(const std::function & func) + { + std::unique_lock guard(lock_, std::defer_lock); + if (!guard.try_lock()) { + return false; + } + + func(value_); + return true; + } + + /** + * @brief set the content and wait until the mutex could be locked (RealtimeBox behavior) + * @return true + */ + template + typename std::enable_if_t, void> set(const T & value) + { + std::lock_guard guard(lock_); + // cppcheck-suppress missingReturn + value_ = value; + } + /** + * @brief access the content (rw) and wait until the mutex could locked + */ + void set(const std::function & func) + { + std::lock_guard guard(lock_); + func(value_); + } + + /** + * @brief get the content and wait until the mutex could be locked (RealtimeBox behaviour) + * @return copy of the value + */ + template + [[nodiscard]] typename std::enable_if_t, U> get() const + { + std::lock_guard guard(lock_); + return value_; + } + /** + * @brief get the content and wait until the mutex could be locked + * @note same signature as in the existing RealtimeBox + */ + template + typename std::enable_if_t, void> get(T & in) const + { + std::lock_guard guard(lock_); + // cppcheck-suppress missingReturn + in = value_; + } + /** + * @brief access the content (r) and wait until the mutex could be locked + * @note only safe way to access pointer type content (r) + * @note same signature as in the existing RealtimeBox + */ + void get(const std::function & func) + { + std::lock_guard guard(lock_); + func(value_); + } + + /** + * @brief provide a custom assignment operator for easier usage + * @note only to be used from non-RT! + */ + template + typename std::enable_if_t, void> operator=(const T & value) + { + set(value); + } + + /** + * @brief provide a custom conversion operator + * @note Can only be used from non-RT! + */ + template >> + [[nodiscard]] operator T() const + { + // Only makes sense with the getNonRT method otherwise we would return an std::optional + return get(); + } + /** + * @brief provide a custom conversion operator + * @note Can be used from non-RT and RT contexts + */ + template >> + [[nodiscard]] operator std::optional() const + { + return tryGet(); + } + + // In case one wants to actually use a pointer + // in this implementation we allow accessing the lock directly. + // Note: Be careful with lock.unlock(). + // It may only be called from the thread that locked the mutex! + [[nodiscard]] const mutex_t & getMutex() const { return lock_; } + [[nodiscard]] mutex_t & getMutex() { return lock_; } + +private: + T value_; + mutable mutex_t lock_; +}; +} // namespace realtime_tools + +#endif // REALTIME_TOOLS__REALTIME_BOX_BEST_EFFORT_H_ diff --git a/test/realtime_box_best_effort_tests.cpp b/test/realtime_box_best_effort_tests.cpp new file mode 100644 index 00000000..1cda8186 --- /dev/null +++ b/test/realtime_box_best_effort_tests.cpp @@ -0,0 +1,179 @@ +// Copyright (c) 2024, Lennart Nachtigall +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Willow Garage, Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// Author: Lennart Nachtigall + +#include +#include + +struct DefaultConstructable +{ + int a = 10; + std::string str = "hallo"; +}; + +struct NonDefaultConstructable +{ + NonDefaultConstructable(int a_, const std::string & str_) : a(a_), str(str_) {} + int a; + std::string str; +}; + +struct FromInitializerList +{ + FromInitializerList(std::initializer_list list) + { + std::copy(list.begin(), list.end(), data.begin()); + } + std::array data; +}; + +using realtime_tools::RealtimeBoxBestEffort; + +TEST(RealtimeBoxBestEffort, empty_construct) +{ + RealtimeBoxBestEffort box; + + auto value = box.get(); + EXPECT_EQ(value.a, 10); + EXPECT_EQ(value.str, "hallo"); +} + +TEST(RealtimeBoxBestEffort, default_construct) +{ + DefaultConstructable data; + data.a = 100; + + RealtimeBoxBestEffort box(data); + + auto value = box.get(); + EXPECT_EQ(value.a, 100); + EXPECT_EQ(value.str, "hallo"); +} + +TEST(RealtimeBoxBestEffort, non_default_constructable) +{ + RealtimeBoxBestEffort box(NonDefaultConstructable(-10, "hello")); + + auto value = box.get(); + EXPECT_EQ(value.a, -10); + EXPECT_EQ(value.str, "hello"); +} +TEST(RealtimeBoxBestEffort, standard_get) +{ + RealtimeBoxBestEffort box(DefaultConstructable{.a = 1000}); + + DefaultConstructable data; + box.get(data); + EXPECT_EQ(data.a, 1000); + data.a = 10000; + + box.set(data); + + auto value = box.get(); + EXPECT_EQ(value.a, 10000); +} + +TEST(RealtimeBoxBestEffort, initializer_list) +{ + RealtimeBoxBestEffort box({1, 2, 3}); + + auto value = box.get(); + EXPECT_EQ(value.data[0], 1); + EXPECT_EQ(value.data[1], 2); + EXPECT_EQ(value.data[2], 3); +} + +TEST(RealtimeBoxBestEffort, assignment_operator) +{ + DefaultConstructable data; + data.a = 1000; + RealtimeBoxBestEffort box; + // Assignment operator is always non RT! + box = data; + + auto value = box.get(); + EXPECT_EQ(value.a, 1000); +} +TEST(RealtimeBoxBestEffort, typecast_operator) +{ + RealtimeBoxBestEffort box(DefaultConstructable{.a = 100, .str = ""}); + + // Use non RT access + DefaultConstructable data = box; + + EXPECT_EQ(data.a, 100); + + // Use RT access -> returns std::nullopt if the mutex could not be locked + std::optional rt_data_access = box; + + if (rt_data_access) { + EXPECT_EQ(rt_data_access->a, 100); + } +} + +TEST(RealtimeBoxBestEffort, pointer_type) +{ + int a = 100; + int * ptr = &a; + + RealtimeBoxBestEffort box(ptr); + // This does not and should not compile! + // auto value = box.get(); + + // Instead access it via a passed function. + // This assures that we access the data within the scope of the lock + box.get([](const auto & i) { EXPECT_EQ(*i, 100); }); + + box.set([](auto & i) { *i = 200; }); + + box.get([](const auto & i) { EXPECT_EQ(*i, 200); }); + + box.tryGet([](const auto & i) { EXPECT_EQ(*i, 200); }); +} + +TEST(RealtimeBoxBestEffort, smart_ptr_type) +{ + std::shared_ptr ptr = std::make_shared(100); + + RealtimeBoxBestEffort box(ptr); + // This does not and should not compile! + // auto value = box.get(); + + // Instead access it via a passed function. + // This assures that we access the data within the scope of the lock + box.get([](const auto & i) { EXPECT_EQ(*i, 100); }); + + box.set([](auto & i) { *i = 200; }); + + box.get([](const auto & i) { EXPECT_EQ(*i, 200); }); + + box.trySet([](const auto & p) { *p = 10; }); + + box.tryGet([](const auto & p) { EXPECT_EQ(*p, 10); }); +}