Add method to iterate bodies without copy

[rschlaikjer]

For environments with large numbers of bodies, the existing pattern of GetBodies into local vector -> iterate -> clear vector results in significant overhead from creating and destroying all the extra shared pointers.

Add an API to allow mapping a function over the bodies in the environment directly, removing the need to allocate memory / increment and decrement refcounts.

As written, this implementation adds a safety check to prevent users modifying the bodies in the env while an iteration is occurring - this is pretty low-overhead, but it could be removed if we want to let users shoot themselves in the foot.

[rdiankov]

If we're going to do this, might as well have a return value for the passed in function, where user could control body removal.

[rschlaikjer]

If we're going to do this, might as well have a return value for the passed in function, where user could control body removal.

I guess would it be better to have two functions (fold analog and a filter analog) or just have filter and require people return true from it if they just wanted fold behaviour? If we split into two, then concurrent folds might be allowable (not allowed to mutate _vecbodies) whereas there can't be two filters at once.

My assumption is that callers are locking the environment before calling Iterate though, so maybe there's no need to optimize for the concurrent case?

[rdiankov]

My assumption is that callers are locking the environment before calling Iterate though, so maybe there's no need to optimize for the concurrent case?

Okay, how did you come to this assumption? No function in EnvironmentBase assumes the environment lock is locked before it is called, so it is dangerous if you all of a sudden start doing that without documenting anything(!)
Let's be more careful.

That being said, an API to access environment services without having to check the lock every time is necessary if we want to optimize even further. One idea I have been flirting with is to pass in a reference to EnvironmentLock& lock in these public functions, which will proves that the caller has the lock.

[rschlaikjer]

Okay, how did you come to this assumption? No function in EnvironmentBase assumes the environment lock is locked before it is called, so it is dangerous if you all of a sudden start doing that

This function is written such that it shouldn't require that you lock the env to call it, but again my assumption about the openrave locking model is that if you want to reliably read/write the data on a body you have acquired a pointer to, you need to hold the environment lock. Checking this, I found the following in the openrave architecture doc, emphasis added:

Because %OpenRAVE is a highly multi-threaded environment, the environment state like
bodies and loaded interfaces could be simultaneously accessed. In order to safely write or
read the state, a user has to lock the environment, which prevents any other process from [...]

I suppose that a caller could re-acquire the lock inside their mapped function for each body, meaning you could have concurrent operations, but that seems less likely than taking the lock once then iterating.

That being said, an API to access environment services without having to check the lock every time is necessary if we want to optimize even further. One idea I have been flirting with is to pass in a reference to EnvironmentLock& lock in these public functions, which will proves that the caller has the lock.

Yeah, I think that adding granularity along the lines of having both

void DoFunction(...args) {
  DECLARE_LOCK(..);
  DoFunctionLocked(...args);
}

void DoFunctionLocked(...args) { 
  // ...
}

might be useful in allowing for more fine-grained lock control. I don't have any exact use cases off the top of my head though.

[rdiankov]

This function is written such that it shouldn't require that you lock the env to call it,

All functions from EnvironmentBase are multi-thread safe at the moment, meaning that a user can call them without locking the environment.

In order to have a non-thread safe function, it needs to be super explicit since otherwise users can forget. my best solution for this is to force passing in the env lock. for example by declaring:

    virtual void IterateBodies(const std::function<BodyReturnCode(const KinBodyPtr&)>& mapFunction, EnvironmentLock& envlock) = 0;

[rschlaikjer]

Updated to take locks internal to the iteration functions.

Also added FilterBodies; you could definitely do fold as filter but it seems a bit more foolproof / easier to read if you see code that does an IterateBodies rather than code that does a FilterBodies but just happens to never remove any. This way we can also be more moderate in our locking - iterate only requires a shared lock, filter requires exclusive.

[rdiankov]

Perhaps the function pointer can take in KinBody& and allow for modification?

[rschlaikjer]

You're right, no need to expose the encapsulation here. Changed both instances.

[rschlaikjer]

Remembered why this contract includes the pointer - it's to allow using this as a way to query a subset of bodies more efficiently, e.g:

std::vector<KinBodyPtr> bodiesMatchingCriteria;
env->IterateBodies([&bodiesMatchingCriteria](const KinBodyPtr& body) { 
  if (body->MeetsCriteria()) {
    bodiesMatchingCriteria.emplace_back(body);
  }
});
// Outside of interface lock, actually process these bodies...

If you have many thousands of bodies, this is a lot more performant than copying all bodies then erasing a bunch of them again. Have reverted the contract here (and explained why in the docstring), but for FilterBodies still only take a kinbody reference.

[rdiankov]

@rschlaikjer perhaps a better name would be RemoveBodiesIf

[rschlaikjer]

Sure, updated