[For testing, do not merge] Debug Bullet self collision checks

[AndyZe]

checkSelfCollision() reports that the robot is always in collision.

Test with: roslaunch moveit_tutorials bullet_collision_checker_tutorial.launch

[j-petit]

Thanks, I'll look into it on the weekend

[AndyZe]

I believe this is a solution for this problem. Although I still have a different setup with other issues.

Add to collision_env_bullet.cpp checkSelfCollisionHelper():

manager_->setActiveCollisionObjects(active_);

[j-petit]

Can someone give me a hint what this warning means? Appears when shutting down the interactive robot.

[WARN] [1607162645.234141136] [ros.rosconsole_bridge.class_loader.ClassLoader]: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.

[rhaschke]

In the context of MoveIt, a typical reason for this warning is that you are releasing the RobotModelLoader while the corresponding RobotModel is still in use.

[j-petit]

Thanks Robert. I am having some workflow issues, maybe you (or anyone else) can help me out here too: I want to build moveit_core in the state from one-year ago to use git bisect and see if this bug was introduced somewhere along the way.

Of course I can do a git checkout one_year_old_commit in moveit_core. However, the other dependency repositories are then still in a different state which leads to a build error in moveit_core. Is there a quick way to reset all the dependency repos to a specific state in the past?

[AndyZe]

@j-petit I'm back to working on this a bit. I started visualizing the collision points always. When it fails, it seems to be that the link transforms aren't right. See pink dot here...

[AndyZe]

Although if I stop using this version:

planning_scene->checkSelfCollision(req, res);

And use this version instead...

planning_scene->getCollisionEnv()->checkSelfCollision(req, res, state);

the result is reasonable. 👍

[AndyZe]

So it seems this isn't actually a bug and I should close this PR. What do you think, @j-petit ?

[AndyZe]

Closing until somebody comments otherwise

[JafarAbdi]

@AndyZe I think this's a bug in the tutorial calling planning_scene->checkCollision(req, res); will check the collision for the current state in the planning scene (which we don't change here), to fix this we need to change this line to be a reference to the current state of the planning scene robot_state::RobotState& state = planning_scene->getCurrentStateNonConst();

[AndyZe]

I think this's a bug in the tutorial ...

Well, if you pass the RobotState as an arg (like the tutorial does) it works fine. It was just my modification that had an issue.

[j-petit]

Both of your solutions are working, but I think changing the robot_state::RobotState& state = planning_scene->getCurrentStateNonConst(); is clearer. I made a PR #568 for this, thanks Jafar for the suggestion. I am just wondering how it worked in the first place, I am pretty sure that the tutorial showed no collision when I first wrote it 🤷‍♂️ .