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])