diff --git a/CMakeLists.txt b/CMakeLists.txt index 21f2e96..de507fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,9 +46,6 @@ 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.h b/include/realtime_tools/realtime_box.h index c5fc300..79308df 100644 --- a/include/realtime_tools/realtime_box.h +++ b/include/realtime_tools/realtime_box.h @@ -1,4 +1,4 @@ -// Copyright (c) 2009, Willow Garage, Inc. +// 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: @@ -26,50 +26,402 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -// Author: Stuart Glaser +// Author: Lennart Nachtigall -#ifndef REALTIME_TOOLS__REALTIME_BOX_H__ -#define REALTIME_TOOLS__REALTIME_BOX_H__ +#ifndef REALTIME_TOOLS__REALTIME_BOX_H_ +#define REALTIME_TOOLS__REALTIME_BOX_H_ +#include #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. +*/ + +// Provide a base template for the class +template > +class RealtimeBox; + +// Provide a specialisation for non pointer types +// NOTE: When migrating to a safe access only version just remove the specialisation for pointer +// and let this be the only version! +template +class RealtimeBox +{ + static_assert(!is_ptr_or_smart_ptr); + 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 RealtimeBox(const T & init = T{}) : value_(init) {} + constexpr explicit RealtimeBox(const T && init) : value_(std::move(init)) {} + + // Only enabled for types that can be constructed from an initializer list + template + constexpr RealtimeBox( + 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); + } - Strongly suggested that you use an std::shared_ptr in this box to - guarantee realtime safety. + /** + * @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_; } - */ -template -class RealtimeBox +private: + T value_; + mutable mutex_t lock_; +}; + +/** + * @brief Specialisation for pointer types. + * WHY is this specialised. We do not want to break compatibility but show a deprecation note + * for get/set etc. methods if used with a pointer type + * They are unsafe to use and should therefore be replace with their correspondents that take an std::function for accessing + * the value behind the pointer +*/ +template +class RealtimeBox { + static_assert(is_ptr_or_smart_ptr); + static_assert( + std::is_same_v || std::is_same_v); + static_assert(std::is_copy_constructible_v, "Passed type must be copy constructible"); + public: - explicit RealtimeBox(const T & initial = T()) : thing_(initial) {} + using mutex_t = mutex_type; + using type = T; + // Provide various constructors + constexpr explicit RealtimeBox(const T & init = T{}) : value_(init) {} + constexpr explicit RealtimeBox(const T && init) : value_(std::move(init)) {} + + // Only enabled for types that can be constructed from an initializer list + template + constexpr RealtimeBox( + 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 + */ + [[deprecated("trySet is not safe for pointer types - use trySet(std::function...) instead")]] + 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 + */ + [[deprecated( + "tryGet is not safe for pointer types - use tryGet(std::function...) " + "instead")]] [[nodiscard]] 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 + */ + [[deprecated("set is not safe for pointer types - use set(std::function...) instead")]] void set(const T & value) { - std::lock_guard guard(thing_lock_RT_); - thing_ = 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_); } - void get(T & ref) + /** + * @brief get the content and wait until the mutex could be locked (RealtimeBox behaviour) + * @return copy of the value + */ + [[deprecated( + "get is not safe for pointer types - use get(std::function...) instead")]] [[nodiscard]] T + 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 + */ + [[deprecated("get is not safe for pointer types - use get(std::function...) instead")]] + 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(thing_lock_RT_); - ref = thing_; + 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: - // The thing that's in the box. - T thing_; - - // Protects access to the thing in the box. This mutex is - // guaranteed to be locked for no longer than the duration of the - // copy, so as long as the copy is realtime safe and the OS has - // priority inheritance for mutexes, this lock can be safely locked - // from within realtime. - std::mutex thing_lock_RT_; + T value_; + mutable mutex_t lock_; }; } // namespace realtime_tools diff --git a/include/realtime_tools/realtime_box_best_effort.h b/include/realtime_tools/realtime_box_best_effort.h deleted file mode 100644 index 4763c5a..0000000 --- a/include/realtime_tools/realtime_box_best_effort.h +++ /dev/null @@ -1,232 +0,0 @@ -// 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 deleted file mode 100644 index 1cda818..0000000 --- a/test/realtime_box_best_effort_tests.cpp +++ /dev/null @@ -1,179 +0,0 @@ -// 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); }); -} diff --git a/test/realtime_box_tests.cpp b/test/realtime_box_tests.cpp index 1c443f6..21d4178 100644 --- a/test/realtime_box_tests.cpp +++ b/test/realtime_box_tests.cpp @@ -1,3 +1,4 @@ +// Copyright (c) 2024, Lennart Nachtigall // Copyright (c) 2019, Open Source Robotics Foundation, Inc. // // Redistribution and use in source and binary forms, with or without @@ -10,7 +11,7 @@ // 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 Open Source Robotics Foundation, Inc. nor the names of its +// * 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. // @@ -29,28 +30,194 @@ #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::RealtimeBox; -class DefaultConstructable +TEST(RealtimeBox, empty_construct) +{ + RealtimeBox box; + + auto value = box.get(); + EXPECT_EQ(value.a, 10); + EXPECT_EQ(value.str, "hallo"); +} + +TEST(RealtimeBox, default_construct) +{ + DefaultConstructable data; + data.a = 100; + + RealtimeBox box(data); + + auto value = box.get(); + EXPECT_EQ(value.a, 100); + EXPECT_EQ(value.str, "hallo"); +} + +TEST(RealtimeBox, non_default_constructable) +{ + RealtimeBox box(NonDefaultConstructable(-10, "hello")); + + auto value = box.get(); + EXPECT_EQ(value.a, -10); + EXPECT_EQ(value.str, "hello"); +} +TEST(RealtimeBox, standard_get) +{ + RealtimeBox 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(RealtimeBox, initializer_list) +{ + RealtimeBox 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(RealtimeBox, assignment_operator) +{ + DefaultConstructable data; + data.a = 1000; + RealtimeBox box; + // Assignment operator is always non RT! + box = data; + + auto value = box.get(); + EXPECT_EQ(value.a, 1000); +} +TEST(RealtimeBox, typecast_operator) +{ + RealtimeBox 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(RealtimeBox, pointer_type) +{ + int a = 100; + int * ptr = &a; + + RealtimeBox 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(RealtimeBox, smart_ptr_type) +{ + std::shared_ptr ptr = std::make_shared(100); + + RealtimeBox> box(ptr); + + // 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); }); +} + +TEST(RealtimeBox, deprecated_note) +{ + int a = 100; + int * ptr = &a; + + RealtimeBox box(ptr); + + int * res; + box.get(res); + EXPECT_EQ(*res, 100); + + EXPECT_EQ(*box.get(), 100); + + std::shared_ptr sptr = std::make_shared(10); + + RealtimeBox> sbox(sptr); + + EXPECT_EQ(*sbox.get(), 10); +} + +// These are the tests from the old RealtimeBox implementation +// They are therefore suffixed with _existing + +class DefaultConstructable_existing { public: - DefaultConstructable() : number_(42) {} - ~DefaultConstructable() {} + DefaultConstructable_existing() : number_(42) {} + ~DefaultConstructable_existing() {} int number_; }; -TEST(RealtimeBox, default_construct) +TEST(RealtimeBox, default_construct_existing) { - DefaultConstructable thing; + DefaultConstructable_existing thing; thing.number_ = 5; - RealtimeBox box; + RealtimeBox box; box.get(thing); EXPECT_EQ(42, thing.number_); } -TEST(RealtimeBox, initial_value) +TEST(RealtimeBox, initial_value_existing) { RealtimeBox box(3.14); double num = 0.0; @@ -58,7 +225,7 @@ TEST(RealtimeBox, initial_value) EXPECT_DOUBLE_EQ(3.14, num); } -TEST(RealtimeBox, set_and_get) +TEST(RealtimeBox, set_and_get_existing) { RealtimeBox box('a');