def test_add_convert_objects():
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
rospy.init_node('test_collision_objects')
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 = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .2
obj2.shapes[0].dimensions[1] = .4
obj2.shapes[0].dimensions[2] = .2
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(6)
sent_twice = 0
while (True):
sent_twice += 1
if sent_twice >= 4 and sent_twice % 2 == 0:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
elif sent_twice > 4 and sent_twice % 2 == 1:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
print 'sending'
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
def test_add_convert_objects():
att_pub = rospy.Publisher('attached_collision_object',AttachedCollisionObject)
rospy.init_node('test_collision_objects')
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 = mapping_msgs.msg.CollisionObjectOperation.ADD
obj2.shapes = [Shape() for _ in range(1)]
obj2.shapes[0].type = Shape.BOX
obj2.shapes[0].dimensions = [float() for _ in range(3)]
obj2.shapes[0].dimensions[0] = .2
obj2.shapes[0].dimensions[1] = .4
obj2.shapes[0].dimensions[2] = .2
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(6)
sent_twice = 0
while(True):
sent_twice += 1
if sent_twice >= 4 and sent_twice%2 == 0:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT
elif sent_twice > 4 and sent_twice%2 == 1:
att_obj.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
print 'sending'
att_obj.object.header.stamp = rospy.Time.now()
att_pub.publish(att_obj)
r.sleep()
def test_attached_object_collision_operations(self):
global set_allowed_collision_service_name
global get_allowed_collision_service_name
global revert_allowed_collision_service_name
full_name = default_prefix+'/'+set_allowed_collision_service_name
rospy.wait_for_service(full_name)
set_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
full_name = default_prefix+'/'+get_allowed_collision_service_name
rospy.wait_for_service(full_name)
get_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
full_name = default_prefix+'/'+revert_allowed_collision_service_name
rospy.wait_for_service(full_name)
revert_allowed_collision_service_proxy = rospy.ServiceProxy(full_name, std_srvs.srv.Empty)
att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)
rospy.init_node('test_allowed_collisions')
att_object = AttachedCollisionObject();
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "base_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.touch_links = [str() for _ in range(1)]
att_object.touch_links[0] = "r_gripper_palm_link"
att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject();
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "base_link"
att_object.object.id = "obj3"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.BOX
att_object.object.shapes[0].dimensions = [float() for _ in range(3)]
att_object.object.shapes[0].dimensions[0] = .2
att_object.object.shapes[0].dimensions[1] = 0.3
att_object.object.shapes[0].dimensions[2] = .05
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = .15
att_object.object.poses[0].position.y = 0
att_object.object.poses[0].position.z = 1.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)
get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
get_res = get_allowed_collision_service_proxy(get_mat)
try:
r_gripper_palm_link_index = get_res.matrix.link_names.index("r_gripper_palm_link")
except ValueError:
self.fail("No r_gripper_palm_link in AllowedCollisionMatrix link names")
try:
r_gripper_r_finger_tip_link_index = get_res.matrix.link_names.index("r_gripper_r_finger_tip_link")
except ValueError:
self.fail("No r_gripper_r_finger_tip_link in AllowedCollisionMatrix link names")
try:
obj3_index = get_res.matrix.link_names.index("obj3")
except ValueError:
self.fail("No obj3 in AllowedCollisionMatrix link names")
self.assertEqual(True, get_res.matrix.entries[obj3_index].enabled[r_gripper_r_finger_tip_link_index])
self.assertEqual(True, get_res.matrix.entries[r_gripper_r_finger_tip_link_index].enabled[obj3_index])
self.assertEqual(True, get_res.matrix.entries[obj3_index].enabled[r_gripper_palm_link_index])
self.assertEqual(True, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj3_index])
#this should overwrite touch links
set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest()
set_allowed_request.ord.collision_operations = [CollisionOperation() for _ in range(1)]
set_allowed_request.ord.collision_operations[0].object1 = "obj3"
set_allowed_request.ord.collision_operations[0].object2 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[0].operation = CollisionOperation.ENABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(False, get_res.matrix.entries[obj3_index].enabled[r_gripper_palm_link_index])
self.assertEqual(False, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj3_index])
set_allowed_request.ord.collision_operations[0].object1 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[0].object2 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
set_allowed_request.ord.collision_operations[0].operation = CollisionOperation.DISABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(True, get_res.matrix.entries[obj3_index].enabled[r_gripper_palm_link_index])
self.assertEqual(True, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj3_index])
def test_attached_object_collision_operations(self):
global set_allowed_collision_service_name
global get_allowed_collision_service_name
global revert_allowed_collision_service_name
full_name = default_prefix + '/' + set_allowed_collision_service_name
rospy.wait_for_service(full_name)
set_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.SetAllowedCollisions)
full_name = default_prefix + '/' + get_allowed_collision_service_name
rospy.wait_for_service(full_name)
get_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, arm_navigation_msgs.srv.GetAllowedCollisionMatrix)
full_name = default_prefix + '/' + revert_allowed_collision_service_name
rospy.wait_for_service(full_name)
revert_allowed_collision_service_proxy = rospy.ServiceProxy(
full_name, std_srvs.srv.Empty)
att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject)
rospy.init_node('test_allowed_collisions')
att_object = AttachedCollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "base_link"
att_object.link_name = "r_gripper_r_finger_tip_link"
att_object.touch_links = [str() for _ in range(1)]
att_object.touch_links[0] = "r_gripper_palm_link"
att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
att_object.object = CollisionObject()
att_object.object.header.stamp = rospy.Time.now()
att_object.object.header.frame_id = "base_link"
att_object.object.id = "obj3"
att_object.object.shapes = [Shape() for _ in range(1)]
att_object.object.shapes[0].type = Shape.BOX
att_object.object.shapes[0].dimensions = [float() for _ in range(3)]
att_object.object.shapes[0].dimensions[0] = .2
att_object.object.shapes[0].dimensions[1] = 0.3
att_object.object.shapes[0].dimensions[2] = .05
att_object.object.poses = [Pose() for _ in range(1)]
att_object.object.poses[0].position.x = .15
att_object.object.poses[0].position.y = 0
att_object.object.poses[0].position.z = 1.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)
get_mat = arm_navigation_msgs.srv.GetAllowedCollisionMatrixRequest()
get_res = get_allowed_collision_service_proxy(get_mat)
try:
r_gripper_palm_link_index = get_res.matrix.link_names.index(
"r_gripper_palm_link")
except ValueError:
self.fail(
"No r_gripper_palm_link in AllowedCollisionMatrix link names")
try:
r_gripper_r_finger_tip_link_index = get_res.matrix.link_names.index(
"r_gripper_r_finger_tip_link")
except ValueError:
self.fail(
"No r_gripper_r_finger_tip_link in AllowedCollisionMatrix link names"
)
try:
obj3_index = get_res.matrix.link_names.index("obj3")
except ValueError:
self.fail("No obj3 in AllowedCollisionMatrix link names")
self.assertEqual(
True, get_res.matrix.entries[obj3_index].
enabled[r_gripper_r_finger_tip_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_r_finger_tip_link_index].
enabled[obj3_index])
self.assertEqual(
True, get_res.matrix.entries[obj3_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj3_index])
#this should overwrite touch links
set_allowed_request = arm_navigation_msgs.srv.SetAllowedCollisionsRequest(
)
set_allowed_request.ord.collision_operations = [
CollisionOperation() for _ in range(1)
]
set_allowed_request.ord.collision_operations[0].object1 = "obj3"
set_allowed_request.ord.collision_operations[
0].object2 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[
0].operation = CollisionOperation.ENABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(
False, get_res.matrix.entries[obj3_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
False, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj3_index])
set_allowed_request.ord.collision_operations[
0].object1 = "r_gripper_palm_link"
set_allowed_request.ord.collision_operations[
0].object2 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
set_allowed_request.ord.collision_operations[
0].operation = CollisionOperation.DISABLE
set_allowed_collision_service_proxy(set_allowed_request)
rospy.sleep(1.)
get_res = get_allowed_collision_service_proxy(get_mat)
self.assertEqual(
True, get_res.matrix.entries[obj3_index].
enabled[r_gripper_palm_link_index])
self.assertEqual(
True, get_res.matrix.entries[r_gripper_palm_link_index].
enabled[obj3_index])
