def main(args):
global p
rospy.init_node('art_db_service_tester', anonymous=True)
p = Program()
p.id = 0
p.name = "Basic pick&place"
p0 = ProgramItem()
p0.id = 0
p0.on_success = 1
p0.on_failure = 100
p0.type = ProgramItem.GET_READY
p.items.append(p0)
p1 = ProgramItem()
p1.id = 1
p1.on_success = 2
p1.on_failure = 100
p1.type = ProgramItem.WAIT
p1.spec = ProgramItem.WAIT_FOR_USER
p.items.append(p1)
p2 = ProgramItem()
p2.id = 2
p2.on_success = 3
p2.on_failure = 100
p2.type = ProgramItem.MANIP_PICK_PLACE
p2.spec = ProgramItem.MANIP_TYPE
p2.object = "profile"
# TODO p2.pick_polygon
p2.place_pose.header.frame_id = "marker"
p2.place_pose.pose.position.x = 0.75
p2.place_pose.pose.position.y = 0.5
p.items.append(p2)
p3 = ProgramItem()
p3.id = 3
p3.on_success = 4
p3.on_failure = 100
p3.type = ProgramItem.WAIT
p3.spec = ProgramItem.WAIT_UNTIL_USER_FINISHES
p.items.append(p3)
p4 = ProgramItem()
p4.id = 4
p4.on_success = 0
p4.on_failure = 100
p4.type = ProgramItem.MANIP_PICK_PLACE
p4.spec = ProgramItem.MANIP_TYPE
p4.object = "profile"
# TODO p4.pick_polygon?
p4.pick_pose = p2.place_pose
p4.place_pose.header.frame_id = "marker"
p4.place_pose.pose.position.x = 0.25
p4.place_pose.pose.position.y = 0.5
p.items.append(p4)
rospy.wait_for_service('/art/db/program/store')
try:
store_program_srv = rospy.ServiceProxy('/art/db/program/store', storeProgram)
resp = store_program_srv(program=p)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return
def test_program(self):
p = Program()
p.id = 999
p.name = "Test pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
p.blocks.append(pb)
p0 = ProgramItem()
p0.id = 1
p0.on_success = 2
p0.on_failure = 0
p0.type = ProgramItem.GET_READY
pb.items.append(p0)
p1 = ProgramItem()
p1.id = 2
p1.on_success = 3
p1.on_failure = 0
p1.type = ProgramItem.WAIT
p1.spec = ProgramItem.WAIT_FOR_USER
pb.items.append(p1)
p2 = ProgramItem()
p2.id = 3
p2.on_success = 4
p2.on_failure = 0
p2.type = ProgramItem.MANIP_PICK_PLACE
p2.spec = ProgramItem.MANIP_TYPE
p2.object = "profile"
# TODO p2.pick_polygon
p2.place_pose.header.frame_id = "marker"
p2.place_pose.pose.position.x = 0.75
p2.place_pose.pose.position.y = 0.5
pb.items.append(p2)
p3 = ProgramItem()
p3.id = 4
p3.on_success = 5
p3.on_failure = 0
p3.type = ProgramItem.WAIT
p3.spec = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(p3)
p4 = ProgramItem()
p4.id = 5
p4.on_success = 0
p4.on_failure = 0
p4.type = ProgramItem.MANIP_PICK_PLACE
p4.spec = ProgramItem.MANIP_TYPE
p4.object = "profile"
# TODO p4.pick_polygon?
p4.pick_pose = p2.place_pose
p4.place_pose.header.frame_id = "marker"
p4.place_pose.pose.position.x = 0.25
p4.place_pose.pose.position.y = 0.5
pb.items.append(p4)
try:
resp_store = self.store_program_srv(program=p)
except rospy.ServiceException:
pass
self.assertEquals(resp_store.success, True, "program_store")
try:
resp_get = self.get_program_srv(id=999)
except rospy.ServiceException:
pass
self.assertEquals(resp_get.success, True, "program_get")
self.assertEquals(resp_get.program.id, 999, "program_get")
def setUp(self):
# TODO add more blocks
self.prog = Program()
self.ph = ProgramHelper()
self.prog.id = 666
self.prog.name = "Basic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
self.prog.blocks.append(pb)
p0 = ProgramItem()
p0.id = 1
p0.on_success = 2
p0.on_failure = 0
p0.type = ProgramItem.GET_READY
pb.items.append(p0)
p1 = ProgramItem()
p1.id = 2
p1.on_success = 3
p1.on_failure = 0
p1.type = ProgramItem.WAIT
p1.spec = ProgramItem.WAIT_FOR_USER
pb.items.append(p1)
p2 = ProgramItem()
p2.id = 3
p2.on_success = 4
p2.on_failure = 0
p2.type = ProgramItem.MANIP_PICK_PLACE
p2.spec = ProgramItem.MANIP_TYPE
p2.object = "profile"
p2.place_pose.header.frame_id = "marker"
p2.place_pose.pose.position.x = 0.75
p2.place_pose.pose.position.y = 0.5
pb.items.append(p2)
p3 = ProgramItem()
p3.id = 4
p3.on_success = 5
p3.on_failure = 0
p3.type = ProgramItem.WAIT
p3.spec = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(p3)
p4 = ProgramItem()
p4.id = 5
p4.on_success = 0
p4.on_failure = 0
p4.type = ProgramItem.MANIP_PICK_PLACE
p4.spec = ProgramItem.MANIP_TYPE
p4.object = "profile"
p4.pick_pose = p2.place_pose
p4.place_pose.header.frame_id = "marker"
p4.place_pose.pose.position.x = 0.25
p4.place_pose.pose.position.y = 0.5
pb.items.append(p4)
