From 9411b30516184e465c67ad9a9d6f70f83a14d65e Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Wed, 2 Apr 2025 14:51:16 +0900 Subject: [PATCH 1/7] Add method to iterate bodies without copy --- include/openrave/environment.h | 7 +++++++ src/libopenrave-core/environment-core.h | 24 ++++++++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/include/openrave/environment.h b/include/openrave/environment.h index f9e837d7b8..0836ba43f8 100644 --- a/include/openrave/environment.h +++ b/include/openrave/environment.h @@ -562,6 +562,13 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this& bodies, uint64_t timeout=0) const = 0; + /// \brief Apply a function to every body in the environment + /// + /// This method allows for iterating over all of the bodies in the env without having to copy the list of bodies first. + /// The environment mutex must be held when calling this function. + /// The callback function must not cause any bodies to be added or removed from the environment, as this would mutate the internal list of bodies while it is being iterated. + virtual void IterateBodies(const std::function& mapFunction) = 0; + /// \brief Fill an array with all robots loaded in the environment. [multi-thread safe] /// /// A separate **interface mutex** is locked for reading the bodies. diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h index 68ede7b0e8..05239dcb30 100644 --- a/src/libopenrave-core/environment-core.h +++ b/src/libopenrave-core/environment-core.h @@ -26,6 +26,7 @@ #include #endif +#include #include #include #include @@ -1639,6 +1640,22 @@ class Environment : public EnvironmentBase return _mutexEnvironment; } + virtual void IterateBodies(const std::function& mapFunction) override + { + // Add one to the active iterator count, and release our read when exiting. + // This reader count allows us to catch abuses of the iterator API (mutation during iteration) that might otherwise cause undefined behaviour. + _vecbodiesActiveReaders.fetch_add(1, std::memory_order::memory_order_acq_rel); + std::shared_ptr __deferReleaseRead{ + nullptr, [&](nullptr_t) { _vecbodiesActiveReaders.fetch_sub(1, std::memory_order::memory_order_acq_rel); }}; + + // Map the provided function over all of the live bodies in the environment + for (const KinBodyPtr& pBody : _vecbodies) { + if (!!pBody) { + mapFunction(pBody); + } + } + } + virtual void GetBodies(std::vector& bodies, uint64_t timeout) const override { TimedSharedLock lock853(_mutexInterfaces, timeout); @@ -3512,6 +3529,7 @@ class Environment : public EnvironmentBase /// assumes environment and _mutexInterfaces are exclusively locked KinBodyPtr _InvalidateKinBodyFromEnvBodyIndex(int bodyIndex) { + OPENRAVE_ASSERT_OP(_vecbodiesActiveReaders.load(std::memory_order::memory_order_acquire), ==, 0); // We are modifying vecbodies, assert there are no concurrent reads KinBodyPtr& pbodyref = _vecbodies.at(bodyIndex); if (!pbodyref) { return KinBodyPtr(); @@ -4021,6 +4039,7 @@ class Environment : public EnvironmentBase /// assuming _mutexInterfaces is exclusively locked inline void _AddKinBodyInternal(KinBodyPtr pbody, int envBodyIndex) { + OPENRAVE_ASSERT_OP(_vecbodiesActiveReaders.load(std::memory_order::memory_order_acquire), ==, 0); // We are modifying vecbodies, assert there are no concurrent reads EnsureVectorSize(_vecbodies, envBodyIndex+1); _vecbodies.at(envBodyIndex) = pbody; @@ -4488,6 +4507,11 @@ class Environment : public EnvironmentBase std::vector _vecbodies; ///< all objects that are collidable (includes robots) sorted by env body index ascending order. Note that some element can be nullptr, and size of _vecbodies should be kept unchanged when body is removed from env. protected by _mutexInterfaces. [0] should always be kept null since 0 means no assignment. + /// Number of active loops that are directly iterating the contents of _vecbodies + /// Any functions that might mutate the contents of _vecbodies should test this value, and throw if it is non-zero + /// This prevents accidental mutate-during-iterate if a caller performs an e.g. Add of a body during IterateBodies. + std::atomic _vecbodiesActiveReaders{0}; + string_map _mapBodyNameIndex; /// maps body name to env body index of bodies stored in _vecbodies sorted by name. used to lookup kin body by name. protected by _mutexInterfaces. string_map _mapBodyIdIndex; /// maps body id to env body index of bodies stored in _vecbodies sorted by name. used to lookup kin body by name. protected by _mutexInterfaces From b3bdc26c954860cfb3249b9083b22725608228fa Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Tue, 29 Apr 2025 13:46:20 +0900 Subject: [PATCH 2/7] Use struct for scoping not ptr --- src/libopenrave-core/environment-core.h | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h index 05239dcb30..f16a73a838 100644 --- a/src/libopenrave-core/environment-core.h +++ b/src/libopenrave-core/environment-core.h @@ -1645,8 +1645,18 @@ class Environment : public EnvironmentBase // Add one to the active iterator count, and release our read when exiting. // This reader count allows us to catch abuses of the iterator API (mutation during iteration) that might otherwise cause undefined behaviour. _vecbodiesActiveReaders.fetch_add(1, std::memory_order::memory_order_acq_rel); - std::shared_ptr __deferReleaseRead{ - nullptr, [&](nullptr_t) { _vecbodiesActiveReaders.fetch_sub(1, std::memory_order::memory_order_acq_rel); }}; + struct ReaderReleaser + { + ReaderReleaser(std::atomic& counter) + : _counter(counter){}; + ~ReaderReleaser() + { + _counter.fetch_sub(1, std::memory_order::memory_order_acq_rel); + } + + private: + std::atomic& _counter; + } __deferReleaseRead{_vecbodiesActiveReaders}; // Map the provided function over all of the live bodies in the environment for (const KinBodyPtr& pBody : _vecbodies) { From 49fa609587ca2259b150d72fe3c5d3c64434884e Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Thu, 22 May 2025 09:57:01 +0900 Subject: [PATCH 3/7] Take locks internally, add filter --- include/openrave/environment.h | 13 +++++-- src/libopenrave-core/environment-core.h | 45 ++++++++++++------------- 2 files changed, 32 insertions(+), 26 deletions(-) diff --git a/include/openrave/environment.h b/include/openrave/environment.h index 87a879e9fb..75a38ebee3 100644 --- a/include/openrave/environment.h +++ b/include/openrave/environment.h @@ -587,13 +587,20 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this& bodies, const std::function& filterFunction, uint64_t timeout = 0) const = 0; - /// \brief Apply a function to every body in the environment + /// \brief Apply a function to every body in the environment. Thread-safe. /// /// This method allows for iterating over all of the bodies in the env without having to copy the list of bodies first. - /// The environment mutex must be held when calling this function. - /// The callback function must not cause any bodies to be added or removed from the environment, as this would mutate the internal list of bodies while it is being iterated. + /// The environment interface mutex is locked internally. + /// The callback function must not call any methods that would cause bodies to be added or removed from the environment, as this would mutate the internal list of bodies while it is being iterated. virtual void IterateBodies(const std::function& mapFunction) = 0; + /// \brief Remove bodies from the environment based on some unary predicate. Thread-safe. + /// + /// Applies the predicate function to every body in the environment, and then removes all bodies for which the predicate returns true. + /// Note that removal of bodies happens concurrently with body iteration. + /// The environment interface mutex is locked internally. + virtual void FilterBodies(const std::function& predicate) = 0; + /// \brief Fill an array with all robots loaded in the environment. [multi-thread safe] /// /// A separate **interface mutex** is locked for reading the bodies. diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h index 5aa01b7847..b061310b26 100644 --- a/src/libopenrave-core/environment-core.h +++ b/src/libopenrave-core/environment-core.h @@ -26,7 +26,6 @@ #include #endif -#include #include #include #include @@ -1649,21 +1648,10 @@ class Environment : public EnvironmentBase virtual void IterateBodies(const std::function& mapFunction) override { - // Add one to the active iterator count, and release our read when exiting. - // This reader count allows us to catch abuses of the iterator API (mutation during iteration) that might otherwise cause undefined behaviour. - _vecbodiesActiveReaders.fetch_add(1, std::memory_order::memory_order_acq_rel); - struct ReaderReleaser - { - ReaderReleaser(std::atomic& counter) - : _counter(counter){}; - ~ReaderReleaser() - { - _counter.fetch_sub(1, std::memory_order::memory_order_acq_rel); - } - - private: - std::atomic& _counter; - } __deferReleaseRead{_vecbodiesActiveReaders}; + // Lock the interface mutex before incrementing the reader count + // Only need a shared lock since we disallow mutation during iteration + EnvironmentLock lockenv(GetMutex()); + SharedLock lockIterateBodies(_mutexInterfaces); // Map the provided function over all of the live bodies in the environment for (const KinBodyPtr& pBody : _vecbodies) { @@ -1673,6 +1661,24 @@ class Environment : public EnvironmentBase } } + virtual void FilterBodies(const std::function& predicate) override + { + // Iterate the bodies in the environment, and remove all bodies for which the predicate returns true + EnvironmentLock lockenv(GetMutex()); + ExclusiveLock lockFilterBodies(_mutexInterfaces); // Need exclusive lock here since we may be modifying _vecbodies + for (const KinBodyPtr& pBody : _vecbodies) { + // Ignore body indices that are empty + if (!pBody) { + continue; + } + + // If the predicate matches the body, invalidate it. + if (predicate(pBody)) { + _InvalidateKinBodyFromEnvBodyIndex(pBody->GetEnvironmentBodyIndex()); + } + } + } + virtual void GetBodies(std::vector& bodies, uint64_t timeout) const override { TimedSharedLock lock853(_mutexInterfaces, timeout); @@ -3555,7 +3561,6 @@ class Environment : public EnvironmentBase /// assumes environment and _mutexInterfaces are exclusively locked KinBodyPtr _InvalidateKinBodyFromEnvBodyIndex(int bodyIndex) { - OPENRAVE_ASSERT_OP(_vecbodiesActiveReaders.load(std::memory_order::memory_order_acquire), ==, 0); // We are modifying vecbodies, assert there are no concurrent reads KinBodyPtr& pbodyref = _vecbodies.at(bodyIndex); if (!pbodyref) { return KinBodyPtr(); @@ -4074,7 +4079,6 @@ class Environment : public EnvironmentBase /// assuming _mutexInterfaces is exclusively locked inline void _AddKinBodyInternal(KinBodyPtr pbody, int envBodyIndex) { - OPENRAVE_ASSERT_OP(_vecbodiesActiveReaders.load(std::memory_order::memory_order_acquire), ==, 0); // We are modifying vecbodies, assert there are no concurrent reads EnsureVectorSize(_vecbodies, envBodyIndex+1); _vecbodies.at(envBodyIndex) = pbody; @@ -4554,11 +4558,6 @@ class Environment : public EnvironmentBase std::vector _vecbodies; ///< all objects that are collidable (includes robots) sorted by env body index ascending order. Note that some element can be nullptr, and size of _vecbodies should be kept unchanged when body is removed from env. protected by _mutexInterfaces. [0] should always be kept null since 0 means no assignment. - /// Number of active loops that are directly iterating the contents of _vecbodies - /// Any functions that might mutate the contents of _vecbodies should test this value, and throw if it is non-zero - /// This prevents accidental mutate-during-iterate if a caller performs an e.g. Add of a body during IterateBodies. - std::atomic _vecbodiesActiveReaders{0}; - string_map _mapBodyNameIndex; /// maps body name to env body index of bodies stored in _vecbodies sorted by name. used to lookup kin body by name. protected by _mutexInterfaces. string_map _mapBodyIdIndex; /// maps body id to env body index of bodies stored in _vecbodies sorted by name. used to lookup kin body by name. protected by _mutexInterfaces From 1b90d9c8235aa8e3d72d6842ac97b39ff4770d31 Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Thu, 22 May 2025 09:59:14 +0900 Subject: [PATCH 4/7] Add lock warnings --- include/openrave/environment.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/openrave/environment.h b/include/openrave/environment.h index 75a38ebee3..8093367799 100644 --- a/include/openrave/environment.h +++ b/include/openrave/environment.h @@ -591,14 +591,16 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this& mapFunction) = 0; /// \brief Remove bodies from the environment based on some unary predicate. Thread-safe. /// /// Applies the predicate function to every body in the environment, and then removes all bodies for which the predicate returns true. /// Note that removal of bodies happens concurrently with body iteration. - /// The environment interface mutex is locked internally. + /// The environment interface mutex is locked internally in exclusive mode, + /// so the predicate must not make any calls that would also attempt to lock this mutex. virtual void FilterBodies(const std::function& predicate) = 0; /// \brief Fill an array with all robots loaded in the environment. [multi-thread safe] From b4698fd738f7d5fb9752f381b0d2c67d46dae870 Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Fri, 23 May 2025 09:04:19 +0900 Subject: [PATCH 5/7] Dereference --- include/openrave/environment.h | 4 ++-- src/libopenrave-core/environment-core.h | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/openrave/environment.h b/include/openrave/environment.h index 8093367799..facda77138 100644 --- a/include/openrave/environment.h +++ b/include/openrave/environment.h @@ -593,7 +593,7 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this& mapFunction) = 0; + virtual void IterateBodies(const std::function& mapFunction) = 0; /// \brief Remove bodies from the environment based on some unary predicate. Thread-safe. /// @@ -601,7 +601,7 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this& predicate) = 0; + virtual void FilterBodies(const std::function& predicate) = 0; /// \brief Fill an array with all robots loaded in the environment. [multi-thread safe] /// diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h index b061310b26..a33eeec117 100644 --- a/src/libopenrave-core/environment-core.h +++ b/src/libopenrave-core/environment-core.h @@ -1646,7 +1646,7 @@ class Environment : public EnvironmentBase return _mutexEnvironment; } - virtual void IterateBodies(const std::function& mapFunction) override + virtual void IterateBodies(const std::function& mapFunction) override { // Lock the interface mutex before incrementing the reader count // Only need a shared lock since we disallow mutation during iteration @@ -1656,12 +1656,12 @@ class Environment : public EnvironmentBase // Map the provided function over all of the live bodies in the environment for (const KinBodyPtr& pBody : _vecbodies) { if (!!pBody) { - mapFunction(pBody); + mapFunction(*pBody); } } } - virtual void FilterBodies(const std::function& predicate) override + virtual void FilterBodies(const std::function& predicate) override { // Iterate the bodies in the environment, and remove all bodies for which the predicate returns true EnvironmentLock lockenv(GetMutex()); @@ -1673,7 +1673,7 @@ class Environment : public EnvironmentBase } // If the predicate matches the body, invalidate it. - if (predicate(pBody)) { + if (predicate(*pBody)) { _InvalidateKinBodyFromEnvBodyIndex(pBody->GetEnvironmentBodyIndex()); } } From faa9b1ea20a352515ee9d923fb1bb6362756b90e Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Tue, 10 Jun 2025 15:36:34 +0900 Subject: [PATCH 6/7] Rename --- include/openrave/environment.h | 2 +- src/libopenrave-core/environment-core.h | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/openrave/environment.h b/include/openrave/environment.h index facda77138..bd65d4bcfd 100644 --- a/include/openrave/environment.h +++ b/include/openrave/environment.h @@ -601,7 +601,7 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this& predicate) = 0; + virtual void RemoveBodiesIf(const std::function& predicate) = 0; /// \brief Fill an array with all robots loaded in the environment. [multi-thread safe] /// diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h index a33eeec117..6b8fefd93a 100644 --- a/src/libopenrave-core/environment-core.h +++ b/src/libopenrave-core/environment-core.h @@ -1648,8 +1648,7 @@ class Environment : public EnvironmentBase virtual void IterateBodies(const std::function& mapFunction) override { - // Lock the interface mutex before incrementing the reader count - // Only need a shared lock since we disallow mutation during iteration + // Ensure we take both the environment and interface mutexes before iterating EnvironmentLock lockenv(GetMutex()); SharedLock lockIterateBodies(_mutexInterfaces); @@ -1661,11 +1660,11 @@ class Environment : public EnvironmentBase } } - virtual void FilterBodies(const std::function& predicate) override + virtual void RemoveBodiesIf(const std::function& predicate) override { // Iterate the bodies in the environment, and remove all bodies for which the predicate returns true EnvironmentLock lockenv(GetMutex()); - ExclusiveLock lockFilterBodies(_mutexInterfaces); // Need exclusive lock here since we may be modifying _vecbodies + ExclusiveLock lockRemoveBodies(_mutexInterfaces); // Need exclusive lock here since we may be modifying _vecbodies for (const KinBodyPtr& pBody : _vecbodies) { // Ignore body indices that are empty if (!pBody) { From 6c59bdc1ca9b8eef9d0db563cffa57ba017d2f46 Mon Sep 17 00:00:00 2001 From: Ross Schlaikjer Date: Thu, 17 Jul 2025 13:17:47 +0900 Subject: [PATCH 7/7] Expose pointer in Iterate to allow capture --- include/openrave/environment.h | 4 +++- src/libopenrave-core/environment-core.h | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/include/openrave/environment.h b/include/openrave/environment.h index bd65d4bcfd..2e8eac97f4 100644 --- a/include/openrave/environment.h +++ b/include/openrave/environment.h @@ -590,10 +590,12 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this& mapFunction) = 0; + virtual void IterateBodies(const std::function& mapFunction) = 0; /// \brief Remove bodies from the environment based on some unary predicate. Thread-safe. /// diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h index 6b8fefd93a..de5fc79031 100644 --- a/src/libopenrave-core/environment-core.h +++ b/src/libopenrave-core/environment-core.h @@ -1646,7 +1646,7 @@ class Environment : public EnvironmentBase return _mutexEnvironment; } - virtual void IterateBodies(const std::function& mapFunction) override + virtual void IterateBodies(const std::function& mapFunction) override { // Ensure we take both the environment and interface mutexes before iterating EnvironmentLock lockenv(GetMutex()); @@ -1655,7 +1655,7 @@ class Environment : public EnvironmentBase // Map the provided function over all of the live bodies in the environment for (const KinBodyPtr& pBody : _vecbodies) { if (!!pBody) { - mapFunction(*pBody); + mapFunction(pBody); } } }