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