class TestProgramHelper(unittest.TestCase): def setUp(self): # TODO add more blocks self.prog = Program() self.ph = ProgramHelper() self.prog.header.id = 666 self.prog.header.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) p = ProgramItem() p.id = 1 p.on_success = 2 p.on_failure = 0 p.type = ProgramItem.GET_READY pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 2 p.on_success = 3 p.on_failure = 0 p.type = ProgramItem.WAIT_FOR_USER pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 3 p.on_success = 31 p.on_failure = 0 p.type = ProgramItem.PICK_FROM_FEEDER p.object.append("profile") pf = PoseStamped() pf.header.frame_id = "marker" pf.pose.position.x = 0.75 pf.pose.position.y = 0.5 p.pose.append(pf) pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 31 p.on_success = 4 p.on_failure = 0 p.type = ProgramItem.VISUAL_INSPECTION p.ref_id.append(3) pf = PoseStamped() pf.header.frame_id = "marker" pf.pose.position.x = 0.75 pf.pose.position.y = 0.5 pf.pose.position.z = 0.1 p.pose.append(pf) pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 4 p.on_success = 5 p.on_failure = 0 p.type = ProgramItem.PLACE_TO_POSE p.ref_id.append(3) p.ref_id.append(5) pp = PoseStamped() pp.header.frame_id = "marker" pp.pose.position.x = 0.75 pp.pose.position.y = 0.5 p.pose.append(pp) pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 5 p.on_success = 6 p.on_failure = 0 p.type = ProgramItem.PICK_FROM_FEEDER p.object.append("profile") pf = PoseStamped() pf.header.frame_id = "marker" pf.pose.position.x = 0.75 pf.pose.position.y = 0.5 p.pose.append(pf) pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 6 p.on_success = 7 p.on_failure = 0 p.type = ProgramItem.GET_READY pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 7 p.on_success = 8 p.on_failure = 0 p.type = ProgramItem.WAIT_UNTIL_USER_FINISHES pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 8 p.on_success = 9 p.on_failure = 0 p.type = ProgramItem.PICK_FROM_POLYGON p.object.append("profile") pp = PolygonStamped() pp.header.frame_id = "marker" pp.polygon.points.append(Point32(0.4, 0.1, 0)) pp.polygon.points.append(Point32(1.0, 0.1, 0)) pp.polygon.points.append(Point32(1.0, 0.6, 0)) pp.polygon.points.append(Point32(0.4, 0.6, 0)) p.polygon.append(pp) pb.items.append(deepcopy(p)) p = ProgramItem() p.id = 9 p.on_success = 4 p.on_failure = 0 p.type = ProgramItem.PLACE_TO_POSE p.ref_id.append(8) pp = PoseStamped() pp.header.frame_id = "marker" pp.pose.position.x = 0.75 pp.pose.position.y = 0.5 p.pose.append(pp) pb.items.append(deepcopy(p)) def test_alt_prog_item(self): prog = Program() prog.header.id = 123 pb = ProgramBlock() pb.id = 10 prog.blocks.append(pb) pi = ProgramItem() pi.id = 1 pi.type = ProgramItem.PICK_FROM_FEEDER pi.object.append("obj1") pi.pose.append(PoseStamped()) pi.on_success = 2 pb.items.append(pi) pi = ProgramItem() pi.id = 2 pi.type = ProgramItem.PLACE_TO_GRID pi.polygon.append(PolygonStamped()) pi.ref_id.append(1) for i in range(0, 3): pi.pose.append(PoseStamped()) pi.on_success = 1 pi.on_failure = 3 pb.items.append(pi) pi = ProgramItem() pi.id = 3 pi.type = ProgramItem.DRILL_POINTS pi.ref_id.append(2) pi.pose.append(PoseStamped()) pi.on_success = 3 pi.on_failure = 1 pb.items.append(pi) res = self.ph.load(prog) self.assertEquals(res, True, "alt program") ret = self.ph.get_object(10, 3) self.assertEquals(ret[0][0], "obj1", "ref_id object") self.assertEquals(ret[1], 1, "ref_id object - source item id") ret = self.ph.get_polygon(10, 3) self.assertEquals(len(ret[0]), 1, "ref_id polygon") self.assertEquals(ret[1], 2, "ref_id polygon - source item id") self.assertRaises(ValueError, self.ph.get_polygon, 10, 1) def test_empty_program(self): res = self.ph.load(Program()) self.assertEquals(res, False, "empty program") def test_empty_block(self): prog = Program() prog.blocks.append(ProgramBlock()) res = self.ph.load(prog) self.assertEquals(res, False, "empty block") def test_invalid_block_id(self): prog = deepcopy(self.prog) prog.blocks[0].id = 0 res = self.ph.load(prog) self.assertEquals(res, False, "invalid block id") def test_invalid_item_id(self): prog = deepcopy(self.prog) prog.blocks[0].items[0].id = 0 res = self.ph.load(prog) self.assertEquals(res, False, "invalid item id") def test_invalid_item_on_success(self): prog = deepcopy(self.prog) prog.blocks[0].items[1].on_success = 1234 res = self.ph.load(prog) self.assertEquals(res, False, "invalid item on_success") def test_invalid_item_on_failure(self): prog = deepcopy(self.prog) prog.blocks[0].items[1].on_failure = 1234 res = self.ph.load(prog) self.assertEquals(res, False, "invalid item on_failure") def test_valid_program(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "valid program") def test_on_success(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "on_success") (block_id, item_id) = self.ph.get_id_on_success(1, 2) self.assertEquals(block_id, 1, "on_success - block_id") self.assertEquals(item_id, 3, "on_success - item_id") def test_on_failure(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "on_failure") (block_id, item_id) = self.ph.get_id_on_failure(1, 4) self.assertEquals(block_id, 0, "on_failure - block_id") self.assertEquals(item_id, 0, "on_failure - item_id") def test_get_item_msg(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_item_msg") msg = self.ph.get_item_msg(1, 4) self.assertEquals(isinstance(msg, ProgramItem), True, "get_item_msg - type") self.assertEquals(msg.id, 4, "get_item_msg - id") def test_get_block_msg(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_block_msg") msg = self.ph.get_block_msg(1) self.assertEquals(isinstance(msg, ProgramBlock), True, "get_block_msg - type") self.assertEquals(msg.id, 1, "get_block_msg - id") def test_get_first_block_id(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_first_block_id") block_id = self.ph.get_first_block_id() self.assertEquals(block_id, self.prog.blocks[0].id, "get_first_block_id") def test_get_first_item_id(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_first_block_id") (block_id, item_id) = self.ph.get_first_item_id() self.assertEquals(block_id, self.prog.blocks[0].id, "get_first_item_id - block") self.assertEquals(item_id, self.prog.blocks[0].items[0].id, "get_first_item_id - item") def test_get_program_id(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_program_id") prog_id = self.ph.get_program_id() self.assertEquals(prog_id, 666, "get_program_id - id") def test_template(self): self.ph.load(self.prog) self.assertEquals(self.ph.program_learned(), True, "test_template") self.ph.load(self.prog, True) self.assertEquals(self.ph.program_learned(), False, "test_template") def test_invalid_ref_id(self): prog = deepcopy(self.prog) prog.blocks[0].items[0].ref_id.append(6587) res = self.ph.load(prog) self.assertEquals(res, False, "test_invalid_ref_id")
class TestProgramHelper(unittest.TestCase): 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) def test_empty_program(self): res = self.ph.load(Program()) self.assertEquals(res, False, "empty program") def test_empty_block(self): prog = Program() prog.blocks.append(ProgramBlock()) res = self.ph.load(prog) self.assertEquals(res, False, "empty block") def test_invalid_block_id(self): self.prog.blocks[0].id = 0 res = self.ph.load(self.prog) self.assertEquals(res, False, "invalid block id") def test_invalid_item_id(self): self.prog.blocks[0].items[0].id = 0 res = self.ph.load(self.prog) self.assertEquals(res, False, "invalid item id") def test_invalid_item_on_success(self): self.prog.blocks[0].items[1].on_success = 1234 res = self.ph.load(self.prog) self.assertEquals(res, False, "invalid item on_success") def test_invalid_item_on_failure(self): self.prog.blocks[0].items[1].on_failure = 1234 res = self.ph.load(self.prog) self.assertEquals(res, False, "invalid item on_failure") def test_valid_program(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "valid program") def test_on_success(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "on_success") (block_id, item_id) = self.ph.get_id_on_success(1, 2) self.assertEquals(block_id, 1, "on_success - block_id") self.assertEquals(item_id, 3, "on_success - item_id") def test_on_failure(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "on_failure") (block_id, item_id) = self.ph.get_id_on_failure(1, 4) self.assertEquals(block_id, 0, "on_failure - block_id") self.assertEquals(item_id, 0, "on_failure - item_id") def test_get_item_msg(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_item_msg") msg = self.ph.get_item_msg(1, 4) self.assertEquals(isinstance(msg, ProgramItem), True, "get_item_msg - type") self.assertEquals(msg.id, 4, "get_item_msg - id") def test_get_block_msg(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_block_msg") msg = self.ph.get_block_msg(1) self.assertEquals(isinstance(msg, ProgramBlock), True, "get_block_msg - type") self.assertEquals(msg.id, 1, "get_block_msg - id") def test_get_first_block_id(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_first_block_id") block_id = self.ph.get_first_block_id() self.assertEquals(block_id, self.prog.blocks[0].id, "get_first_block_id") def test_get_first_item_id(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_first_block_id") (block_id, item_id) = self.ph.get_first_item_id() self.assertEquals(block_id, self.prog.blocks[0].id, "get_first_item_id - block") self.assertEquals(item_id, self.prog.blocks[0].items[0].id, "get_first_item_id - item") def test_get_program_id(self): res = self.ph.load(self.prog) self.assertEquals(res, True, "get_program_id") prog_id = self.ph.get_program_id() self.assertEquals(prog_id, 666, "get_program_id - id")