demo_graspEventdetect.py 文件源码

def main(limb_name, reset):
    """
    Parameters
    ----------
    limb : str
        Which limb to use. Choices are {'left', 'right'}
    reset : bool
        Whether to use previously saved picking and placing poses or
        to save new ones by using 0g mode and the OK cuff buttons.
    """
    # Initialise ros node
    rospy.init_node("pick_and_place", anonymous=False)
    #pick_pose = []
    #place_pose = []
    #for ii in range(3):
    if reset or not rospy.has_param('~pick_and_place_poses'):
        rospy.loginfo(
            'Saving picking pose for %s limb' % limb_name)
        pick_pose = limb_pose(limb_name)
        rospy.sleep(1)
        place_pose = limb_pose(limb_name)
        rospy.set_param('~pick_and_place_poses',
                        {'pick': pick_pose.tolist(),
                         'place': place_pose.tolist()})
        #rospy.loginfo('pick_pose is %s' % pick_pose)
        #rospy.loginfo('place_pose is %s' % place_pose)
    else:
        pick_pose = rospy.get_param('~pick_and_place_poses/pick')
        place_pose =  rospy.get_param('~pick_and_place_poses/place')

    b = Baxter(limb_name)

    c1 = Controller()

    f = FilterValues()
    f.start_recording()
    #for ii in range(3):
    b.pick(pick_pose, controller=c1)
    b.place(place_pose)

    c1.save_centeringerr()

    f.stop_recording()
    f.convertandsave() #convert to numpy and save the recoreded data
    f.filter()
    f.plot()