def setUp(self):

        rospy.init_node('test_motion_execution_buffer')
        
        self.tf = TransformListener()        

        self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
        self.traj_pub = rospy.Publisher('rosie_path',DisplayTrajectory,latch=True)
        
        rospy.wait_for_service("ompl_planning/plan_kinematic_path")
        self.motion_planning_service = rospy.ServiceProxy("ompl_planning/plan_kinematic_path", GetMotionPlan)

        obj1 = CollisionObject()
    
        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "obj1";
        obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(1)]
        obj1.shapes[0].type = Shape.CYLINDER
        obj1.shapes[0].dimensions = [float() for _ in range(2)]
        obj1.shapes[0].dimensions[0] = .1
        obj1.shapes[0].dimensions[1] = 1.5
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = .9
        obj1.poses[0].position.y = -.6
        obj1.poses[0].position.z = .75
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1
        
        self.obj_pub.publish(obj1)

        rospy.sleep(5.0)
Exemple #2
0
def add_cylinder(id, bottom, radius, height, frameid, pub):
    cylinder_object = CollisionObject()
    cylinder_object.id = id
    cylinder_object.operation.operation = CollisionObjectOperation.ADD
    cylinder_object.header.frame_id = frameid
    cylinder_object.header.stamp = rospy.Time.now()

    shp = Shape()
    shp.type = Shape.CYLINDER
    shp.dimensions = [0, 0]
    shp.dimensions[0] = radius
    shp.dimensions[1] = height

    pose = Pose()
    pose.position.x = bottom[0]
    pose.position.y = bottom[1]
    pose.position.z = bottom[2] + height/2.
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 0
    pose.orientation.w = 1
    cylinder_object.shapes.append(shp)
    cylinder_object.poses.append(pose)

    pub.publish(cylinder_object)
    def setUp(self):

        rospy.init_node('test_motion_execution_buffer')

        self.tf = TransformListener()

        self.obj_pub = rospy.Publisher('collision_object',
                                       CollisionObject,
                                       latch=True)

        self.move_arm_action_client = actionlib.SimpleActionClient(
            "move_right_arm", MoveArmAction)
        self.move_arm_action_client.wait_for_server()

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "obj1"
        obj1.operation.operation = mapping_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] = 2.0
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = .9
        obj1.poses[0].position.y = -.5
        obj1.poses[0].position.z = 1.0
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1
def add_cylinder(id, bottom, radius, height, frameid):
    cylinder_object = CollisionObject()
    cylinder_object.id = id
    cylinder_object.operation.operation = CollisionObjectOperation.ADD
    cylinder_object.header.frame_id = frameid
    cylinder_object.header.stamp = rospy.Time.now()

    shp = Shape()
    shp.type = Shape.CYLINDER
    shp.dimensions = [0, 0]
    shp.dimensions[0] = radius
    shp.dimensions[1] = height

    pose = Pose()
    pose.position.x = bottom[0]
    pose.position.y = bottom[1]
    pose.position.z = bottom[2] + height / 2.
    pose.orientation.x = 0
    pose.orientation.y = 0
    pose.orientation.z = 0
    pose.orientation.w = 1
    cylinder_object.shapes.append(shp)
    cylinder_object.poses.append(pose)

    pub.publish(cylinder_object)
    def setUp(self):

        rospy.init_node('test_motion_execution_buffer')
        
        self.tf = TransformListener()        

        self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
        
        self.move_arm_action_client = actionlib.SimpleActionClient("move_irp6p", MoveArmAction)
        self.move_arm_action_client.wait_for_server()

        obj1 = CollisionObject()
    
        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "world"
        obj1.id = "obj2";
        obj1.operation.operation = mapping_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.0
        obj1.shapes[0].dimensions[1] = 4.0
        obj1.shapes[0].dimensions[2] = 0.8
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = 1.0
        obj1.poses[0].position.y = -.5
        obj1.poses[0].position.z = 0.25
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1
        
        self.obj_pub.publish(obj1)

        rospy.sleep(1.0)
    def setUp(self):

        rospy.init_node('test_allowed_collisions')

        obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)
        att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject,latch=True)

        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)   

        clear_all_att = AttachedCollisionObject()
        clear_all_att.object.header.stamp = rospy.Time.now()
        clear_all_att.object.header.frame_id = "base_link"
        clear_all_att.link_name = "all"
        clear_all_att.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        clear_all_obj = CollisionObject()
        clear_all_obj.header.stamp = rospy.Time.now()
        clear_all_obj.header.frame_id = "base_link"
        clear_all_obj.id = "all"
        clear_all_obj.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
        
        empty_req = std_srvs.srv.EmptyRequest()

        revert_allowed_collision_service_proxy(empty_req)
Exemple #7
0
    def setUp(self):

        rospy.init_node('test_allowed_collisions')

        obj_pub = rospy.Publisher('collision_object',
                                  CollisionObject,
                                  latch=True)
        att_pub = rospy.Publisher('attached_collision_object',
                                  AttachedCollisionObject,
                                  latch=True)

        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)

        clear_all_att = AttachedCollisionObject()
        clear_all_att.object.header.stamp = rospy.Time.now()
        clear_all_att.object.header.frame_id = "base_link"
        clear_all_att.link_name = "all"
        clear_all_att.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        clear_all_obj = CollisionObject()
        clear_all_obj.header.stamp = rospy.Time.now()
        clear_all_obj.header.frame_id = "base_link"
        clear_all_obj.id = "all"
        clear_all_obj.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        empty_req = std_srvs.srv.EmptyRequest()

        revert_allowed_collision_service_proxy(empty_req)
    def tearDown(self):
        obj1 = CollisionObject()
        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "all"
        obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        self.obj_pub.publish(obj1)
    def tearDown(self):
        obj1 = CollisionObject()
        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "world"
        obj1.id = "all";
        obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        self.obj_pub.publish(obj1)
Exemple #10
0
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()
Exemple #12
0
    def test_add_convert_objects(self):

        rospy.init_node('test_collision_objects')

        obj_pub = rospy.Publisher('collision_object',
                                  CollisionObject,
                                  latch=True)
        att_pub = rospy.Publisher('attached_collision_object',
                                  AttachedCollisionObject,
                                  latch=True)

        global get_collision_objects_service_name
        full_name = default_prefix + '/' + get_collision_objects_service_name
        rospy.wait_for_service(full_name)
        get_collision_objects_service = rospy.ServiceProxy(
            full_name, arm_navigation_msgs.srv.GetCollisionObjects)

        clear_all_att = AttachedCollisionObject()
        clear_all_att.object.header.stamp = rospy.Time.now()
        clear_all_att.object.header.frame_id = "base_link"
        clear_all_att.link_name = "all"
        clear_all_att.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        clear_all_obj = CollisionObject()
        clear_all_obj.header.stamp = rospy.Time.now()
        clear_all_obj.header.frame_id = "base_link"
        clear_all_obj.id = "all"
        clear_all_obj.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        #tripling for publish wonkitude

        att_pub.publish(clear_all_att)

        obj_pub.publish(clear_all_obj)

        rospy.sleep(2.)

        get_collision_objects_req = arm_navigation_msgs.srv.GetCollisionObjectsRequest(
        )
        try:
            get_collision_objects_res = get_collision_objects_service(
                get_collision_objects_req)
        except rospy.ServiceException, e:
            print "Service call failed: %s" % e
Exemple #13
0
    def setUp(self):

        rospy.init_node('test_get_base_state_validity')

        self.tf = TransformListener()

        self.obj_pub = rospy.Publisher('collision_object',
                                       CollisionObject,
                                       latch=True)

        rospy.wait_for_service(default_prefix + '/get_robot_state')
        self.get_robot_state_server = rospy.ServiceProxy(
            default_prefix + '/get_robot_state', GetRobotState)

        rospy.wait_for_service(default_prefix + '/get_state_validity')
        self.get_state_validity_server = rospy.ServiceProxy(
            default_prefix + '/get_state_validity', GetStateValidity)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = 'odom_combined'
        obj1.id = 'table'
        obj1.operation.operation = mapping_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.0
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .1
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = 4.25
        obj1.poses[0].position.y = 0
        obj1.poses[0].position.z = .75
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        self.obj_pub.publish(obj1)

        rospy.sleep(5.)
    def setUp(self):

        rospy.init_node('test_motion_execution_buffer')

        self.tf = TransformListener()

        self.obj_pub = rospy.Publisher('collision_object',
                                       CollisionObject,
                                       latch=True)
        self.traj_pub = rospy.Publisher('rosie_path',
                                        DisplayTrajectory,
                                        latch=True)

        rospy.wait_for_service("ompl_planning/plan_kinematic_path")
        self.motion_planning_service = rospy.ServiceProxy(
            "ompl_planning/plan_kinematic_path", GetMotionPlan)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "obj1"
        obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(1)]
        obj1.shapes[0].type = Shape.CYLINDER
        obj1.shapes[0].dimensions = [float() for _ in range(2)]
        obj1.shapes[0].dimensions[0] = .1
        obj1.shapes[0].dimensions[1] = 1.5
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = .9
        obj1.poses[0].position.y = -.6
        obj1.poses[0].position.z = .75
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        self.obj_pub.publish(obj1)

        rospy.sleep(5.0)
    def setUp(self):

        rospy.init_node('test_get_base_state_validity')

        self.tf = TransformListener()
        
        self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)

        rospy.wait_for_service(default_prefix+'/get_robot_state')
        self.get_robot_state_server = rospy.ServiceProxy(default_prefix+'/get_robot_state', GetRobotState)
        
        rospy.wait_for_service(default_prefix+'/get_state_validity')
        self.get_state_validity_server = rospy.ServiceProxy(default_prefix+'/get_state_validity', GetStateValidity)
        
        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = 'odom_combined'
        obj1.id = 'table'
        obj1.operation.operation = mapping_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.0
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .1
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = 4.25
        obj1.poses[0].position.y = 0
        obj1.poses[0].position.z = .75
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1
    
        self.obj_pub.publish(obj1)
        
        rospy.sleep(5.)
Exemple #16
0
    def setUp(self):

        rospy.init_node('test_allowed_collision_near_start')

        self.tf = TransformListener()

        self.obj_pub = rospy.Publisher('collision_object',
                                       CollisionObject,
                                       latch=True)

        rospy.wait_for_service(default_prefix + '/get_robot_state')
        self.get_robot_state_server = rospy.ServiceProxy(
            default_prefix + '/get_robot_state', GetRobotState)

        rospy.wait_for_service(default_prefix + '/get_state_validity')
        self.get_state_validity_server = rospy.ServiceProxy(
            default_prefix + '/get_state_validity', GetStateValidity)

        rospy.wait_for_service(default_prefix + '/get_group_info')
        self.get_group_info_server = rospy.ServiceProxy(
            default_prefix + '/get_group_info', GetGroupInfo)

        rospy.sleep(3.)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "r_gripper_palm_link"
        obj1.id = "test_object"
        obj1.operation.operation = mapping_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] = .03
        obj1.shapes[0].dimensions[1] = .03
        obj1.shapes[0].dimensions[2] = .03
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = 0
        obj1.poses[0].position.y = 0
        obj1.poses[0].position.z = 0
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1

        self.obj_pub.publish(obj1)

        rospy.sleep(2.)

        # now we put another object in collision with the base
        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_link"
        obj2.id = "base_object"
        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] = .1
        obj2.shapes[0].dimensions[1] = .1
        obj2.shapes[0].dimensions[2] = .1
        obj2.poses = [Pose() for _ in range(1)]
        obj2.poses[0].position.x = 0
        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

        self.obj_pub.publish(obj2)

        rospy.sleep(5.)
    def setUp(self):

        self.tf = TransformListener()
        
        self.move_arm_action_client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction)

        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 = mapping_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(4)]
        obj1.poses = [Pose() for _ in range(4)]

        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[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.0
        obj1.shapes[1].dimensions[2] = .2
        obj1.poses[1].position.x = .95
        obj1.poses[1].position.y = -.25
        obj1.poses[1].position.z = .92
        obj1.poses[1].orientation.x = 0
        obj1.poses[1].orientation.y = 0
        obj1.poses[1].orientation.z = 0
        obj1.poses[1].orientation.w = 1

        obj1.shapes[2].type = Shape.BOX
        obj1.shapes[2].dimensions = [float() for _ in range(3)]
        obj1.shapes[2].dimensions[0] = .2
        obj1.shapes[2].dimensions[1] = .1
        obj1.shapes[2].dimensions[2] = .2
        obj1.poses[2].position.x = .8
        obj1.poses[2].position.y = -.5
        obj1.poses[2].position.z = .78
        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[3].type = Shape.BOX
        obj1.shapes[3].dimensions = [float() for _ in range(3)]
        obj1.shapes[3].dimensions[0] = .2
        obj1.shapes[3].dimensions[1] = .1
        obj1.shapes[3].dimensions[2] = .2
        obj1.poses[3].position.x = .8
        obj1.poses[3].position.y = .18
        obj1.poses[3].position.z = .78
        obj1.poses[3].orientation.x = 0
        obj1.poses[3].orientation.y = 0
        obj1.poses[3].orientation.z = 0
        obj1.poses[3].orientation.w = 1
        obj_pub.publish(obj1)

        rospy.sleep(5.0)
    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.0)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now() - rospy.Duration(0.1)
        obj1.header.frame_id = "base_footprint"
        obj1.id = "obj2"
        obj1.operation.operation = mapping_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] = 0.5
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = 0.2
        obj1.poses[0].position.x = 0.95
        obj1.poses[0].position.y = -0.25
        obj1.poses[0].position.z = 0.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] = 0.5
        obj1.shapes[2].dimensions[1] = 0.1
        obj1.shapes[2].dimensions[2] = 1.0
        obj1.poses[2].position.x = 0.95
        obj1.poses[2].position.y = -0.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] = 0.5
        obj1.shapes[1].dimensions[1] = 0.1
        obj1.shapes[1].dimensions[2] = 1.0
        obj1.poses[1].position.x = 0.95
        obj1.poses[1].position.y = 0.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 = mapping_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] = 0.02
        att_object.object.shapes[0].dimensions[1] = 0.1
        att_object.object.poses = [Pose() for _ in range(1)]
        att_object.object.poses[0].position.x = -0.02
        att_object.object.poses[0].position.y = 0.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 test_get_state_validity(self):

        req = GetRobotStateRequest()

        cur_state = self.get_robot_state_server.call(req)
        
        #for i in range(len(cur_state.robot_state.joint_state.name)):
        #    print cur_state.robot_state.joint_state.name[i], cur_state.robot_state.joint_state.position[i] 

        state_req = GetStateValidityRequest()
        state_req.robot_state = cur_state.robot_state

        group_req = GetGroupInfoRequest()
        group_req.group_name = 'right_arm'

        group_res = self.get_group_info_server.call(group_req)

        self.failIf(len(group_res.link_names) == 0)

        right_arm_links = group_res.link_names

        group_req.group_name = ''
        
        group_res = self.get_group_info_server.call(group_req)
        
        self.failIf(len(group_res.link_names) == 0)

        state_req.ordered_collision_operations.collision_operations = arm_navigation_msgs_utils.make_disable_allowed_collisions_with_exclusions(group_res.link_names,
                                                                                                                                                      right_arm_links)

        for i in state_req.ordered_collision_operations.collision_operations:
            print 'Disabling ', i.object1
        
        state_req.check_collisions = True

        res = self.get_state_validity_server.call(state_req)

        #should be in collision
        self.failIf(res.error_code.val == res.error_code.SUCCESS)
        self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED) 

        #should be some contacts
        self.failIf(len(res.contacts) == 0)

        for c in res.contacts:
        
            #getting everything in common frame of base_link
            contact_point = PointStamped()
            contact_point.header = c.header
            contact_point.point = c.position
            contact_point_base = self.tf.transformPoint("base_link",contact_point)

            gripper_point = PointStamped()
            gripper_point.header.stamp = c.header.stamp
            gripper_point.header.frame_id = "r_gripper_palm_link"
            gripper_point.point.x = 0
            gripper_point.point.y = 0
            gripper_point.point.z = 0
            gripper_point_base = self.tf.transformPoint("base_link", gripper_point)

            print 'x diff:', abs(gripper_point_base.point.x-contact_point_base.point.x)
            print 'y diff:', abs(gripper_point_base.point.y-contact_point_base.point.y)
            print 'z diff:', abs(gripper_point_base.point.z-contact_point_base.point.z)

            self.failIf(abs(gripper_point_base.point.x-contact_point_base.point.x) > .031)
            self.failIf(abs(gripper_point_base.point.y-contact_point_base.point.y) > .031)
            self.failIf(abs(gripper_point_base.point.z-contact_point_base.point.z) > .031)

        #now we delete obj1
        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_link"
        obj2.id = "test_object"
        obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        self.obj_pub.publish(obj2)
        
        rospy.sleep(5.)

        cur_state = self.get_robot_state_server.call(req)
        state_req.robot_state = cur_state.robot_state

        res = self.get_state_validity_server.call(state_req)

        # base shouldn't collide due to child only links
        self.failIf(res.error_code.val != res.error_code.SUCCESS)

        # now it should collide
        state_req.ordered_collision_operations.collision_operations = []

        res = self.get_state_validity_server.call(state_req)

        # base shouldn't collide due to child only links
        self.failIf(res.error_code.val == res.error_code.SUCCESS)
    def setUp(self):

        rospy.init_node('test_allowed_collision_near_start')

        self.tf = TransformListener()
        
        self.obj_pub = rospy.Publisher('collision_object',CollisionObject,latch=True)

        rospy.wait_for_service(default_prefix+'/get_robot_state')
        self.get_robot_state_server = rospy.ServiceProxy(default_prefix+'/get_robot_state', GetRobotState)
        
        rospy.wait_for_service(default_prefix+'/get_state_validity')
        self.get_state_validity_server = rospy.ServiceProxy(default_prefix+'/get_state_validity', GetStateValidity)

        rospy.wait_for_service(default_prefix+'/get_group_info')
        self.get_group_info_server = rospy.ServiceProxy(default_prefix+'/get_group_info', GetGroupInfo)
        
        rospy.sleep(3.)

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "r_gripper_palm_link"
        obj1.id = "test_object"
        obj1.operation.operation = mapping_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] = .03
        obj1.shapes[0].dimensions[1] = .03
        obj1.shapes[0].dimensions[2] = .03
        obj1.poses = [Pose() for _ in range(1)]
        obj1.poses[0].position.x = 0
        obj1.poses[0].position.y = 0
        obj1.poses[0].position.z = 0
        obj1.poses[0].orientation.x = 0
        obj1.poses[0].orientation.y = 0
        obj1.poses[0].orientation.z = 0
        obj1.poses[0].orientation.w = 1
    
        self.obj_pub.publish(obj1)

        rospy.sleep(2.)

        # now we put another object in collision with the base
        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_link"
        obj2.id = "base_object"
        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] = .1
        obj2.shapes[0].dimensions[1] = .1
        obj2.shapes[0].dimensions[2] = .1
        obj2.poses = [Pose() for _ in range(1)]
        obj2.poses[0].position.x = 0
        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

        self.obj_pub.publish(obj2)
        
        rospy.sleep(5.)
Exemple #21
0
    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 = mapping_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 = mapping_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 testAllowedNotAllowedInitialContact(self):

        #adding object in collision with base

        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_link"
        obj2.id = "base_object"
        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] = .1
        obj2.shapes[0].dimensions[1] = .1
        obj2.shapes[0].dimensions[2] = .1
        obj2.poses = [Pose() for _ in range(1)]
        obj2.poses[0].position.x = 0
        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

        self.obj_pub.publish(obj2)

        rospy.sleep(5.)

        joint_names = [
            '%s_%s' % ('r', j) for j in [
                'shoulder_pan_joint', 'shoulder_lift_joint',
                'upper_arm_roll_joint', 'elbow_flex_joint',
                'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'
            ]
        ]
        goal = MoveArmGoal()

        goal.motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]

        goal.motion_plan_request.group_name = "right_arm"
        goal.motion_plan_request.num_planning_attempts = 1
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        goal.motion_plan_request.planner_id = ""
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"

        goal.motion_plan_request.goal_constraints.joint_constraints = [
            JointConstraint() for i in range(len(joint_names))
        ]
        for i in range(len(joint_names)):
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].joint_name = joint_names[i]
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].position = 0.0
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_above = 0.08
            goal.motion_plan_request.goal_constraints.joint_constraints[
                i].tolerance_below = 0.08

        goal.motion_plan_request.goal_constraints.joint_constraints[
            0].position = -2.0
        goal.motion_plan_request.goal_constraints.joint_constraints[
            3].position = -0.2
        goal.motion_plan_request.goal_constraints.joint_constraints[
            5].position = -0.2

        self.move_arm_action_client.send_goal(goal)

        r = rospy.Rate(10)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                    and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        #should still have succeedeed
        final_state = self.move_arm_action_client.get_state()
        self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)

        # but we can still overwrite
        coll = CollisionOperation()
        coll.object1 = "base_link"
        coll.object2 = coll.COLLISION_SET_OBJECTS
        coll.operation = coll.ENABLE

        goal.motion_plan_request.ordered_collision_operations.collision_operations.append(
            coll)

        goal.motion_plan_request.goal_constraints.joint_constraints[
            0].position = 0.0
        goal.motion_plan_request.goal_constraints.joint_constraints[
            3].position = -0.2
        goal.motion_plan_request.goal_constraints.joint_constraints[
            5].position = -0.2

        self.move_arm_action_client.send_goal(goal)

        r = rospy.Rate(10)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
                    and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        #should still have succeedeed
        final_state = self.move_arm_action_client.get_state()
        self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
Exemple #23
0
    def test_get_state_validity(self):

        req = GetRobotStateRequest()

        cur_state = self.get_robot_state_server.call(req)

        #for i in range(len(cur_state.robot_state.joint_state.name)):
        #    print cur_state.robot_state.joint_state.name[i], cur_state.robot_state.joint_state.position[i]

        state_req = GetStateValidityRequest()
        state_req.robot_state = cur_state.robot_state

        group_req = GetGroupInfoRequest()
        group_req.group_name = 'right_arm'

        group_res = self.get_group_info_server.call(group_req)

        self.failIf(len(group_res.link_names) == 0)

        right_arm_links = group_res.link_names

        group_req.group_name = ''

        group_res = self.get_group_info_server.call(group_req)

        self.failIf(len(group_res.link_names) == 0)

        state_req.ordered_collision_operations.collision_operations = arm_navigation_msgs_utils.make_disable_allowed_collisions_with_exclusions(
            group_res.link_names, right_arm_links)

        for i in state_req.ordered_collision_operations.collision_operations:
            print 'Disabling ', i.object1

        state_req.check_collisions = True

        res = self.get_state_validity_server.call(state_req)

        #should be in collision
        self.failIf(res.error_code.val == res.error_code.SUCCESS)
        self.assertEqual(res.error_code.val,
                         res.error_code.COLLISION_CONSTRAINTS_VIOLATED)

        #should be some contacts
        self.failIf(len(res.contacts) == 0)

        for c in res.contacts:

            #getting everything in common frame of base_link
            contact_point = PointStamped()
            contact_point.header = c.header
            contact_point.point = c.position
            contact_point_base = self.tf.transformPoint(
                "base_link", contact_point)

            gripper_point = PointStamped()
            gripper_point.header.stamp = c.header.stamp
            gripper_point.header.frame_id = "r_gripper_palm_link"
            gripper_point.point.x = 0
            gripper_point.point.y = 0
            gripper_point.point.z = 0
            gripper_point_base = self.tf.transformPoint(
                "base_link", gripper_point)

            print 'x diff:', abs(gripper_point_base.point.x -
                                 contact_point_base.point.x)
            print 'y diff:', abs(gripper_point_base.point.y -
                                 contact_point_base.point.y)
            print 'z diff:', abs(gripper_point_base.point.z -
                                 contact_point_base.point.z)

            self.failIf(
                abs(gripper_point_base.point.x -
                    contact_point_base.point.x) > .031)
            self.failIf(
                abs(gripper_point_base.point.y -
                    contact_point_base.point.y) > .031)
            self.failIf(
                abs(gripper_point_base.point.z -
                    contact_point_base.point.z) > .031)

        #now we delete obj1
        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_link"
        obj2.id = "test_object"
        obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE

        self.obj_pub.publish(obj2)

        rospy.sleep(5.)

        cur_state = self.get_robot_state_server.call(req)
        state_req.robot_state = cur_state.robot_state

        res = self.get_state_validity_server.call(state_req)

        # base shouldn't collide due to child only links
        self.failIf(res.error_code.val != res.error_code.SUCCESS)

        # now it should collide
        state_req.ordered_collision_operations.collision_operations = []

        res = self.get_state_validity_server.call(state_req)

        # base shouldn't collide due to child only links
        self.failIf(res.error_code.val == res.error_code.SUCCESS)
Exemple #24
0
    def setUp(self):

        self.tf = TransformListener()

        self.move_arm_action_client = actionlib.SimpleActionClient(
            "move_right_arm", MoveArmAction)

        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 = mapping_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(4)]
        obj1.poses = [Pose() for _ in range(4)]

        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[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.0
        obj1.shapes[1].dimensions[2] = .2
        obj1.poses[1].position.x = .95
        obj1.poses[1].position.y = -.25
        obj1.poses[1].position.z = .92
        obj1.poses[1].orientation.x = 0
        obj1.poses[1].orientation.y = 0
        obj1.poses[1].orientation.z = 0
        obj1.poses[1].orientation.w = 1

        obj1.shapes[2].type = Shape.BOX
        obj1.shapes[2].dimensions = [float() for _ in range(3)]
        obj1.shapes[2].dimensions[0] = .2
        obj1.shapes[2].dimensions[1] = .1
        obj1.shapes[2].dimensions[2] = .2
        obj1.poses[2].position.x = .8
        obj1.poses[2].position.y = -.5
        obj1.poses[2].position.z = .78
        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[3].type = Shape.BOX
        obj1.shapes[3].dimensions = [float() for _ in range(3)]
        obj1.shapes[3].dimensions[0] = .2
        obj1.shapes[3].dimensions[1] = .1
        obj1.shapes[3].dimensions[2] = .2
        obj1.poses[3].position.x = .8
        obj1.poses[3].position.y = .18
        obj1.poses[3].position.z = .78
        obj1.poses[3].orientation.x = 0
        obj1.poses[3].orientation.y = 0
        obj1.poses[3].orientation.z = 0
        obj1.poses[3].orientation.w = 1
        obj_pub.publish(obj1)

        rospy.sleep(5.0)
    def test_object_collisions(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)   

        obj_pub = rospy.Publisher('collision_object',CollisionObject)
        att_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject)

        rospy.init_node('test_allowed_collisions')

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "obj1";
        obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(2)]
        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = 1.0
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .05
        obj1.shapes[1].type = Shape.BOX
        obj1.shapes[1].dimensions = [float() for _ in range(3)]
        obj1.shapes[1].dimensions[0] = 1.0
        obj1.shapes[1].dimensions[1] = 1.0
        obj1.shapes[1].dimensions[2] = .05
        obj1.poses = [Pose() for _ in range(2)]
        obj1.poses[0].position.x = 1.0
        obj1.poses[0].position.y = 0
        obj1.poses[0].position.z = .5
        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.poses[1].position.x = 1.0
        obj1.poses[1].position.y = 0
        obj1.poses[1].position.z = .75
        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)
        
        obj2 = CollisionObject();
        
        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_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] = 1.0
        obj2.shapes[0].dimensions[1] = 1.0
        obj2.shapes[0].dimensions[2] = .05
        obj2.poses = [Pose() for _ in range(1)]
        obj2.poses[0].position.x = .15
        obj2.poses[0].position.y = 0
        obj2.poses[0].position.z = .5
        obj2.poses[0].orientation.x = 0
        obj2.poses[0].orientation.y = 0
        obj2.poses[0].orientation.z = 0
        obj2.poses[0].orientation.w = 1
        
        obj_pub.publish(obj2)

        rospy.sleep(2.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:
            obj1_index = get_res.matrix.link_names.index("obj1")
        except ValueError:
            self.fail("No obj1 in AllowedCollisionMatrix link names")
         
        self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index)
        self.failIf(len(get_res.matrix.entries) <= obj1_index)

        self.failIf(len(get_res.matrix.entries[obj1_index].enabled) <= r_gripper_palm_link_index)
        self.failIf(len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <= obj1_index)
         
        self.assertEqual(False, get_res.matrix.entries[obj1_index].enabled[r_gripper_palm_link_index])
        self.assertEqual(False, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj1_index])
        
        #now disable between all objects

        #if enable just this link, it should be true
        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 = "r_gripper_palm_link"
        set_allowed_request.ord.collision_operations[0].object2 = CollisionOperation.COLLISION_SET_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[obj1_index].enabled[r_gripper_palm_link_index])
        self.assertEqual(True, get_res.matrix.entries[r_gripper_palm_link_index].enabled[obj1_index])
Exemple #26
0
    def test_object_collisions(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)

        obj_pub = rospy.Publisher('collision_object', CollisionObject)
        att_pub = rospy.Publisher('attached_collision_object',
                                  AttachedCollisionObject)

        rospy.init_node('test_allowed_collisions')

        obj1 = CollisionObject()

        obj1.header.stamp = rospy.Time.now()
        obj1.header.frame_id = "base_link"
        obj1.id = "obj1"
        obj1.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
        obj1.shapes = [Shape() for _ in range(2)]
        obj1.shapes[0].type = Shape.BOX
        obj1.shapes[0].dimensions = [float() for _ in range(3)]
        obj1.shapes[0].dimensions[0] = 1.0
        obj1.shapes[0].dimensions[1] = 1.0
        obj1.shapes[0].dimensions[2] = .05
        obj1.shapes[1].type = Shape.BOX
        obj1.shapes[1].dimensions = [float() for _ in range(3)]
        obj1.shapes[1].dimensions[0] = 1.0
        obj1.shapes[1].dimensions[1] = 1.0
        obj1.shapes[1].dimensions[2] = .05
        obj1.poses = [Pose() for _ in range(2)]
        obj1.poses[0].position.x = 1.0
        obj1.poses[0].position.y = 0
        obj1.poses[0].position.z = .5
        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.poses[1].position.x = 1.0
        obj1.poses[1].position.y = 0
        obj1.poses[1].position.z = .75
        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)

        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_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] = 1.0
        obj2.shapes[0].dimensions[1] = 1.0
        obj2.shapes[0].dimensions[2] = .05
        obj2.poses = [Pose() for _ in range(1)]
        obj2.poses[0].position.x = .15
        obj2.poses[0].position.y = 0
        obj2.poses[0].position.z = .5
        obj2.poses[0].orientation.x = 0
        obj2.poses[0].orientation.y = 0
        obj2.poses[0].orientation.z = 0
        obj2.poses[0].orientation.w = 1

        obj_pub.publish(obj2)

        rospy.sleep(2.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:
            obj1_index = get_res.matrix.link_names.index("obj1")
        except ValueError:
            self.fail("No obj1 in AllowedCollisionMatrix link names")

        self.failIf(len(get_res.matrix.entries) <= r_gripper_palm_link_index)
        self.failIf(len(get_res.matrix.entries) <= obj1_index)

        self.failIf(
            len(get_res.matrix.entries[obj1_index].enabled) <=
            r_gripper_palm_link_index)
        self.failIf(
            len(get_res.matrix.entries[r_gripper_palm_link_index].enabled) <=
            obj1_index)

        self.assertEqual(
            False, get_res.matrix.entries[obj1_index].
            enabled[r_gripper_palm_link_index])
        self.assertEqual(
            False, get_res.matrix.entries[r_gripper_palm_link_index].
            enabled[obj1_index])

        #now disable between all objects

        #if enable just this link, it should be true
        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 = "r_gripper_palm_link"
        set_allowed_request.ord.collision_operations[
            0].object2 = CollisionOperation.COLLISION_SET_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[obj1_index].
            enabled[r_gripper_palm_link_index])
        self.assertEqual(
            True, get_res.matrix.entries[r_gripper_palm_link_index].
            enabled[obj1_index])
    def testAllowedNotAllowedInitialContact(self):

        #adding object in collision with base

        obj2 = CollisionObject()

        obj2.header.stamp = rospy.Time.now()
        obj2.header.frame_id = "base_link"
        obj2.id = "base_object"
        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] = .1
        obj2.shapes[0].dimensions[1] = .1
        obj2.shapes[0].dimensions[2] = .1
        obj2.poses = [Pose() for _ in range(1)]
        obj2.poses[0].position.x = 0
        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

        self.obj_pub.publish(obj2)

        rospy.sleep(5.)

        joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']]
        goal = MoveArmGoal()

        goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]

        goal.motion_plan_request.group_name = "right_arm"
        goal.motion_plan_request.num_planning_attempts = 1
        goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
        goal.motion_plan_request.planner_id = ""
        goal.planner_service_name = "ompl_planning/plan_kinematic_path"

        goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))]
        for i in range(len(joint_names)):
            goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i]
            goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0
            goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08
            goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08

        goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0
        goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
        goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2

        self.move_arm_action_client.send_goal(goal)

        r = rospy.Rate(10)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
               cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        #should still have succeedeed
        final_state = self.move_arm_action_client.get_state()
        self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED)

        # but we can still overwrite
        coll = CollisionOperation()
        coll.object1 = "base_link"
        coll.object2 = coll.COLLISION_SET_OBJECTS
        coll.operation = coll.ENABLE

        goal.motion_plan_request.ordered_collision_operations.collision_operations.append(coll)

        goal.motion_plan_request.goal_constraints.joint_constraints[0].position = 0.0
        goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2
        goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2

        self.move_arm_action_client.send_goal(goal)

        r = rospy.Rate(10)

        while True:
            cur_state = self.move_arm_action_client.get_state()
            if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and
               cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
                break

        #should still have succeedeed
        final_state = self.move_arm_action_client.get_state()
        self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)