Attached Collision Object Being Ignored

edit

I have a manipulator with interchangeable end effectors. I am trying to attach the different end effectors to the end of the manipulator and add them to the collision scene so they do not collide with anything.

The AttachedCollisionObject message, including the mesh and frame information, is generated in a rospy rqt GUI and sent to an action server that does a bunch of planning and executing trajectory moves, including attaching and detaching EEFs. This is working well on the real robot, except for the collision objects. The collision objects look like they are added to the environment and attached to the robot in rViz. They show up (in transparent purple) and move with the manipulator in the correct fashion. However, the move_group does not seem to be counting it when considering collisions. Planning and executing still works as expected, but the attached object is allowed to collide with other objects in the environment (the table and manipulator, defined in the URDF). Interestingly, the rest of the collision scene is working in that no self collisions are allowed besides the attached end effector. Some screenshots:

This is the rViz output before attaching the collision model:

Immediately after attaching:

Planning so the attached tool collides with the table

This is the MoveIt! output during the planning phase while planning into (what should be) a collision

[ INFO] [1560536843.745995889]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ WARN] [1560536843.747570276]: Cannot find planning configuration for group 'manipulator' using planner 'RRTConnectkConfigDefault'. Will use defaults instead.
[ INFO] [1560536843.747920448]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1560536843.748462132]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1560536843.779068325]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1560536843.779108449]: Solution found in 0.031089 seconds
[ INFO] [1560536843.814415606]: SimpleSetup: Path simplification took 0.035256 seconds and changed from 3 to 2 states

The AttachedCollisionObject message creation:

def shovel_ee():
    shovel_pose = Pose()
    shovel_pose.orientation.w = 1

    shovel_collision = moveit_msgs.msg.AttachedCollisionObject()
    shovel_collision.object.header.frame_id = ee_frame
    shovel_collision.object.id = "Shovel"
    shovel_collision.link_name = ee_frame

    shovel_collision.object.meshes.append(create_moveit_mesh("/home/adamp/catkin_ws/src/Herbie/herbie_description/meshes/Tool_Shovel.stl"))
    shovel_collision.object.mesh_poses.append(shovel_pose)

    return shovel_collision

def attach_shovel():
    segment = herbie_interface.msg.AutonomousSegment()
    segment.segment_name = "Attach Shovel"
    segment.segment_type = segment.ATTACH_COLLISION_OBJECTS
    segment.collision_objects.append(shovel_ee())
    segment.collision_objects[0].object.operation = segment.collision_objects[0].object.ADD

    return segment

The segment.collision_objects in the attach_shovel function is sent to the action server to add it to the robot. That code is shown here:

moveit_msgs::AttachedCollisionObject attach_collision_object;
std::vector<moveit_msgs::CollisionObject> collision_objects;
moveit_msgs::PlanningScene planning_scene;
moveit_msgs::CollisionObject remove_object;

attach_collision_object = it->segment_collision_objects[0];
collision_objects.push_back(it->segment_collision_objects[0].object);
planning_scene.world.collision_objects.push_back(it->segment_collision_objects[0].object);
remove_object.id = "Shovel";
remove_object.header.frame_id = it->segment_collision_objects[0].object.id;
remove_object.operation = remove_object.REMOVE;
planning_scene.is_diff = true;
srv.request.scene = planning_scene;
planning_scene_diff_client_.call(srv);

planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene.robot_state.attached_collision_objects.push_back(attach_collision_object);
planning_scene.robot_state.is_diff = true;

srv.request ...