ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

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

asked 2020-05-18 05:37:40 -0500

Rufus gravatar image

updated 2020-05-19 03:50:57 -0500

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.

edit retag flag offensive close merge delete

Comments

This Q&A seems related: #q352656.

gvdhoorn gravatar image gvdhoorn  ( 2020-05-19 02:22:34 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-05-19 04:10:13 -0500

Rufus gravatar image

updated 2020-05-19 21:29:53 -0500

It appears that changing

ur10_planning_scene_monitor -> startSceneMonitor();

to

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

as well as adding a 2 second delay between

update_planning_scene_service_client.call(apply_scene_msg)

and

planning_scene_monitor::LockedPlanningSceneRO lscene(ur10_planning_scene_monitor);

allows me to reliably get an lscene that contains the collision object.

This seems counter to what the tutorial mentions about service call being synchronous (blocks until diff is applied). Though it may refer to blocking until the diff is applied only to move_group's PlanningSceneMonitor, and not every listening PlanningSceneMonitor

The Q&A for this behavior can be found here: https://answers.ros.org/question/3527...

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2020-05-18 05:37:40 -0500

Seen: 184 times

Last updated: May 19 '20