Revision history [back]

Every time that you add or modifice a collision object (by publishing the topic), you need "refresh" the environment with set_planning_scene:

arm_navigation_msgs::SetPlanningSceneDiff::Request set_planning_scene_diff_req;
arm_navigation_msgs::SetPlanningSceneDiff::Response set_planning_scene_diff_res;

ros::Duration(0.5).sleep();
if(publisher == attached_object_pub) {
    publisher.publish(msg_obj);
}
else {
    publisher.publish(msg_obj.object);
}
ros::Duration(0.5).sleep();

//Actualiza el environtment
if(!set_planning_scene_diff.call(set_planning_scene_diff_req, set_planning_scene_diff_res)) {
   ROS_ERROR("Can't set planning scene");
}