예제 #1
0
def main(args):
    context = zmq.Context()
    socket = context.socket(zmq.PUB)
    socket.connect('tcp://{}:{}'.format(args.host, args.port))
    print("Starting camera publisher!")
    i = 0
    delay = 1.0 / 30.0
    img = Image.open(args.input_img)
    while True:
        # TODO -- chage timestamp to nanosec
        send_image(socket, img, i, '{:6f}'.format(time.time()))
        i += 1
        time.sleep(delay)
def main():
    """RSDK Joint Trajectory Example: Simple Action Client

    Creates a client of the Joint Trajectory Action Server
    to send commands of standard action type,
    control_msgs/FollowJointTrajectoryAction.

    Make sure to start the joint_trajectory_action_server.py
    first.

    Get two points from the user.

    Use the Joint Trajectory Action Server to find a path between them.
    
    Repeat the trajectory until program is stopped.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__)
    required = parser.add_argument_group("required arguments")
    required.add_argument(
        "-l", "--limb", required=True, choices=["left", "right"], help="send joint trajectory to which limb"
    )
    required.add_argument("-f", "--folder", required=True, help="path to assets/ folder containing help images")
    args = parser.parse_args(rospy.myargv()[1:])
    print args
    limb = args.limb

    print ("Initializing node... ")
    rospy.init_node("point_input_trajectory_%s" % (limb,))
    print ("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    print ("Enabling robot... ")
    rs.enable()

    traj = common.Trajectory(limb)
    rospy.on_shutdown(traj.stop)

    filenames = ["getpoint1.png", "getpoint2.png", "executing.png"]
    filenames = [args.folder + filename for filename in filenames]

    for filename in filenames:
        if not os.access(filename, os.R_OK):
            rospy.logerr("Cannot read file at '%s'" % (filename,))
            return 1

    limbInterface = baxter_interface.Limb(limb)

    common.send_image(filenames[0])
    # Get the current position
    buttonpress = common.ButtonListener()
    buttonpress.subscribe("/robot/digital_io/" + limb + "_lower_button/state")
    points = []

    while not buttonpress.pressed:
        rospy.sleep(0.5)

    points.append(buttonpress.getButtonPressTraj(limb, limbInterface, traj))
    print "Got first position:"
    print points[0]

    buttonpress.pressed = False

    common.send_image(filenames[1])

    while not buttonpress.pressed:
        rospy.sleep(0.5)

    points.append(buttonpress.getButtonPressTraj(limb, limbInterface, traj))
    print "Got second position:"
    print points[1]

    common.send_image(filenames[2])
    print ("Running. Ctrl-c to quit")

    i = 0
    while i < 1000 and not rospy.is_shutdown():
        # Make sure the points alternate
        traj.add_point(points[i % 2], 15.0)
        traj.add_point(points[(i + 1) % 2], 15.0)
        traj.start()
        traj.wait()
        traj.clear(limb)
        print "Completed test", i
        i += 1
예제 #3
0
def main():
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-l', '--limb', required=True, choices=['left', 'right'],
        help='send joint trajectory to which limb'
    )
    required.add_argument(
        '-f', '--folder', help='path to assets/ folder containing help images'
    )
    args = parser.parse_args(rospy.myargv()[1:])
    print args
    limb = args.limb

    print("Initializing node... ")
    rospy.init_node("servo_to_object_%s" % (limb,))
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    print("Enabling robot... ")
    rs.enable()

    #Calibrate gripper
    gripper_if = baxter_interface.Gripper(limb)
    if not gripper_if.calibrated():
        print "Calibrating gripper"
        gripper_if.calibrate()

    if args.folder is None:
        args.folder = "/home/jackie/ros_ws/src/baxter_demos/assets/"
    # First, get start and end configurations from the user
    filenames = ["getpoint1.png", "getpoint2.png", "executing_grasp.png"]
    filenames = [args.folder+filename for filename in filenames]
    
    for filename in filenames:
        if not os.access(filename, os.R_OK):
            rospy.logerr("Cannot read file at '%s'" % (filename,))
            return 1

    limbInterface = baxter_interface.Limb(limb)

    #rospy.on_shutdown(display)

    common.send_image(filenames[0])
    #Get the current position
    buttonpress = common.ButtonListener()
    buttonpress.subscribe("/robot/digital_io/"+limb+"_lower_button/state")
    points = []

    while not buttonpress.pressed:
        rospy.sleep(params['button_rate'])
    
    points.append( buttonpress.getButtonPress(limbInterface))
    print "Got first position:"
    print points[0]

    buttonpress.pressed = False

    common.send_image(filenames[1])

    while not buttonpress.pressed:
        rospy.sleep(params['button_rate'])
 
    points.append( buttonpress.getButtonPress(limbInterface))
    print "Got second position:"
    print points[1]

    common.send_image(filenames[2])

    # Command start configuration

    limbInterface.move_to_joint_positions(points[0])

    iksvc, ns = ik_command.connect_service(limb)

    rate = rospy.Rate(params['rate'])
    dc = DepthCaller(limb, iksvc)

    # Subscribe to estimate_depth
    # Move to pose published by estimate_depth
    while (not dc.done) and (not rospy.is_shutdown()):
        rate.sleep()

    print "Starting visual servoing"
    
    # Subscribe to object_finder and start visual servoing/grasping
    vc = VisualCommand(iksvc, limb)
    vc.subscribe()

    while (not vc.done) and (not rospy.is_shutdown()):
        rate.sleep()

    limbInterface.move_to_joint_positions(points[1])

    # Let go
    gripper_if.open()
예제 #4
0
def main():
    """RSDK Joint Trajectory Example: Simple Action Client

    Creates a client of the Joint Trajectory Action Server
    to send commands of standard action type,
    control_msgs/FollowJointTrajectoryAction.

    Make sure to start the joint_trajectory_action_server.py
    first.

    Get two points from the user.

    Use the Joint Trajectory Action Server to find a path between them.
    
    Repeat the trajectory until program is stopped.
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument('-l',
                          '--limb',
                          required=True,
                          choices=['left', 'right'],
                          help='send joint trajectory to which limb')
    required.add_argument('-f',
                          '--folder',
                          required=True,
                          help='path to assets/ folder containing help images')
    args = parser.parse_args(rospy.myargv()[1:])
    print args
    limb = args.limb

    print("Initializing node... ")
    rospy.init_node("point_input_trajectory_%s" % (limb, ))
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    print("Enabling robot... ")
    rs.enable()

    traj = common.Trajectory(limb)
    rospy.on_shutdown(traj.stop)

    filenames = ["getpoint1.png", "getpoint2.png", "executing.png"]
    filenames = [args.folder + filename for filename in filenames]

    for filename in filenames:
        if not os.access(filename, os.R_OK):
            rospy.logerr("Cannot read file at '%s'" % (filename, ))
            return 1

    limbInterface = baxter_interface.Limb(limb)

    common.send_image(filenames[0])
    #Get the current position
    buttonpress = common.ButtonListener()
    buttonpress.subscribe("/robot/digital_io/" + limb + "_lower_button/state")
    points = []

    while not buttonpress.pressed:
        rospy.sleep(0.5)

    points.append(buttonpress.getButtonPressTraj(limb, limbInterface, traj))
    print "Got first position:"
    print points[0]

    buttonpress.pressed = False

    common.send_image(filenames[1])

    while not buttonpress.pressed:
        rospy.sleep(0.5)

    points.append(buttonpress.getButtonPressTraj(limb, limbInterface, traj))
    print "Got second position:"
    print points[1]

    common.send_image(filenames[2])
    print("Running. Ctrl-c to quit")

    i = 0
    while i < 1000 and not rospy.is_shutdown():
        #Make sure the points alternate
        traj.add_point(points[i % 2], 15.0)
        traj.add_point(points[(i + 1) % 2], 15.0)
        traj.start()
        traj.wait()
        traj.clear(limb)
        print "Completed test", i
        i += 1