Added support for baked collision checking

[mkoval]

No description provided.

[mkoval]

Replace these runtime_errors with openrave_exception.

[mkoval]

There was a logic error before where stop() was being called when returning from InitPlan. This reset() the baked KinBody pointer, which caused a SEGFAULT in Plan(). I don't see why stop() is necessary at all, so I removed it entirely.

@cdellin: What was the motivation behind adding stop()? Is it safe to proceed without it?

[cdellin]

The pointer to the baked kinbody must be removed before the planner returns, since it owns raw pointers into internal collision checker datastructures which are not guaranteed to persist past the end of the planning call (from what I remember). That was the point of the boost scoped exit logic. I don't understand why resetting an empty smart pointer would cause a SEGFAULT, so I'm not convinced the proposed change actually solves the problem, and I'm afraid removing the stop method introduces other bugs as well. If this is working, I'm ok with keeping it temporarily, but let's definitely not merge until we take a closer look. I will have some cycles tonight.

[mkoval]

A "planning call" consists of InitPlan followed Plan. The old logic deleted the baked KinBody when InitPlan returns, then attempted to use the KinBody in Plan to perform collision checks during planning. This SEGFAULTs because the baked KinBody no longer exists.

I don't understand how this could create memory issues because (as far as I can tell) we don't allocate anything that needs cleanup. If cleanup is necessary, than the baking implementation in PrPy is also broken.

[cdellin]

Then the problem is that the old code didn't correctly call start() in PlanPath. The PrPy implementation relies on the baking object going out of scope in Python, and the baked kinbody therefore getting destructed.

[mkoval]

For another point of comparison, personalrobotics/comps#9 only reset() the pointers: (1) if an error occurs in InitPath or (2) after planning finishes in PlanPath. This is better because it doesn't SEGFAULT in normal operation, but is still dangerous because of personalrobotics/comps#10.

Then the problem is that the old code didn't correctly call start() in PlanPath.

If this is truly necessary, then I am fine with restoring stop() and moving both start() and stop() to PlanPath.

The PrPy implementation relies on the baking object going out of scope in Python, and the baked kinbody therefore getting destructed.

This Python garbage collector is deterministic and may not destruct the KinBody when it leaves scope. If this is indeed necessary, then we must explicitly del the object. It is probably best to do this using a context manager or an explicit try-finally block.

---

I still don't understand why all of this is necessary. What data structure could possibly go out of scope when the planner returns? We don't allocate any data structures in the planner!

[cdellin]

Yeah, looks like the start() call in PlanPath was accidentally removed in a merge in the past couple weeks. Let me try to narrow down when the offending merge was. (See

or_ompl/src/OMPLPlanner.cpp

Line 348 in 00e53f3

// start validity checker

for the original code.)

[cdellin]

The planner constructs a baked kinbody object which internally points into the collision checker. If after the planner returns, the collision checker is removed from the environment and destructed, then that kinbody must no longer be used (and actually, the non-destructed kinbody may prevent the collision checker from being destroyed properly).

I agree that we should use a context manager to handle baking in PrPy to ensure that the kinbody never outlives the collision checker.

[cdellin]

I also can see that the comps code is incorrect -- I can fix it when.I get in front of a laptop again (-:.

[mkoval]

I looked at the implementation of BakedKinBody and don't see the issue. It does store raw BroadPhaseCollisionManager *, but it also stores a BroadPhaseCollisionManagerPtr smart pointer to each of those objects. Everything else uses smart pointers.

If after the planner returns, the collision checker is removed from the environment and destructed, then that kinbody must no longer be used (and actually, the non-destructed kinbody may prevent the collision checker from being destroyed properly).

Even if I am wrong about the ownership semantics, this is only a problem if the KinBody outlives the collision checker. My changes tie the lifetime of the baked KinBody to the lifetime of the Planner.

Two potential fixes:

1. Modify the baked KinBody hold a shared_ptr to the collision checker.
2. Modify the Planner to hold a shared_ptr to the collision checker.

Both of those changes guarantee that the KinBody will be destructed before the collision checker without introducing this (obviously very error-prone) manual resource management.

[cdellin]

I agree your proposed fixes will address the immediate issue, but I really don't like the way they assert ownership over the collision checker. This will create unexpected behavior when the user attempts to destroy the collision checker, but it is not destructed because some random planner still holds a pointer that it's not even using. I will put together a better solution to this tonight.

[cdellin]

Yup, bug was introduced by a merge I made on July 21 (d99d6e7).

[cdellin]

I just applied the quick fix to address that merge problem from last week, so it certainly shouldn't segfault any longer (I just tested with some log files). I'll implement better logic later tonight.

[mkoval]

This needlessly bakes the collision checker twice.

[cdellin]

This is delicate -- what if the user has changed the environment between calls to InitPlan and PlanPath? For example, what if they want to use different padding/models for validating starts/goals than for doing the actual planning (not sure if anyone actually does this, but they could). I currently get around this by baking in each call to a function, since I can rely on the environment lock.

[mkoval]

@cdellin Any update on that new logic? 😁

[cdellin]

No update yest (-: although I expect that it would look like an RAII-style handler that accomplishes the same thing that the BOOST_SCOPE_EXIT stuff does now.

[mkoval]

This is sufficiently un-busted that I am merging the current version. I created an issue #103 to track the minor double-baked issue.

[cdellin]

I'm happy this got merged (and thanks for making the separate issue for the double-baked thing). @mkoval I'm still curious about my comment here: c475b3b. What do you think?