Hi all,

I am currently working with ROS indigo and ROS Kinetic. I have a pick and place application using a UR arm. After following different answers on ROS here, I managed to import an object (dae) in the Moveit environment an attach it to the arm C:\fakepath\1.png. After attaching the Box to the gripper, whenever I gave a new goal with the interactive marker in Moveit, the plan computation failed. The gripper was shown red when box was attached to it , meaning the gripper link of the arm was in potential collision with the box. (plz correct me If I have understood wrong).I did some digging on the ROS answers to see the problem. So considering the problem then I was able to update the collision matrix following these links. link 1, link 2, link 3. Now the problem is the box is attached to the arm and now if I give a new Goal by moving the interactive marker, the plan is always computed and executed well. But Moveit is ignoring the collision of the Box with the rest of the arm and the robot. It does not take it into account the collision with other links and parts of the robot, which means when it will compute a new plan to move to the goal, it will not take into account the collision and the box will collide with the robot / or pass through it. I am attaching the images of the scenario. C:\fakepath\2.png, C:\fakepath\3.png. The box is immersed into the geometry of the robot and the arm links.

I also get an error in the terminals when the ACM is updated. plese check the lower left terminal in the picture C:\fakepath\4.png

Now my objective is to attach the box with the robot. and then

1. Planning should be made possible after attaching
2. Move-it also takes the box/object into account and its collisions and then do the move plan.

I am also attaching the full code to enable experts to see what am I doing wrong. ( I am not an expert with ROS and Moveit). I am a newbe...and I followed the Moveit Tutorials.

int main(int argc, char **argv)
{
  ros::init (argc, argv, "add_object");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  ros::NodeHandle node_handle;
  ros::Duration sleep_time(5.0);
  sleep_time.sleep();

// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Note that this topic may need to be remapped in the launch file 

  ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
  while(planning_scene_diff_publisher.getNumSubscribers() < 1)
  {
    ros::WallDuration sleep_t(0.5);
    sleep_t.sleep();
  }

  moveit_msgs::AttachedCollisionObject attached_object;
  attached_object.link_name = "gripper_link";
  attached_object.object.header.frame_id = "gripper_link";      // The header must contain a valid TF frame
  attached_object.object.id = "box";     // The id of the object

    Vector3d scale(1, 1, 1);
      shapes::Mesh* m = shapes::createMeshFromResource("package://work/meshes/box.dae",scale); 
     ROS_INFO("box loaded");
    shape_msgs::Mesh mesh;
    shapes::ShapeMsg mesh_msg;  
    shapes::constructMsgFromShape(m, mesh_msg);
    mesh = boost::get<shape_msgs::Mesh>(mesh_msg);
    attached_object.object.meshes.resize(1);
    attached_object.object.mesh_poses.resize(1);

  attached_object.object.meshes[0]=mesh;
  //geometry_msgs::Pose target_pose1;    // A default pose
  attached_object.object.mesh_poses[0].position.x = 0.0;
  attached_object.object.mesh_poses[0].position.y = 0.0;
  attached_object.object.mesh_poses[0].position.z = -0.232; //-0.145
  attached_object.object.mesh_poses[0].orientation.w = 0.0;
  attached_object.object.mesh_poses[0].orientation.x = 0.0;
  attached_object.object.mesh_poses[0].orientation.y = 0.0;
  attached_object.object.mesh_poses[0].orientation.z = 0.0;

  attached_object.object.meshes.push_back(mesh);
  attached_object.object.mesh_poses.push_back(attached_object.object.mesh_poses[0]);

// Note that attaching an object to the robot requires 
// the corresponding operation to be specified as an ADD operation

  attached_object.object.operation = attached_object.object.ADD;


// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to 
// the set of collision objects in the "world" part of the 
// planning scene. Note that we are using only the "object" 
// field of the attached_object message here.

  ROS_INFO("Adding box to the World");
  moveit_msgs::PlanningScene planning_scene_add;
  planning_scene_add.world.collision_objects.push_back(attached_object.object);
  planning_scene_add.is_diff = true;
  planning_scene_diff_publisher.publish(planning_scene_add);
  sleep_time.sleep();

    ros::ServiceClient planning_scene_diff_client =
      node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
  planning_scene_diff_client.waitForExistence();
  // and send the diffs to the planning scene via a service call:
  moveit_msgs::ApplyPlanningScene srv;
  srv.request.scene = planning_scene_add;
  planning_scene_diff_client.call(srv);


    /* First, define the REMOVE object message*/
  moveit_msgs::CollisionObject remove_object;
  remove_object.id = "box";
  remove_object.header.frame_id = "gripper_link";
  remove_object.operation = remove_object.REMOVE;

  // Note how we make sure that the diff message contains no other
  // attached objects or collisions objects by clearing those fields
  // first.
  /* Carry out the REMOVE + ATTACH operation */
  ROS_INFO("Attaching the object to the gripper and removing it from the world.");
  planning_scene_add.world.collision_objects.clear();
  planning_scene_add.world.collision_objects.push_back(remove_object);
  planning_scene_add.robot_state.attached_collision_objects.push_back(attached_object);
  planning_scene_add.is_diff = true;
  planning_scene_add.robot_state.is_diff=true;
  planning_scene_diff_publisher.publish(planning_scene_add);

  sleep_time.sleep();



  // Detach an object from the robot
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  // Detaching an object from the robot requires two operations
  //  * Detaching the object from the robot
  //  * Re-introducing the object into the environment

        moveit_msgs::PlanningScene currentScene;
    moveit_msgs::PlanningScene newSceneDiff;

     ros::ServiceClient client_get_scene = node_handle.serviceClient<moveit_msgs::GetPlanningScene>("/get_planning_scene");
    moveit_msgs::GetPlanningScene scene_srv;
    scene_srv.request.components.components = scene_srv.request.components.ALLOWED_COLLISION_MATRIX;


        if(!client_get_scene.call(scene_srv))
    {
        ROS_WARN("Failed to call service /get_planning_scene");
    }
    else
    {
        ROS_INFO_STREAM("Initial scene!");
        currentScene = scene_srv.response.scene;

        moveit_msgs::AllowedCollisionMatrix currentACM = currentScene.allowed_collision_matrix;


        ROS_ERROR_STREAM("size of acm_entry_names before " << currentACM.entry_names.size());
        ROS_ERROR_STREAM("size of acm_entry_values before " << currentACM.entry_values.size());
        ROS_ERROR_STREAM("size of acm_entry_values[0].entries before " << currentACM.entry_values[0].enabled.size());



        currentACM.entry_names.push_back("box");
        moveit_msgs::AllowedCollisionEntry entry;
        entry.enabled.resize(currentACM.entry_names.size());

        for(int i = 0; i < entry.enabled.size(); i++)
            entry.enabled[i] = true;

        //add new row to allowed collsion matrix
        currentACM.entry_values.push_back(entry);

        for(int i = 0; i < currentACM.entry_values.size(); i++)
        {
            //extend the last column of the matrix
            currentACM.entry_values[i].enabled.push_back(true);
        }

        newSceneDiff.is_diff = true;
        newSceneDiff.allowed_collision_matrix = currentACM;

        planning_scene_diff_publisher.publish(newSceneDiff);


     }

      if(!client_get_scene.call(scene_srv))
     {
       ROS_WARN("Failed to call service /get_planning_scene");
     }
     else
     {
         ROS_INFO_STREAM("Modified scene!");
         currentScene = scene_srv.response.scene;
         moveit_msgs::AllowedCollisionMatrix currentACM = currentScene.allowed_collision_matrix;

         ROS_ERROR_STREAM("size of acm_entry_names after " << currentACM.entry_names.size());
         ROS_ERROR_STREAM("size of acm_entry_values after " << currentACM.entry_values.size());
         ROS_ERROR_STREAM("size of acm_entry_values[0].entries after " << currentACM.entry_values[0].enabled.size());
     }




  ros::shutdown();
  return 0;
}

@gvdhoorn...here is the console output of the error on the update of Allowed Collision Matrix.

Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1503916840.548132777, 783.823000000]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1503916840.548198858, 783.823000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1503916840.548223229, 783.823000000]: MoveGroup context initialization complete

You can start planning now!

[ERROR] [1503916896.587386677, 839.724000000]: Found empty JointState message
[ INFO] [1503916896.587476339, 839.724000000]: Removing world object with the same name as newly attached object: 'box'
[ INFO] [1503916896.589061357, 839.725000000]: Attached object 'box' to link 'gripper_link'
[ERROR] [1503916901.594320512, 844.724000000]: Number of entries is incorrect for link 'box' in AllowedCollisionMatrix message

I hope I have made the problem as clear and precise and possible. Help in this regards is highly appreciated.