From d2deffe3a677138ba9dc1fad666084e06a6c5a22 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Mon, 5 Apr 2021 00:09:29 -0400 Subject: [PATCH 01/13] new realtime_buffer-like class that works for reading and writing to realtime threads --- CMakeLists.txt | 3 + include/realtime_tools/realtime_barrier.hpp | 437 ++++++++++++++++++++ test/realtime_barrier_tests.cpp | 217 ++++++++++ 3 files changed, 657 insertions(+) create mode 100644 include/realtime_tools/realtime_barrier.hpp create mode 100644 test/realtime_barrier_tests.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4edcf36e..373f9957 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,9 @@ if(BUILD_TESTING) ament_add_gmock(realtime_buffer_tests test/realtime_buffer_tests.cpp) target_link_libraries(realtime_buffer_tests ${PROJECT_NAME} ${GMOCK_MAIN_LIBRARIES}) + ament_add_gmock(realtime_barrier_tests test/realtime_barrier_tests.cpp) + target_link_libraries(realtime_barrier_tests ${PROJECT_NAME} ${GMOCK_MAIN_LIBRARIES}) + ament_add_gmock(realtime_clock_tests test/realtime_clock_tests.cpp) target_link_libraries(realtime_clock_tests ${PROJECT_NAME} ${GMOCK_MAIN_LIBRARIES}) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp new file mode 100644 index 00000000..c12239eb --- /dev/null +++ b/include/realtime_tools/realtime_barrier.hpp @@ -0,0 +1,437 @@ +// +// Created by guru on 2/22/21. +// + +#ifndef REALTIME_TOOLS__REALTIME_BARRIER_H +#define REALTIME_TOOLS__REALTIME_BARRIER_H + +#include +#include + + +namespace realtime_tools +{ + +///@brief Attempt a lock on a resource but fail if already locked +class try_lock : public std::unique_lock +{ +public: + explicit try_lock(std::mutex & m) + : std::unique_lock(m, std::try_to_lock) + {} +}; + +///@brief Lock a resource or wait for it to be free +class wait_lock : public std::unique_lock +{ +public: + explicit wait_lock(std::mutex & m) + : std::unique_lock(m, std::defer_lock) + { + // will wait some time + while (!try_lock()) { + std::this_thread::sleep_for(std::chrono::microseconds(500)); + } + } +}; + +///@brief default options for realtime acess to resources +struct realtime +{ + using lock_strategy = try_lock; +}; + +///@brief default options for non-realtime access to resources +struct non_realtime +{ + using lock_strategy = wait_lock; +}; + + +///@brief Implements the underlying mechanisms for communicating between threads. +/// MemoryBarrier implements a double buffer swapping mechanism for interchanging data between +/// threads. It allocates two T objects on the heap. When MemoryBarrier::swap() is called the two +/// pointers are swapped. Generally one pointer is accessed from the non-realtime thread and the +/// other is used by the realtime thread. Only one-way communication, in either direction, is +/// possible. For two-way communication two separate MemoryBarriers can be used as a pair. +/// +/// You should not use this class directly, you will work with ReadBarrier, WriteBarrier and +// DirectAccess classes only. +/// +/// General strategy: +/// For non-RT writing to RT thread, non-RT thread writes but doesnt swap, RT swaps and reads. +/// For RT writing back to non-RT, RT writes but doesnt swap, NRT swaps and reads at will +template +class MemoryBarrier +{ +public: + ///@brief Provides direct access to data + /// Should only be used from within the realtime thread. Default template args specify + /// DirectAccess should only try to lock the resource and on failure the DirectAccess object + /// would be equal to nullptr and therefore evaluate to false. + /// This is also used internally by the non-RT ReadBarrier and WriteBarrier classes but + /// implements the thread-safe access. + template + class DirectAccess : public lock_strategy + { +public: + using MemoryBarrierType = MemoryBarrier; + + explicit DirectAccess(MemoryBarrierType & mem_barrier) noexcept + : lock_strategy(mem_barrier.mutex_), mem_(&mem_barrier), obj_(nullptr) + { + obj_ = _get(); // will return nullptr if we dont own the lock yet + } + + template + explicit DirectAccess(X & mem_barrier) noexcept + : lock_strategy(mem_barrier.memory().mutex_), mem_(&mem_barrier.memory()), obj_(nullptr) + { + + obj_ = _get(); // will return nullptr if we dont own the lock yet + } + + template + explicit DirectAccess(X * mem_barrier) noexcept + : lock_strategy(mem_barrier->memory().mutex_), mem_(&mem_barrier->memory()), obj_(nullptr) + { + obj_ = _get(); // will return nullptr if we dont own the lock yet + } + + + // do not allow copying, only move semantics + DirectAccess(const DirectAccess &) = delete; + + DirectAccess & operator=(const DirectAccess &) = delete; + + DirectAccess(DirectAccess && move) noexcept + : mem_(move.mem_), obj_(move.obj_) + { + move.mem_ = nullptr; + } + + DirectAccess & operator=(DirectAccess && move) noexcept + { + mem_ = move.mem_; + move.mem_ = nullptr; + obj_ = move.obj_; + return *this; + } + + inline void reset() + { + if (lock_strategy::owns_lock()) { + lock_strategy::unlock(); + } + mem_ = nullptr; + obj_ = nullptr; + } + + inline bool new_data_available() const + { + return mem_->new_data_available(); + } + + inline void new_data_available(bool avail) + { + if (!lock_strategy::owns_lock()) { + throw std::runtime_error("can't modify an unlocked MemoryBarrier"); + } + mem_->new_data_available_ = avail; + } + + inline void swap() + { + if (!std::unique_lock::owns_lock()) { + throw std::runtime_error("can't swap an unlocked MemoryBarrier"); + } + mem_->swap(); + obj_ = nullptr; // must explicitly set to null so _get doesnt return cached value + obj_ = _get(); + } + + inline explicit operator bool() const + {return mem_ != nullptr && get();} + + inline bool operator==(std::nullptr_t) const + {return mem_ == nullptr && get();} + + inline bool operator!=(std::nullptr_t) const + {return mem_ != nullptr && get();} + + // smartptr semantics + inline T * get() + {return _get();} + + inline const T * get() const + {return _get();} + + inline T * operator->() + {return get();} + + inline const T * operator->() const + {return get();} + + inline T & operator*() + {return *get();} + + inline const T & operator*() const + {return *get();} + +private: + MemoryBarrierType * mem_; + mutable T * obj_; + + template + inline typename std::enable_if_t::value, + T *> + _get() + { + return obj_ ? + obj_ : + lock_strategy::owns_lock() ? + obj_ = mem_->rt_ : + nullptr; + } + + template + inline typename std::enable_if_t::value, T *> + _get() + { + return obj_ ? + obj_ : + lock_strategy::owns_lock() ? + obj_ = mem_->nrt_ : + nullptr; + } + + template + inline typename std::enable_if_t::value, + const T *> + _get() const + { + return obj_ ? + obj_ : + lock_strategy::owns_lock() ? + obj_ = mem_->rt_ : + nullptr; + } + + template + inline typename std::enable_if_t::value, const T *> + _get() const + { + return obj_ ? + obj_ : + lock_strategy::owns_lock() ? + obj_ = mem_->nrt_ : + nullptr; + } + + }; + + + MemoryBarrier() + : nrt_(new T()), rt_(new T()), polarity_(false), new_data_available_(false) + { + } + + explicit MemoryBarrier(const T & data) + : nrt_(new T(data)), rt_(new T(data)), polarity_(false), new_data_available_(false) + { + } + + explicit MemoryBarrier(MemoryBarrier && move) noexcept + : nrt_(move.nrt_), rt_(move.rt_), polarity_(move.polarity_), new_data_available_( + move.new_data_available_) + { + move.nrt_ = nullptr; + move.rt_ = nullptr; + move.polarity_ = false; + move.new_data_available_ = false; + } + + /*MemoryBarrier(const MemoryBarrier& copy) + : in_(new T(*copy.in_)), out_(new T(*copy.out_)), polarity_(copy.polarity_), new_data_available_(copy.new_data_available_) {} }*/ + + // allow moving but not copying + MemoryBarrier(const MemoryBarrier &) = delete; + + MemoryBarrier & operator=(const MemoryBarrier &) = delete; + + + // todo: perhaps we need a new_data on both nrt and rt side + // true if new data is available, depends on if the memory is being used for Read from RT or Write to RT mode + inline bool new_data_available() const + {return new_data_available_;} + + void initialize(const T & value) + { + wait_lock guard(mutex_); + if (guard.owns_lock()) { + *nrt_ = *rt_ = value; + } else { + throw std::runtime_error("request to initialize realtime barrier failed trying to lock"); + } + } + +protected: + T * nrt_; + T * rt_; + bool polarity_; // flipped each time barrier rotates (swaps) + bool new_data_available_; + + // Set as mutable so that readFromNonRT() can be performed on a const buffer + mutable std::mutex mutex_; + + inline void swap() + { + // swap pointers + T * tmp = nrt_; + nrt_ = rt_; + rt_ = tmp; + polarity_ = !polarity_; + } + + friend class DirectAccess; + + friend class DirectAccess; +}; + +///@brief Create a barrier for reading data from a realtime thread +/// ReadBarrier implements reading data from a realtime thread using a MemoryBarrier. For example, +/// this is used to transfer state data out of hardware interfaces. The default constructor will +// create a new memory barrier for use. There is an alternate constructor if you want to create +// your own MemoryBarrier explicitly. +template +class ReadBarrier +{ +public: + using MemoryBarrierType = MemoryBarrier; + //using DirectAccessType = typename T::DirectAccess<>; + + ReadBarrier() noexcept + : mem_(new MemoryBarrierType()), owns_mem(true) + { + } + + explicit ReadBarrier(MemoryBarrierType & mem_barrier) noexcept + : mem_(&mem_barrier), owns_mem(false) + { + } + + virtual ~ReadBarrier() + { + if (owns_mem) { + delete mem_; + } + } + + //inline operator MemoryBarrier&() { return *mem_; } + + ///@brief Access the underlying MemoryBarrier object + inline MemoryBarrierType & memory() + {return *mem_;} + + ///@brief Get current value from non-realtime side without affecting the barrier. + /// Copy the current data into dest. No swap is performed and the new_data_available flag is unaffected. + bool current(T & dest) + { + typename MemoryBarrierType::template DirectAccess direct(*mem_); + if (direct) { + dest = *direct; + return true; + } else { + return false; + } + } + + ///@brief Read data out of the realtime thread + /// Swap RT buffer for non-RT. Copy the new data into val and reset the new_data_available flag. + // todo: since this read also swaps, should it be called read_and_swap() or pop() or pull() + bool pull(T & dest) + { + typename MemoryBarrierType::template DirectAccess direct(*mem_); + if (direct && mem_->new_data_available()) { + direct.swap(); + direct.new_data_available(false); + dest = *direct; + return true; + } else { + return false; // failed to read + } + } + +private: + MemoryBarrierType * mem_; + bool owns_mem; +}; + + +///@brief Create a barrier for writing data to a realtime thread +/// WriteBarrier implements writing data to a realtime thread from a non-realtime thread using a +/// MemoryBarrier. For example, this is used to transfer commands to a hardware interface. The +/// default constructor will create a new memory barrier for use. There is an alternate +/// constructor if you want to create your own MemoryBarrier explicitly. +template +class WriteBarrier +{ +public: + using MemoryBarrierType = MemoryBarrier; + //using DirectAccessType = DirectAccess; + + WriteBarrier() noexcept + : mem_(new MemoryBarrierType()) + { + } + + WriteBarrier(MemoryBarrierType & mem_barrier) noexcept + : mem_(&mem_barrier) + { + } + + ///@brief Access the underlying MemoryBarrier object + inline MemoryBarrierType & memory() + {return *mem_;} + + ///@brief Get current value from non-realtime side without affecting the barrier. + /// Copy the current data into dest. No swap is performed and the new_data_available flag + /// is unaffected. + bool current(T & dest) + { + typename MemoryBarrierType::template DirectAccess direct(*mem_); + if (direct) { + dest = *direct; + return true; + } else { + return false; + } + } + + ///@brief Write data into the realtime thread + /// Copy the new data from val into non-RT buffer then swap RT buffer for non-RT and set the + // new_data_available flag. + bool push(const T & data) + { + typename MemoryBarrierType::template DirectAccess direct(*mem_); + if (direct) { + *direct = data; + direct.swap(); + direct.new_data_available(true); + return true; + } else { + return false; + } + } + +private: + MemoryBarrierType * mem_; +}; + +} + + +#endif // REALTIME_TOOLS__REALTIME_BARRIER_H diff --git a/test/realtime_barrier_tests.cpp b/test/realtime_barrier_tests.cpp new file mode 100644 index 00000000..77c7a8d0 --- /dev/null +++ b/test/realtime_barrier_tests.cpp @@ -0,0 +1,217 @@ +/* + * Copyright (c) 2019, Open Source Robotics Foundation, Inc. + * All rights reserved. + * + * 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 OWNER 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. + */ + +#include +#include + +using realtime_tools::MemoryBarrier; +using realtime_tools::ReadBarrier; +using realtime_tools::WriteBarrier; + +class DefaultConstructable +{ +public: + DefaultConstructable() + : number_(42) {} + ~DefaultConstructable() {} + int number_; +}; + +/* + * MemoryBarriers - base thread barrier mechanisms + */ + +TEST(MemoryBarrier, default_construct) +{ + MemoryBarrier buffer; + decltype(buffer)::DirectAccess<> da(buffer); + EXPECT_EQ(42, da->number_); +} + +TEST(MemoryBarrier, default_construct_new_data_flag_is_false) +{ + MemoryBarrier buffer; + EXPECT_FALSE(buffer.new_data_available()); +} + +TEST(MemoryBarrier, initialize_new_data) +{ + MemoryBarrier buffer; + DefaultConstructable x; + x.number_ = 12; + buffer.initialize(x); + EXPECT_FALSE(buffer.new_data_available()); // should still be false + decltype(buffer)::DirectAccess<> da(buffer); + EXPECT_EQ(12, da->number_); +} + + +/* + * MemoryBarrier::DirectAccess - directly manipulate a memory barrier + */ + +TEST(DirectAccess, new_data_available_flag) +{ + MemoryBarrier buffer; + decltype(buffer)::DirectAccess<> da(buffer); + EXPECT_EQ(42, da->number_); + EXPECT_FALSE(da.new_data_available()); + da.new_data_available(true); + EXPECT_TRUE(da.new_data_available()); +} + + +TEST(DirectAccess, reset_releases_resource) +{ + MemoryBarrier buffer; + decltype(buffer)::DirectAccess rt(buffer); + EXPECT_NE(rt.get(), nullptr); + EXPECT_TRUE(rt); + rt->number_ = 12; + EXPECT_EQ(12, rt->number_); + rt.reset(); + EXPECT_EQ(rt.get(), nullptr); + EXPECT_FALSE(rt); + decltype(buffer)::DirectAccess rt2(buffer); + EXPECT_NE(rt2.get(), nullptr); + EXPECT_TRUE(rt2); + EXPECT_EQ(12, rt2->number_); +} + +TEST(DirectAccess, swap_buffers) +{ + MemoryBarrier buffer; + decltype(buffer)::DirectAccess nrt(buffer); + nrt->number_ = 24; + nrt.reset(); // required or RT request below will cause a deadlock + + decltype(buffer)::DirectAccess rt(buffer); + rt->number_ = 12; + rt.swap(); + EXPECT_EQ(24, rt->number_); + rt.reset(); // required to prevent deadlock again + + // now check non-realtime + decltype(buffer)::DirectAccess nrt2(buffer); + EXPECT_EQ(12, nrt2->number_); +} + +TEST(DirectAccess, realtime_fails_on_locked_resource) +{ + MemoryBarrier buffer; + decltype(buffer)::DirectAccess nrt(buffer); + + decltype(buffer)::DirectAccess rt(buffer); + EXPECT_EQ(rt.get(), nullptr); + EXPECT_FALSE(rt); +} + + +/* + * Read Barriers + */ + +TEST(ReadBarrier, default_construct) +{ + ReadBarrier buffer; + decltype(buffer)::MemoryBarrierType::DirectAccess<> da(buffer); + EXPECT_EQ(42, da->number_); +} + +TEST(ReadBarrier, write_from_RT_to_nRT) +{ + // write state to non-RT + ReadBarrier buffer; + decltype(buffer)::MemoryBarrierType::DirectAccess<> da_writer(buffer); + // typeof(state) is MemoryBarrier::DirectAccess + da_writer->number_ = 8; // using smart_ptr-like semantics + da_writer.new_data_available(true); // indicate to non-RT there is new state data + da_writer.reset(); // unlock the state barrier +} + +TEST(ReadBarrier, read_on_nRT) +{ + // write state to non-RT + ReadBarrier buffer; + + // repeat RT write + decltype(buffer)::MemoryBarrierType::DirectAccess<> da_writer(buffer); + // typeof(state) is MemoryBarrier::DirectAccess + da_writer->number_ = 8; // using smart_ptr-like semantics + da_writer.new_data_available(true); // indicate to non-RT there is new state data + da_writer.reset(); // unlock the state barrier + + // now test the read from nRT thread + // get the data from RT thread and write into our user + DefaultConstructable state; + EXPECT_TRUE(buffer.memory().new_data_available()); + EXPECT_TRUE(buffer.pull(state)); + EXPECT_EQ(8, state.number_); +} + + +/* + * Write Barriers + */ + +TEST(WriteBarrier, default_construct) +{ + WriteBarrier buffer; + decltype(buffer)::MemoryBarrierType::DirectAccess<> da(buffer.memory()); + EXPECT_EQ(42, da->number_); +} + +TEST(WriteBarrier, write_to_RT_from_nRT) +{ + WriteBarrier buffer; + + DefaultConstructable command; + command.number_ = 52; + + // write commands to RT thread + EXPECT_FALSE(buffer.memory().new_data_available()); + EXPECT_TRUE(buffer.push(command)); + EXPECT_TRUE(buffer.memory().new_data_available()); +} + +TEST(WriteBarrier, read_on_RT) +{ + WriteBarrier buffer; + + DefaultConstructable command; + command.number_ = 52; + + // write commands to RT thread + EXPECT_TRUE(buffer.push(command)); + + // confirm read from RT + decltype(buffer)::MemoryBarrierType::DirectAccess<> da_reader(buffer); + EXPECT_TRUE(da_reader.new_data_available()); + EXPECT_EQ(da_reader->number_, 52); // using smart_ptr-like semantics +} From 601f8a797899300a99762266b55b21794a7c3b4e Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Mon, 5 Apr 2021 00:38:31 -0400 Subject: [PATCH 02/13] cpplint fixes --- include/realtime_tools/realtime_barrier.hpp | 109 +++++++++++++------- test/realtime_barrier_tests.cpp | 14 ++- 2 files changed, 76 insertions(+), 47 deletions(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index c12239eb..db52d038 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -1,9 +1,43 @@ -// -// Created by guru on 2/22/21. -// - -#ifndef REALTIME_TOOLS__REALTIME_BARRIER_H -#define REALTIME_TOOLS__REALTIME_BARRIER_H +/* + * Copyright (c) 2021 Colin F. MacKenzie, Inc. + * All rights reserved. + * + * 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 OWNER 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. + */ + +/* + * Publishing ROS messages is difficult, as the publish function is + * not realtime safe. This class provides the proper locking so that + * you can call publish in realtime and a separate (non-realtime) + * thread will ensure that the message gets published over ROS. + * + * Author: Colin F. MacKenzie + */ + +#ifndef REALTIME_TOOLS__REALTIME_BARRIER_HPP_ +#define REALTIME_TOOLS__REALTIME_BARRIER_HPP_ #include #include @@ -12,7 +46,7 @@ namespace realtime_tools { -///@brief Attempt a lock on a resource but fail if already locked +/// @brief Attempt a lock on a resource but fail if already locked class try_lock : public std::unique_lock { public: @@ -21,7 +55,7 @@ class try_lock : public std::unique_lock {} }; -///@brief Lock a resource or wait for it to be free +/// @brief Lock a resource or wait for it to be free class wait_lock : public std::unique_lock { public: @@ -35,20 +69,20 @@ class wait_lock : public std::unique_lock } }; -///@brief default options for realtime acess to resources +/// @brief default options for realtime acess to resources struct realtime { using lock_strategy = try_lock; }; -///@brief default options for non-realtime access to resources +/// @brief default options for non-realtime access to resources struct non_realtime { using lock_strategy = wait_lock; }; -///@brief Implements the underlying mechanisms for communicating between threads. +/// @brief Implements the underlying mechanisms for communicating between threads. /// MemoryBarrier implements a double buffer swapping mechanism for interchanging data between /// threads. It allocates two T objects on the heap. When MemoryBarrier::swap() is called the two /// pointers are swapped. Generally one pointer is accessed from the non-realtime thread and the @@ -56,7 +90,7 @@ struct non_realtime /// possible. For two-way communication two separate MemoryBarriers can be used as a pair. /// /// You should not use this class directly, you will work with ReadBarrier, WriteBarrier and -// DirectAccess classes only. +/// DirectAccess classes only. /// /// General strategy: /// For non-RT writing to RT thread, non-RT thread writes but doesnt swap, RT swaps and reads. @@ -65,7 +99,7 @@ template class MemoryBarrier { public: - ///@brief Provides direct access to data + /// @brief Provides direct access to data /// Should only be used from within the realtime thread. Default template args specify /// DirectAccess should only try to lock the resource and on failure the DirectAccess object /// would be equal to nullptr and therefore evaluate to false. @@ -81,22 +115,21 @@ class MemoryBarrier explicit DirectAccess(MemoryBarrierType & mem_barrier) noexcept : lock_strategy(mem_barrier.mutex_), mem_(&mem_barrier), obj_(nullptr) { - obj_ = _get(); // will return nullptr if we dont own the lock yet + obj_ = _get(); // will return nullptr if we dont own the lock yet } template explicit DirectAccess(X & mem_barrier) noexcept : lock_strategy(mem_barrier.memory().mutex_), mem_(&mem_barrier.memory()), obj_(nullptr) { - - obj_ = _get(); // will return nullptr if we dont own the lock yet + obj_ = _get(); // will return nullptr if we dont own the lock yet } template explicit DirectAccess(X * mem_barrier) noexcept : lock_strategy(mem_barrier->memory().mutex_), mem_(&mem_barrier->memory()), obj_(nullptr) { - obj_ = _get(); // will return nullptr if we dont own the lock yet + obj_ = _get(); // will return nullptr if we dont own the lock yet } @@ -230,7 +263,6 @@ class MemoryBarrier obj_ = mem_->nrt_ : nullptr; } - }; @@ -254,9 +286,6 @@ class MemoryBarrier move.new_data_available_ = false; } - /*MemoryBarrier(const MemoryBarrier& copy) - : in_(new T(*copy.in_)), out_(new T(*copy.out_)), polarity_(copy.polarity_), new_data_available_(copy.new_data_available_) {} }*/ - // allow moving but not copying MemoryBarrier(const MemoryBarrier &) = delete; @@ -264,7 +293,8 @@ class MemoryBarrier // todo: perhaps we need a new_data on both nrt and rt side - // true if new data is available, depends on if the memory is being used for Read from RT or Write to RT mode + // true if new data is available, depends on if the memory is being used for Read + // from RT or Write to RT mode inline bool new_data_available() const {return new_data_available_;} @@ -301,7 +331,7 @@ class MemoryBarrier friend class DirectAccess; }; -///@brief Create a barrier for reading data from a realtime thread +/// @brief Create a barrier for reading data from a realtime thread /// ReadBarrier implements reading data from a realtime thread using a MemoryBarrier. For example, /// this is used to transfer state data out of hardware interfaces. The default constructor will // create a new memory barrier for use. There is an alternate constructor if you want to create @@ -311,7 +341,6 @@ class ReadBarrier { public: using MemoryBarrierType = MemoryBarrier; - //using DirectAccessType = typename T::DirectAccess<>; ReadBarrier() noexcept : mem_(new MemoryBarrierType()), owns_mem(true) @@ -330,14 +359,17 @@ class ReadBarrier } } - //inline operator MemoryBarrier&() { return *mem_; } - - ///@brief Access the underlying MemoryBarrier object + /// @brief Access the underlying MemoryBarrier object inline MemoryBarrierType & memory() {return *mem_;} - ///@brief Get current value from non-realtime side without affecting the barrier. - /// Copy the current data into dest. No swap is performed and the new_data_available flag is unaffected. + /// @brief Returns true if new data is available to read + inline bool new_data_available() const + {return mem_->new_data_available();} + + /// @brief Get current value from non-realtime side without affecting the barrier. + /// Copy the current data into dest. No swap is performed and the new_data_available flag is + /// unaffected. bool current(T & dest) { typename MemoryBarrierType::template DirectAccess direct(*mem_); @@ -349,7 +381,7 @@ class ReadBarrier } } - ///@brief Read data out of the realtime thread + /// @brief Read data out of the realtime thread /// Swap RT buffer for non-RT. Copy the new data into val and reset the new_data_available flag. // todo: since this read also swaps, should it be called read_and_swap() or pop() or pull() bool pull(T & dest) @@ -361,7 +393,7 @@ class ReadBarrier dest = *direct; return true; } else { - return false; // failed to read + return false; // failed to read } } @@ -371,7 +403,7 @@ class ReadBarrier }; -///@brief Create a barrier for writing data to a realtime thread +/// @brief Create a barrier for writing data to a realtime thread /// WriteBarrier implements writing data to a realtime thread from a non-realtime thread using a /// MemoryBarrier. For example, this is used to transfer commands to a hardware interface. The /// default constructor will create a new memory barrier for use. There is an alternate @@ -381,23 +413,22 @@ class WriteBarrier { public: using MemoryBarrierType = MemoryBarrier; - //using DirectAccessType = DirectAccess; WriteBarrier() noexcept : mem_(new MemoryBarrierType()) { } - WriteBarrier(MemoryBarrierType & mem_barrier) noexcept + explicit WriteBarrier(MemoryBarrierType & mem_barrier) noexcept : mem_(&mem_barrier) { } - ///@brief Access the underlying MemoryBarrier object + /// @brief Access the underlying MemoryBarrier object inline MemoryBarrierType & memory() {return *mem_;} - ///@brief Get current value from non-realtime side without affecting the barrier. + /// @brief Get current value from non-realtime side without affecting the barrier. /// Copy the current data into dest. No swap is performed and the new_data_available flag /// is unaffected. bool current(T & dest) @@ -411,7 +442,7 @@ class WriteBarrier } } - ///@brief Write data into the realtime thread + /// @brief Write data into the realtime thread /// Copy the new data from val into non-RT buffer then swap RT buffer for non-RT and set the // new_data_available flag. bool push(const T & data) @@ -431,7 +462,7 @@ class WriteBarrier MemoryBarrierType * mem_; }; -} +} // namespace realtime_tools -#endif // REALTIME_TOOLS__REALTIME_BARRIER_H +#endif // REALTIME_TOOLS__REALTIME_BARRIER_HPP_ diff --git a/test/realtime_barrier_tests.cpp b/test/realtime_barrier_tests.cpp index 77c7a8d0..34fb38f6 100644 --- a/test/realtime_barrier_tests.cpp +++ b/test/realtime_barrier_tests.cpp @@ -149,10 +149,9 @@ TEST(ReadBarrier, write_from_RT_to_nRT) // write state to non-RT ReadBarrier buffer; decltype(buffer)::MemoryBarrierType::DirectAccess<> da_writer(buffer); - // typeof(state) is MemoryBarrier::DirectAccess - da_writer->number_ = 8; // using smart_ptr-like semantics - da_writer.new_data_available(true); // indicate to non-RT there is new state data - da_writer.reset(); // unlock the state barrier + da_writer->number_ = 8; // using smart_ptr-like semantics + da_writer.new_data_available(true); // indicate to non-RT there is new state data + da_writer.reset(); // unlock the state barrier } TEST(ReadBarrier, read_on_nRT) @@ -162,10 +161,9 @@ TEST(ReadBarrier, read_on_nRT) // repeat RT write decltype(buffer)::MemoryBarrierType::DirectAccess<> da_writer(buffer); - // typeof(state) is MemoryBarrier::DirectAccess - da_writer->number_ = 8; // using smart_ptr-like semantics - da_writer.new_data_available(true); // indicate to non-RT there is new state data - da_writer.reset(); // unlock the state barrier + da_writer->number_ = 8; // using smart_ptr-like semantics + da_writer.new_data_available(true); // indicate to non-RT there is new state data + da_writer.reset(); // unlock the state barrier // now test the read from nRT thread // get the data from RT thread and write into our user From 5b7f60b5bbf26a5f6ea669c0dc7fdac23768c3fe Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Thu, 6 May 2021 11:25:59 -0400 Subject: [PATCH 03/13] updated copyright only --- include/realtime_tools/realtime_barrier.hpp | 2 +- test/realtime_barrier_tests.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index db52d038..bde70114 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2021 Colin F. MacKenzie, Inc. + * Copyright (c) 2021 FlyingEinstein.com * All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/test/realtime_barrier_tests.cpp b/test/realtime_barrier_tests.cpp index 34fb38f6..83ae80b2 100644 --- a/test/realtime_barrier_tests.cpp +++ b/test/realtime_barrier_tests.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019, Open Source Robotics Foundation, Inc. + * Copyright (c) 2021, FlyingEinstein.com * All rights reserved. * * Redistribution and use in source and binary forms, with or without From a93f8ebb1b2fb39238c8ded62dde1456337bef12 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Wed, 16 Jun 2021 23:23:29 -0400 Subject: [PATCH 04/13] typo fix in comments Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index bde70114..f959470d 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -69,7 +69,7 @@ class wait_lock : public std::unique_lock } }; -/// @brief default options for realtime acess to resources +/// @brief Default options for realtime access to resources struct realtime { using lock_strategy = try_lock; From 4cc9e9af3eac62f1b92069d0d52d25eb929d0a0e Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Wed, 16 Jun 2021 23:23:45 -0400 Subject: [PATCH 05/13] capitalization Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index f959470d..55a80d8e 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -75,7 +75,7 @@ struct realtime using lock_strategy = try_lock; }; -/// @brief default options for non-realtime access to resources +/// @brief Default options for non-realtime access to resources struct non_realtime { using lock_strategy = wait_lock; From 57e790d05b6b128314321a03559bf271243f5012 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Wed, 16 Jun 2021 23:24:08 -0400 Subject: [PATCH 06/13] doc text fix Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index 55a80d8e..8a7b1df5 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -444,7 +444,7 @@ class WriteBarrier /// @brief Write data into the realtime thread /// Copy the new data from val into non-RT buffer then swap RT buffer for non-RT and set the - // new_data_available flag. + /// new_data_available flag. bool push(const T & data) { typename MemoryBarrierType::template DirectAccess direct(*mem_); From 2c7e723dff0784c0de913990ada77a162ac18b9c Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Wed, 16 Jun 2021 23:31:02 -0400 Subject: [PATCH 07/13] conjunction injunction Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index 8a7b1df5..f856fad2 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -93,7 +93,7 @@ struct non_realtime /// DirectAccess classes only. /// /// General strategy: -/// For non-RT writing to RT thread, non-RT thread writes but doesnt swap, RT swaps and reads. +/// For non-RT writing to RT thread, non-RT thread writes but doesn't swap, RT swaps and reads. /// For RT writing back to non-RT, RT writes but doesnt swap, NRT swaps and reads at will template class MemoryBarrier From b2e653166056a2e52fe6c8e01afb4727c492f68e Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Wed, 16 Jun 2021 23:31:51 -0400 Subject: [PATCH 08/13] conjunction injunction and grammar Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index f856fad2..6720a136 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -94,7 +94,7 @@ struct non_realtime /// /// General strategy: /// For non-RT writing to RT thread, non-RT thread writes but doesn't swap, RT swaps and reads. -/// For RT writing back to non-RT, RT writes but doesnt swap, NRT swaps and reads at will +/// For RT writing to non-RT, RT thread writes but doesn't swap, non-RT swaps and reads. template class MemoryBarrier { From 11ea68ee4915dd1d40579d5d35cbad1d29617ada Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Wed, 16 Jun 2021 23:33:42 -0400 Subject: [PATCH 09/13] conjunction injunction Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index 6720a136..d6f6ce7f 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -115,7 +115,7 @@ class MemoryBarrier explicit DirectAccess(MemoryBarrierType & mem_barrier) noexcept : lock_strategy(mem_barrier.mutex_), mem_(&mem_barrier), obj_(nullptr) { - obj_ = _get(); // will return nullptr if we dont own the lock yet + obj_ = _get(); // Will return nullptr if we don't own the lock yet } template From 3895ee992cc1b18a82301c97e9caec252f7301b7 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Wed, 16 Jun 2021 23:59:01 -0400 Subject: [PATCH 10/13] grammar Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index d6f6ce7f..d5530946 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -169,7 +169,7 @@ class MemoryBarrier inline void new_data_available(bool avail) { if (!lock_strategy::owns_lock()) { - throw std::runtime_error("can't modify an unlocked MemoryBarrier"); + throw std::runtime_error("Can't modify an unlocked MemoryBarrier"); } mem_->new_data_available_ = avail; } From bbb9135f7669175046a48aa885b200d079335785 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Wed, 16 Jun 2021 23:59:36 -0400 Subject: [PATCH 11/13] grammar Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index d5530946..a3854022 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -177,7 +177,7 @@ class MemoryBarrier inline void swap() { if (!std::unique_lock::owns_lock()) { - throw std::runtime_error("can't swap an unlocked MemoryBarrier"); + throw std::runtime_error("Can't swap an unlocked MemoryBarrier"); } mem_->swap(); obj_ = nullptr; // must explicitly set to null so _get doesnt return cached value From 1ee36ec8e7e4079117501b7580fcdec5c76cac1f Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Thu, 17 Jun 2021 00:07:18 -0400 Subject: [PATCH 12/13] grammar Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index a3854022..e6da5577 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -180,7 +180,7 @@ class MemoryBarrier throw std::runtime_error("Can't swap an unlocked MemoryBarrier"); } mem_->swap(); - obj_ = nullptr; // must explicitly set to null so _get doesnt return cached value + obj_ = nullptr; // Must explicitly set to null so _get() doesn't return cached value obj_ = _get(); } From 5607dfcb7d6ecbe7948fa99a8e29fa53b7c786b5 Mon Sep 17 00:00:00 2001 From: Colin MacKenzie Date: Thu, 17 Jun 2021 00:12:30 -0400 Subject: [PATCH 13/13] fix to not-null operator Co-authored-by: Matt Reynolds --- include/realtime_tools/realtime_barrier.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realtime_tools/realtime_barrier.hpp b/include/realtime_tools/realtime_barrier.hpp index e6da5577..84f4cda2 100644 --- a/include/realtime_tools/realtime_barrier.hpp +++ b/include/realtime_tools/realtime_barrier.hpp @@ -188,7 +188,7 @@ class MemoryBarrier {return mem_ != nullptr && get();} inline bool operator==(std::nullptr_t) const - {return mem_ == nullptr && get();} + {return mem_ == nullptr || get() == nullptr;} inline bool operator!=(std::nullptr_t) const {return mem_ != nullptr && get();}