def test_stow():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
#rospy.wait_for_service('update_calibration')
#s = rospy.ServiceProxy('update_calibration',CalibrationUpdate)
#response = s(upper_left=Point(1250,35,1037), upper_right=Point(1250,-835,1037))
items = ["jane_eyre_dvd"]
pos_info = ItemLocations({"bin_G": ["elmers_washable_no_run_school_glue"]},
items)
# find stow location
r = ShelfRandomLocationStow(pos_info)
item = "jane_eyre_dvd"
print "try to find a location for %s" % item
(target_bin, target_pos) = r.find_location(item)
print "result:", (target_bin, target_pos)
if raw_input("press enter to continue or q to quit: ") == "q":
return
# move to tote
#result = arm_control_wrapper.move_left_arm_to_bin('bin_D', 'pre', keitai='nut000', kakujiku='kakujiku')
result = arm_control_wrapper.move_left_arm_to_bin("tote",
"photo",
keitai='fut000',
kakujiku='kakujiku')
if raw_input("press enter to continue or q to quit: ") == "q":
return
# check grabbability
v = VacuumGrabbing()
print "check if we can grab %s from %s" % (item, "tote")
result = v.can_grab_item(item, "tote", items)
print "result:", result
if not result:
print "did not detect item"
return
if raw_input("press enter to continue or q to quit: ") == "q":
return
# take the item
print "take item"
result = v.grab_from_tote(item)
print "result:", result
if not result:
print "did not take item"
return
if raw_input("press enter to continue or q to quit: ") == "q":
return
# put it into the shelf
print "stow it"
result = v.stow_in_shelf(target_bin, target_pos)
print "result:", result
def test_pick():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
rospy.wait_for_service('update_calibration')
s = rospy.ServiceProxy('update_calibration', CalibrationUpdate)
response = s(upper_left=Point(1250, 35, 1037),
upper_right=Point(1250, -835, 1037))
r = RandomLocationStow("pos_info_dummy")
item = "jane_eyre_dvd"
print "try to find a location for %s" % item
target = r.find_location(item)
print "result:", target
if raw_input("press enter to continue or q to quit: ") == "q":
return
#arm_control_wrapper.move_right_arm_global(target)
if raw_input("press enter to continue or q to quit: ") == "q":
return
# get into position
bin = "bin_G"
pos = "photo"
print "move to %s/%s" % (bin, pos)
#result = arm_control_wrapper.move_left_arm_global([698,-356,-260,179,0,179])
#print "result:", result
result = arm_control_wrapper.move_left_arm_to_bin(bin,
pos,
keitai='nut000',
kakujiku='chokusen')
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# check grabbability
v = VacuumGrabbing()
print "check if we can grab %s from %s" % (item, [
"elmers_washable_no_run_school_glue", "dasani_water_bottle",
"jane_eyre_dvd"
])
result = v.can_grab_item(item, bin, [
"elmers_washable_no_run_school_glue", "dasani_water_bottle",
"jane_eyre_dvd"
])
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# take the item
print "take item"
result = v.grab_from_shelf(item, bin)
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# put it into the box
print "stow it"
result = v.stow_in_tote(target)
print "result:", result
def test_vacuum_control():
v = VacuumGrabbing()
for (desc, fun) in [("on", v.vacuum_on), ("normal", v.vacuum_normal)]:
print "switch vacuum to %s" % desc
fun()
if raw_input("press enter to continue or q to quit: ") == "q":
return
def test_pipe_control():
v = VacuumGrabbing()
for (deg, fun) in [("0", v.pipe_0_deg), ("90", v.pipe_90_deg),
("0", v.pipe_0_deg)]:
print "switch pipe to %s degrees" % deg
fun()
if raw_input("press enter to continue or q to quit: ") == "q":
return
def test_can_grab_item():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
rospy.wait_for_service('update_calibration')
s = rospy.ServiceProxy('update_calibration', CalibrationUpdate)
response = s(upper_left=Point(1300, 35, 1037),
upper_right=Point(1250, -835, 1037))
v = VacuumGrabbing()
# get into position
bin = "bin_G"
pos = "photo"
print "move to %s/%s" % (bin, pos)
result = arm_control_wrapper.move_left_arm_to_bin(bin,
pos,
keitai='nut000',
kakujiku='chokusen')
print "result:", result
# check if we can grab various items
correct_inventory = [
"dasani_water_bottle", "elmers_washable_no_run_school_glue",
"peva_shower_curtain_liner", "i_am_a_bunny_book",
"dr_browns_bottle_brush", "staples_index_cards"
] #"ticonderoga_12_pencils", "rawlings_baseball", "easter_turtle_sippy_cup", "fiskars_scissors_red"]
extended_inventory = [
"elmers_washable_no_run_school_glue", "dasani_water_bottle"
]
wrong_inventory = ["dasani_water_bottle"]
for inventory in [correct_inventory, extended_inventory, wrong_inventory]:
for item in [
"elmers_washable_no_run_school_glue", "dasani_water_bottle"
]:
print "check if we can grab %s from %s" % (item, inventory)
result = v.can_grab_item(item, bin, inventory)
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
# check how completely bogus item names work
print "check if we can grab %s from %s" % ("hogehoge", correct_inventory)
result = v.can_grab_item("hogehoge", bin, correct_inventory)
print "result:", result