Add method to iterate bodies without copy

include/openrave/environment.h

@@ -587,6 +587,22 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this<Envir
     /// \throw openrave_exception with ORE_Timeout error code
     virtual void GetBodiesMatchingFilter(std::vector<KinBodyPtr>& bodies, const std::function<bool(const KinBody&)>& filterFunction, uint64_t timeout = 0) const = 0;
 
+    /// \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 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 cause a deadlock attempting to exclusively lock the interface mutex while the thread already holds it in shared mode.
+    virtual void IterateBodies(const std::function<void(KinBody&)>& 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 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<bool(const KinBody&)>& predicate) = 0;
+
     /// \brief Fill an array with all robots loaded in the environment. <b>[multi-thread safe]</b>
     ///
     /// A separate **interface mutex** is locked for reading the bodies.

src/libopenrave-core/environment-core.h

@@ -1646,6 +1646,39 @@ class Environment : public EnvironmentBase
         return _mutexEnvironment;
     }
 
+    virtual void IterateBodies(const std::function<void(KinBody&)>& mapFunction) override
+    {
+        // 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) {
+            if (!!pBody) {
+                mapFunction(*pBody);
+            }
+        }
+    }
+
+    virtual void FilterBodies(const std::function<bool(const KinBody&)>& 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<KinBodyPtr>& bodies, uint64_t timeout) const override
     {
         TimedSharedLock lock853(_mutexInterfaces, timeout);