Locking of the planning scene

[AndyZe]

There are about 200 public member functions in the planning scene!

Currently we lock the entire PS for reading or writing. It's all-or-nothing. We can have any number of read-locks but only one write-lock at a time. Write-locks are bad.

We've encountered some limits with planning scene locking when trying to use it with a realtime control loop at ~250 Hz. How can we make PS locking more efficient?

[AndyZe]

Here's one way to do it.

A lot of the PS member functions don't need to be write-locked. Like this one, which we use a lot:

bool PlanningScene::isStateColliding(const moveit_msgs::msg::RobotState& state, const std::string& group, bool verbose) const

My fuzzy and humble proposal is to reduce the scope of LockedPlanningSceneRW so it doesn't affect these const member functions. Possibly this could be done by moving the non-const member functions to a struct inside PlanningScene and only locking that struct. Does that make sense or am I crazy?

[sjahr]

I think you're right with the observation that this class needs to be refactored. Is it really safe to assume that we don't need to lock all const member functions? My guess would be that during the time a function writes to data structure we need to lock it for reading as well because during the writing process this data structure is in an undefined state. Maybe it would be possible to R/W lock only the part of the planning scene that is currently written to.

[tylerjw]

It is unsafe to read data that another thread might be writing to. This is a classic case of a data race and can lead to hard-to-track-down bugs. When dealing with shared data, these two rules help you avoid data races:

1. If a mutable (non-const) reference exists, it can be the only one, and there can be no const readers.
2. There can be any number of constant readers simultaneously.

We need to have one view of the world that is constantly updated and also used for potentially time-consuming computations like collision checking on multiple threads. The current approach of read-and-write locks has these downsides:

1. It is easy to use incorrectly (create data races, deadlocks, etc.)
2. It is easy to write software that performs poorly (too much time spent waiting on locks)

To address these issues, I think the right approach is to find ways to make copying less expensive (because copies of the data can be used on multiple threads simultaneously with no synchronization).

To do this, we should investigate persistent data structures that are cheap to update and copy.

But before we do that, we should determine what the normal access patterns are and where the performance bottlenecks are and take that information into account when designing changes to the planning scene.

[tylerjw]

To comment on the original issue of cleaning up the API, though. I think this can be done separately from changing the underlying data storage by separating functions that operate on a planning scene and the storage and locking of the planning scene. Thoughts?

[AndyZe]

I think eliminating mutable returns would be helpful, like this:

collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst();

We don't know what the user is going to do with the ACM they've retrieved, so the whole PS gets locked.

If there were no mutable returns, locking might be a lot more sane.

[AndyZe]

OK.

Don't you think it's a little dangerous to give the user access to the ACM like this:

auto& acm = scene->getAllowedCollisionMatrixNonConst();

When they probably only need this:

scene->updateAllowedCollisionMatrixEntry("link_1", "link_2", true);
or
scene->clearAllowedCollisionMatrix();

[AndyZe]

It looks like we are starting to have 2 different discussions:

• How to lock the PS
• API improvements

[rhaschke]

Don't you think it's a little dangerous to give the user access to the ACM like this:

Why do you think, this is dangerous? Both, with the current as well as your newly suggested API, the user can modify the ACM as he likes. However, by introducing your new methods, the mess of API methods is increased - by adding more methods.

[tylerjw]

Mutable access to shared data results in either need to use synchronization primitives or accept race conditions.

[rhaschke]

Sure, one should use a RW lock on the planning scene before retrieving the ACM like this. Wasn't it implemented like this?

[rhaschke]

Changing API w/o understanding the underlying problem doesn't seem to be very promising to me.
Did you analyze your actual problem? Why do you observe locking issues?
Are there too many readers blocking a write-lock?
Does the exclusive write-lock take too long?

[tylerjw]

The data contained inside an AllowedCollisionMatrix is this:

  std::map<std::string, std::map<std::string, AllowedCollision::Type> > entries_;
  std::map<std::string, std::map<std::string, DecideContactFn> > allowed_contacts_;

  std::map<std::string, AllowedCollision::Type> default_entries_;
  std::map<std::string, DecideContactFn> default_allowed_contacts_;

Has it been shown that copying this data is expensive and justifies synchronization primitives (lock mutexes) to be handled by the various threads accessing the PSM?

The original complaint is about how the interface for the planning scene is complex due to these mutexes. I do not think it is fair to assume that mutexes are less computationally expensive than copying data structures.

There are optimizations we could make to how this data is stored to make copying cheaper (make the data persistent and calculate diffs). Still, I would not be surprised if the normal size of this object is small enough that copying it is considerably less expensive than locking a mutex while interacting with it.

If we must maintain the mutex we should change the interface for updating to something like this:

void update(std::function<void(collision_detection::AllowedCollisionMatrix& acm)> fn);

That way, the user writes a lambda or passes a function that changes the acm instead of handling the mutex themselves.

[rhaschke]

I generally agree with your argumentation. But, performing a 3-way merge will be costly as well and requires to log changes performed by other threads. As there might be multiple other threads operating on the same data structure, this ultimately becomes n-way merge and intractable in my opinion.

[tylerjw]

I'm suggesting, as a first pass, we reject changes that have an out-of-date previous state. It is possible to create data structures that have the feature of efficient 3-way merges, but that is a complexity I'm not ready to introduce.

[rhaschke]

My suggestion is - as I wrote before - to first analyze the encountered deadlock in more detail and then take measures to fix it. 😉

[tylerjw]

The question I think I'm trying to ask is if there is some way we could change the API that would remove the opportunity for deadlocks created by users.

[rhaschke]

That's an ambitious goal. But as pointed out above, introducing smaller but more critical data sections typically increases the risk for deadlocks.

[AndyZe]

Saw this in the Tesseract readme. It's related.

Most if not all components can be cloned (performing a deep copy). This was a critical performance decision which is different from MoveIt. MoveIt leverages the statement that all const function must be thread safe. This imposes a performance issue when it comes to motion planning requiring function to copy data. Instead Tesseract leverages the ability to clone to achieve optimal performance during motion planning and real-time execution and obstacle avoidance. It leverages cloning so process planners may allocate a cloned object per planner and thread along with allowing the object to be non const.

[tylerjw]

The words in this are confusing, but here is my best understanding of what they do. Each thread maintains its copy of the planning scene, and therefore, they don't need synchronization primitives, making the interface thread-safe. Is that your understanding?

[AndyZe]

yes

[tylerjw]

One thing they are not saying (that has been part of the above discussion) is that the clone action must be synchronized. However, if you design your system where that happens infrequently, that can give you performance benefits. I have not read the code in tesseract. There are a few ways this synchronization can work. One is to have the global one you clone a ref-counted pointer (shared_ptr) to an immutable object. Then the synchronized copy is cheap (less expensive than deep copying the internals of the object) because the act of copying is just copying the shared_ptr and incrementing the reference count. Another approach would be to use a mutex when copying and doing deep copies. One advantage of the deep copy (what I think tessaract is doing) is that you can then treat the deep copy as mutable safely after you copy it.