def reset_attached_objects(self):
'''
Removes all collision objects that are currently attached to the robot.
'''
#and all attached objects
reset_attached_objects = AttachedCollisionObject()
reset_attached_objects.link_name = 'all'
reset_attached_objects.object.header.frame_id = 'base_link'
reset_attached_objects.object.header.stamp = rospy.Time.now()
reset_attached_objects.object = self._get_reset_object()
self._publish(reset_attached_objects, self._attached_object_pub)
rospy.logdebug('Attached object reset')
def reset_attached_objects(self):
'''
Removes all collision objects that are currently attached to the robot.
'''
#and all attached objects
reset_attached_objects = AttachedCollisionObject()
reset_attached_objects.link_name = 'all'
reset_attached_objects.object.header.frame_id = 'base_link'
reset_attached_objects.object.header.stamp = rospy.Time.now()
reset_attached_objects.object = self._get_reset_object()
self._publish(reset_attached_objects, self._attached_object_pub)
rospy.logdebug('Attached object reset')
def detach_object(self, arm_name, object_id):
'''
Detaches a single object from the arm and removes it from the collision space entirely.
The object must have been attached in the world or planning scene diff previously. This removes the object
from the planning scene even if it currently exists in the world.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
**Returns:**
False if the object was not previously attached in the world or the diff and therefore cannot be
detached; True otherwise.
'''
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
#did the object ever exist in the world?
#if so, we need to continually remove it after detaching
#but it will always be in
#corresponding collision object if it exists
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
#if the planning scene originally added this object, we could
#just remove it from the list of collision objects but we have
#no way of knowing that at this point
co.operation.operation = co.operation.REMOVE
for p in range(len(co.poses)):
co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p])
co.header.frame_id = self.world_frame
break
for ao in self.current_diff.planning_scene_diff.attached_collision_objects:
if ao.object.id == object_id:
rospy.loginfo('Object was attached by the planning scene interface')
self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao)
self.send_diff()
return True
rospy.loginfo('Object was not attached by the planning scene interface')
aos = self.attached_collision_objects()
for ao in aos:
if ao.object.id == object_id:
obj.object = ao.object
obj.object.operation.operation = obj.object.operation.REMOVE
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
self.send_diff()
return True
rospy.logwarn('Object '+object_id+' not attached to gripper')
return False
def detach_object(self, arm_name, object_id):
'''
Detaches a single object from the arm and removes it from the collision space entirely.
The object must have been attached in the world or planning scene diff previously. This removes the object
from the planning scene even if it currently exists in the world.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') from which to detach the object
**object_id (string):** The ID of the object to detach
**Returns:**
False if the object was not previously attached in the world or the diff and therefore cannot be
detached; True otherwise.
'''
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
#did the object ever exist in the world?
#if so, we need to continually remove it after detaching
#but it will always be in
#corresponding collision object if it exists
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
#if the planning scene originally added this object, we could
#just remove it from the list of collision objects but we have
#no way of knowing that at this point
co.operation.operation = co.operation.REMOVE
for p in range(len(co.poses)):
co.poses[p] = self.transform_pose(self.world_frame, co.header.frame_id, co.poses[p])
co.header.frame_id = self.world_frame
break
for ao in self.current_diff.planning_scene_diff.attached_collision_objects:
if ao.object.id == object_id:
rospy.loginfo('Object was attached by the planning scene interface')
self.current_diff.planning_scene_diff.attached_collision_objects.remove(ao)
self.send_diff()
return True
rospy.loginfo('Object was not attached by the planning scene interface')
aos = self.attached_collision_objects()
for ao in aos:
if ao.object.id == object_id:
obj.object = ao.object
obj.object.operation.operation = obj.object.operation.REMOVE
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
self.send_diff()
return True
rospy.logwarn('Object '+object_id+' not attached to gripper')
return False
def reset_collision_map(self):
#remove all objects
reset_object = CollisionObject()
reset_object.operation.operation = CollisionObjectOperation.REMOVE
reset_object.header.frame_id = "base_link"
reset_object.header.stamp = rospy.Time.now()
reset_object.id = "all"
self.object_in_map_pub.publish(reset_object)
#and all attached objects
reset_attached_objects = AttachedCollisionObject()
reset_attached_objects.link_name = "all"
reset_attached_objects.object.header.frame_id = "base_link"
reset_attached_objects.object.header.stamp = rospy.Time.now()
reset_attached_objects.object = reset_object
self.attached_object_pub.publish(reset_attached_objects)
#and clear the octomap
self.clear_octomap()
rospy.loginfo("collision objects reset")
self.object_in_map_current_id = 0.
return 1
def reset_collision_map(self):
#remove all objects
reset_object = CollisionObject()
reset_object.operation.operation = CollisionObjectOperation.REMOVE
reset_object.header.frame_id = "base_link"
reset_object.header.stamp = rospy.Time.now()
reset_object.id = "all"
self.object_in_map_pub.publish(reset_object)
#and all attached objects
reset_attached_objects = AttachedCollisionObject()
reset_attached_objects.link_name = "all"
reset_attached_objects.object.header.frame_id = "base_link"
reset_attached_objects.object.header.stamp = rospy.Time.now()
reset_attached_objects.object = reset_object
self.attached_object_pub.publish(reset_attached_objects)
#and clear the octomap
self.clear_octomap()
rospy.loginfo("collision objects reset")
self.object_in_map_current_id = 0.
return 1
def attach_object_to_gripper(self, arm_name, object_id):
'''
Attaches an object to the robot's end effector.
The object must already exist in the planning scene
(although it does not need to exist in the current state of the world). This does NOT "snap" the object to
the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect
to the robot's end effector instead of the world. Collisions will be checked between the object and the
world as the object moves, but collisions between the object and the end effector will be ignored.
The link the object is attached to and the links with which collisions are ignored are defined by the hand
description (see hand_description.py).
The object will be attached to the robot according to the current state of the robot and object in the
planning scene as maintained by this class (i.e. with any prior changes you made to the diff).
To undo this function, you can use detach_object and add_object but you must remember the position of the
object where it was originally attached! If you call detach_and_add_back_attached_object, it will add
the object back at its current location in the planning scene, NOT at the location at which it was
originally attached.
The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing
an attached object with operation ADD and a collision object with operation REMOVE.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to
**object_id (string):** The ID of the object to attach
**Returns:**
False if the object did not previously exist in the world or planning scene diff and therefore cannot be
attached; True otherwise.
'''
#Note: It is important to send the full collision object (not just the id) in the attached collision object.
#This was quite complicated to figure out the first time.
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
obj.touch_links = self.hands[arm_name].touch_links
diff_obj = None
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
rospy.loginfo('Object was added to or modified in planning scene.')
diff_obj = co
break
if not diff_obj:
rospy.loginfo('Object was not previously added to or modified in planning scene.')
diff_obj = self.collision_object(object_id)
if not diff_obj:
rospy.logerr('Cannot attach object '+object_id+'. This object does not exist')
return False
self.current_diff.planning_scene_diff.collision_objects.append(diff_obj)
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
#convert it into the frame of the hand - it remains stationary with respect to this
for p in range(len(diff_obj.poses)):
diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p])
diff_obj.header.frame_id = obj.link_name
diff_obj.operation.operation = diff_obj.operation.REMOVE
obj.object = copy.deepcopy(diff_obj)
obj.object.operation.operation = obj.object.operation.ADD
self.send_diff()
return True
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient(
"move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object', CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now() - rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2"
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
def attach_object_to_gripper(self, arm_name, object_id):
'''
Attaches an object to the robot's end effector.
The object must already exist in the planning scene
(although it does not need to exist in the current state of the world). This does NOT "snap" the object to
the end effector. Rather, now when the robot moves, the object is assumed to remain stationary with respect
to the robot's end effector instead of the world. Collisions will be checked between the object and the
world as the object moves, but collisions between the object and the end effector will be ignored.
The link the object is attached to and the links with which collisions are ignored are defined by the hand
description (see hand_description.py).
The object will be attached to the robot according to the current state of the robot and object in the
planning scene as maintained by this class (i.e. with any prior changes you made to the diff).
To undo this function, you can use detach_object and add_object but you must remember the position of the
object where it was originally attached! If you call detach_and_add_back_attached_object, it will add
the object back at its current location in the planning scene, NOT at the location at which it was
originally attached.
The planning scene doesn't support ATTACH_AND_REMOVE so we do those two operations simultaneously, passing
an attached object with operation ADD and a collision object with operation REMOVE.
**Args:**
**arm_name (string):** The arm ('left_arm' or 'right_arm') to attach the object to
**object_id (string):** The ID of the object to attach
**Returns:**
False if the object did not previously exist in the world or planning scene diff and therefore cannot be
attached; True otherwise.
'''
#Note: It is important to send the full collision object (not just the id) in the attached collision object.
#This was quite complicated to figure out the first time.
obj = AttachedCollisionObject()
obj.link_name = self.hands[arm_name].attach_link
obj.touch_links = self.hands[arm_name].touch_links
diff_obj = None
for co in self.current_diff.planning_scene_diff.collision_objects:
if co.id == object_id:
rospy.loginfo('Object was added to or modified in planning scene.')
diff_obj = co
break
if not diff_obj:
rospy.loginfo('Object was not previously added to or modified in planning scene.')
diff_obj = self.collision_object(object_id)
if not diff_obj:
rospy.logerr('Cannot attach object '+object_id+'. This object does not exist')
return False
self.current_diff.planning_scene_diff.collision_objects.append(diff_obj)
self.current_diff.planning_scene_diff.attached_collision_objects.append(obj)
#convert it into the frame of the hand - it remains stationary with respect to this
for p in range(len(diff_obj.poses)):
diff_obj.poses[p] = self.transform_pose(obj.link_name, diff_obj.header.frame_id, diff_obj.poses[p])
diff_obj.header.frame_id = obj.link_name
diff_obj.operation.operation = diff_obj.operation.REMOVE
obj.object = copy.deepcopy(diff_obj)
obj.object.operation.operation = obj.object.operation.ADD
self.send_diff()
return True
def test_add_convert_objects():
obj_pub = rospy.Publisher('collision_object',CollisionObject)
att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)
rospy.init_node('test_collision_objects')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()
obj1.header.frame_id = "base_link"
obj1.id = "obj1";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(1)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .1
obj1.shapes[0].dimensions[1] = .1
obj1.shapes[0].dimensions[2] = .75
obj1.poses = [Pose() for _ in range(1)]
obj1.poses[0].position.x = .6
obj1.poses[0].position.y = -.6
obj1.poses[0].position.z = .375
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
att_obj = AttachedCollisionObject()
att_obj.link_name = "r_gripper_palm_link"
att_obj.touch_links = ['r_gripper_palm_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link',
'r_gripper_r_finger_tip_link', 'r_gripper_l_finger_tip_link', 'r_wrist_roll_link', 'r_wrist_flex_link', 'r_forearm_link', 'r_gripper_motor_accelerometer_link']
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "r_gripper_palm_link"
obj2.id = "obj2";
obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.CYLINDER
obj2.shapes[0].dimensions = [float() for _ in range(2)]
obj2.shapes[0].dimensions[0] = .025
obj2.shapes[0].dimensions[1] = .5
obj2.poses = [Pose() for _ in range(1)]
obj2.poses[0].position.x = .12
obj2.poses[0].position.y = 0
obj2.poses[0].position.z = 0
obj2.poses[0].orientation.x = 0
obj2.poses[0].orientation.y = 0
obj2.poses[0].orientation.z = 0
obj2.poses[0].orientation.w = 1
att_obj.object = obj2
r = rospy.Rate(.1)
while(True):
obj1.header.stamp = rospy.Time.now()
obj_pub.publish(obj1)
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
def setUp(self):
self.tf = TransformListener()
self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)
att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
obj_pub = rospy.Publisher('collision_object',CollisionObject)
rospy.init_node('test_motion_execution_buffer')
#let everything settle down
rospy.sleep(5.)
obj1 = CollisionObject()
obj1.header.stamp = rospy.Time.now()-rospy.Duration(.1)
obj1.header.frame_id = "base_footprint"
obj1.id = "obj2";
obj1.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
obj1.shapes = [Shape() for _ in range(3)]
obj1.poses = [Pose() for _ in range(3)]
obj1.shapes[0].type = Shape.BOX
obj1.shapes[0].dimensions = [float() for _ in range(3)]
obj1.shapes[0].dimensions[0] = .5
obj1.shapes[0].dimensions[1] = 1.0
obj1.shapes[0].dimensions[2] = .2
obj1.poses[0].position.x = .95
obj1.poses[0].position.y = -.25
obj1.poses[0].position.z = .62
obj1.poses[0].orientation.x = 0
obj1.poses[0].orientation.y = 0
obj1.poses[0].orientation.z = 0
obj1.poses[0].orientation.w = 1
obj1.shapes[2].type = Shape.BOX
obj1.shapes[2].dimensions = [float() for _ in range(3)]
obj1.shapes[2].dimensions[0] = .5
obj1.shapes[2].dimensions[1] = .1
obj1.shapes[2].dimensions[2] = 1.0
obj1.poses[2].position.x = .95
obj1.poses[2].position.y = -.14
obj1.poses[2].position.z = 1.2
obj1.poses[2].orientation.x = 0
obj1.poses[2].orientation.y = 0
obj1.poses[2].orientation.z = 0
obj1.poses[2].orientation.w = 1
obj1.shapes[1].type = Shape.BOX
obj1.shapes[1].dimensions = [float() for _ in range(3)]
obj1.shapes[1].dimensions[0] = .5
obj1.shapes[1].dimensions[1] = .1
obj1.shapes[1].dimensions[2] = 1.0
obj1.poses[1].position.x = .95
obj1.poses[1].position.y = .12
obj1.poses[1].position.z = 1.2
obj1.poses[1].orientation.x = 0
obj1.poses[1].orientation.y = 0
obj1.poses[1].orientation.z = 0
obj1.poses[1].orientation.w = 1
obj_pub.publish(obj1)
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.object.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject();
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
att_object.object.id = "pole"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.CYLINDER
att_object.object.shapes[0].dimensions = [float() for _ in range(2)]
att_object.object.shapes[0].dimensions[0] = .02
att_object.object.shapes[0].dimensions[1] = .1
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = -.02
att_object.object.poses[0].position.y = .04
att_object.object.poses[0].position.z = 0
att_object.object.poses[0].orientation.x = 0
att_object.object.poses[0].orientation.y = 0
att_object.object.poses[0].orientation.z = 0
att_object.object.poses[0].orientation.w = 1
att_pub.publish(att_object)
rospy.sleep(5.0)
def get_obj():
attached_obj = AttachedCollisionObject()
obj = CollisionObject()
shape = Shape()
pose = Pose()
vert = Point()
verts = []
in_verts = True
in_position = True
f = open('test.txt', 'r')
for line in f:
fields = line.split(':')
fields = [fields[i].strip() for i in range(len(fields))]
# print fields
if fields[0] == "frame_id":
obj.header.frame_id = fields[1]
elif fields[0] == "id":
obj.id = fields[1]
elif fields[0] == "operation" and fields[1] != "":
obj.operation.operation = int(fields[1])
elif fields[0] == "type":
shape.type = int(fields[1])
elif fields[0] == "triangles":
array = fields[1][1:-2]
ind = array.split(',')
inds = [int(i) for i in ind]
shape.triangles = inds
elif fields[0] == "x" and in_verts:
vert = Point()
vert.x = float(fields[1])
elif fields[0] == "y" and in_verts:
vert.y = float(fields[1])
elif fields[0] == "z" and in_verts:
vert.z = float(fields[1])
verts.append(vert)
elif fields[0] == "poses":
in_verts = False
elif fields[0] == "x" and in_position:
pose.position.x = float(fields[1])
elif fields[0] == "y" and in_position:
pose.position.y = float(fields[1])
elif fields[0] == "z" and in_position:
pose.position.z = float(fields[1])
in_position = False
elif fields[0] == "x" and not in_position:
pose.orientation.x = float(fields[1])
elif fields[0] == "y" and not in_position:
pose.orientation.y = float(fields[1])
elif fields[0] == "z" and not in_position:
pose.orientation.z = float(fields[1])
elif fields[0] == "w":
pose.orientation.w = float(fields[1])
obj.id = "graspable_object_1001"
shape.vertices = verts
obj.shapes = [shape]
obj.poses = [pose]
attached_obj.object = obj
return attached_obj
