Collision Checking between collision object and object attached to the robot not detected

edit

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 ...