Obstacles added through service call (/apply_planning_scene) not reflected in PlanningSceneMonitor

edit

Setup:

std::unique_ptr<robot_model_loader::RobotModelLoader> ur10_model_loader = std::make_unique<robot_model_loader::RobotModelLoader>("robot_description");
robot_model::RobotModelPtr ur10_kinematic_model = ur10_model_loader -> getModel();

planning_scene_monitor::PlanningSceneMonitorPtr ur10_planning_scene_monitor;
ur10_planning_scene_monitor = planning_scene_monitor::PlanningSceneMonitorPtr(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));
ur10_planning_scene_monitor -> startSceneMonitor();
ur10_planning_scene_monitor -> startWorldGeometryMonitor();
ur10_planning_scene_monitor -> startStateMonitor();
ur10_planning_pipeline = std::make_shared<planning_pipeline::PlanningPipeline>
                         (planning_pipeline::PlanningPipeline(ur10_kinematic_model, nodeHandle, "planning_plugin", "request_adapters"));
robot_model::JointModelGroup* manipulator_joint_model_group = ur10_kinematic_model -> getJointModelGroup(MANIPULATOR_JOINT_GROUP);

I add an obstacle into the scene with the following:

moveit_msgs::PlanningScene planning_scene_msg;
{// scope
    planning_scene_monitor::LockedPlanningSceneRO planning_sceneRO (ur10_planning_scene_monitor);
    planning_sceneRO -> getPlanningSceneMsg(planning_scene_msg);
}// scope

planning_scene_msg.world.collision_objects.clear();

planning_scene_msg.world.collision_objects.push_back(robot_workspace); // where robot_workspace has been filled with obstacles
planning_scene_msg.is_diff = true;
planning_scene_msg.robot_state.is_diff = true;

ros::ServiceClient update_planning_scene_service_client = nodeHandle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
moveit_msgs::ApplyPlanningScene apply_scene_msg;
apply_scene_msg.request.scene = planning_scene_msg;
update_planning_scene_service_client.call(apply_scene_msg)

I can verify that the obstacle is visible in rviz under "Scene Geometry".

I invoke the planner as follows

    planning_scene_monitor::LockedPlanningSceneRO lscene(ur10_planning_scene_monitor);
    ur10_planning_pipeline -> generatePlan(lscene, motion_request, motion_response);

However, the generated trajectory just passes through the obstacle.

----------------------------------- UPDATE 1 ------------------------------------------

After much digging and referring to moveit/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp, changing

ur10_planning_pipeline -> generatePlan(lscene, motion_request, motion_response);

to

ur10_planning_pipeline -> generatePlan(lscene->diff(planning_scene_msg), motion_request, motion_response);

solves the problem. However, I don't believe that is the correct way to fix this.

Further digging revealed that the lscene from

planning_scene_monitor::LockedPlanningSceneRO lscene(ur10_planning_scene_monitor);

does not get updated with the obstacle, i.e.

    moveit_msgs::PlanningScene tmp;
    lscene->getPlanningSceneMsg(tmp);
    ROS_INFO("collision objects: %ld", tmp.world.collision_objects.size());

Still gives 0 obstacles.

Shouldn't the PlanningSceneMonitor be automatically updated when the /apply_planning_scene service is called?

Using

ur10_planning_scene_monitor->monitorDiffs(true)

or

ur10_planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE);

or

ur10_planning_scene_monitor->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, "/move_group/monitored_planning_scene");

or

ur10_planning_scene_monitor -> startSceneMonitor("/move_group/monitored_planning_scene");

does not solve the problem.