Can't remove added collision model

edit

I am doing manipulation with the PR2, and have trouble with removing a collision model I added earlier. I realize that this might be because I don't fully comprehend how the arm_navigation stack works.

I am adding the collision model of the picked object to the collision map, and subsequently adding it as an attached object. I have tried using the /environment_server/set_planning_scene_diff service, but this fails to even add the collision model in the first place.

So I first add it by publishing a CollisionObject message on /collision_object and then an AttachedCollsionObject message on /attached_collision_object. This works fine for attaching the object, as I can see it on the /attached_objects marker in Rviz and I can send MoveArm goals afterwards.

However, none of the following work for removal of the collision object after placing it:

In terminal:

rosservice call /collider_node/reset "{}"

In node:

reset_object = arm_nav_msg.AttachedCollisionObject()
reset_object.link_name = "all"
reset_object.object.header.stamp = rospy.Time.now()
reset_object.object.header.frame_id = "/base_link"
reset_object.object.operation.operation = reset_object.object.operation.REMOVE
reset_object.object.id = "all"
attach_pub.publish(reset_object) # Publishes to /attached_collision_object
collision_pub.publish(reset_object.object) # Publishes to /collision_object

Even though this is the only ways to do it I could come across. It is still there in Rviz, and arm navigation fails, since the gripper has moved, and is now "in contact" with the attached object (which hasn't been removed).

Obviously, restarting the arm navigation does the trick, but that shouldn't really be necessary, eh?