Crash in (PointCloud)OctomapUpdater after PlanningSceneMonitor::clearOctomap() has been called

[simonschmeisser]

Description

I am currently updating from a kinetic snapshot from 2016-07-24 to released kinetic binaries. Now I always get crashes when a new PointCloud arrives and is processed by PointCloudOctomapUpdater while TrajectoryExecutionManager::waitForExecution() is running.

Do I need to lock the PlanningSceneMonitor during trajectory execution? Should not be necessary right?

Your environment

• ROS Distro: Kinetic
• OS Version: Ubuntu 16.04
• Binary build
• If binary, which release version? 0.9.3-0xenial-20161126-065956-0800 (ros-kinetic-moveit-core)

Backtrace or Console output

Here is the backtrace of the three relevant threads, I fear it doesn't help too much
https://gist.github.com/simonschmeisser/c7f1d1be6591952d505fa05b307af982

[v4hn]

Did you rebuild your full workspace?
We broke ABI in the TrajectoryExecutionManager to add a validation feature.
See moveit/moveit.ros.org#115 (comment) for some more hints on the new release.

[simonschmeisser]

Yes, did a full rebuild (several times ;) ). I found some places in our code where the validation failed and fixed them. I can generally execute trajectories just fine. It only crashes once a update to the octomap is triggered.

[v4hn]

Hm, @rhaschke reviewed the locking code there some time ago, but I don't know whether he has time to look at this at the moment. Could you try a from-source build with debug symbols?
I don't have a setup around where I could debug this.

[simonschmeisser]

I'm trying that at the moment but getting different errors, maybe I should uninstall the binaries first

[v4hn]

Hm, if you also build ompl in that workspace, that's probably a problem, but otherwise it should work.
So if you find a bug that makes it fail, please report it :)

[davetcoleman]

Theres a lot of octomap-related PRs here: https://github.com/ros-planning/moveit_ros/pulls

[simonschmeisser]

So compiling kinetic-devel from trunk worked and it's running until the same or similar issue.

Here is the relevant part of the backtrace: https://gist.github.com/simonschmeisser/6f1c2e5d1cf91912478148a28cfcb814

I'm a bit confused if the error might be on my side. I'm locking the PlanningSceneUpdater to create a copy, then afterwards I do some collision checking on that copy while a update to the Octomap of the "main" PlanningScene arrives. Those two should not interfere (if I'm doing things right at least). Here's the relevant code

planning_scene::PlanningScenePtr origScene;
{
    //We still don't have any guarantee for being in sync here!
    //should be fixed
    //once the octomap starts being updated, it will lock the PlanningScene
    //but if we ask to soon, we might get the lock first
    ros::Duration(0.03).sleep();
    ros::Duration(1).sleep();
    mt::Profiler::ScopedBlock prof("Waiting for Octomap?");

    //TODO: RO should be sufficient
    planning_scene_monitor::LockedPlanningSceneRW ps(m_planningSceneMonitor);
    ps->getCurrentStateNonConst().update();
    origScene = ps->diff();
    origScene->decoupleParent();
}

// remove potentially attached objects
robot_state::RobotState fullState = origScene->getCurrentStateNonConst();
fullState.clearAttachedBodies();
fullState.update();
origScene->setCurrentState(fullState);

Eigen::Affine3d preApproach = m_transitPose;

robot_state::RobotState transitState = origScene->getCurrentState();
robot_state::GroupStateValidityCallbackFn constraint_fn = bind(&isStateValid, origScene.get(), _1, _2, _3);
bool transitPossible = transitState.setFromIK(transitState.getJointModelGroup("manipulator"), preApproach, m_group->lastLink(), 0, 0, constraint_fn);
std::cout << "transit point can be reached? " << (transitPossible?"yes":"no") << std::endl;

(Note: somehow actually the two sleeps above where not sufficient and we're running into the case where cloning happens before the relevant octomap/pointcloud is processed)

[simonschmeisser]

I did some further investigation and it turns out my code is running fine unless I either call

 PlanningSceneMonitor::clearOctomap()

or the following code (that looks a bit strange) that we apparently use to reset the PlanningScene/PlanningSceneMonitor entirely

planning_scene_monitor::LockedPlanningSceneRW ps(m_planningSceneMonitor);

ps->getWorldNonConst()->clearObjects();
ps->getCurrentStateNonConst().clearAttachedBodies();
moveit_msgs::PlanningScene msg;
ps->getPlanningSceneMsg(msg);
planning_scene_diff_publisher_.publish(msg);

As soon as the next PointCloud arrives, I get a segfault in PointCloudOctomapUpdater. This should be reproducible using just a MoveGroup and a (PointCloud)OctomapUpdater and doing a publish, resetOctomap, publish

https://gist.github.com/simonschmeisser/e52ad234018d4dae7d1c352de4437f88

[simonschmeisser]

There is a fix in the devel branch of octomap, just needs to be released ...

[v4hn]

Thanks for debugging this!
Let's hope this will be released into kinetic soon..
I'll leave this ticket open until it is.