Example #1
0
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")
Example #2
0
def main(args):

    if len(args) < 2:

        print "This script needs program id as argument."
        return

    rospy.init_node('plot_program', anonymous=True)

    art = ArtApiHelper()
    ph = ProgramHelper()

    prog = art.load_program(int(args[1]))

    if prog is None:

        print "Failed to load program"
        return

    if not ph.load(prog):

        print "Faulty program"
        return

    A = pgv.AGraph(label="<<B>Program ID: " + str(prog.header.id) + "<br/>" +
                   escape(prog.header.name) + "</B>>",
                   directed=True,
                   strict=False)
    A.graph_attr['outputorder'] = 'edgesfirst'

    A.add_node("start", label="Program start")
    A.add_node("b0i0", label="Program end")

    A.add_edge("start", get_node_name(*ph.get_first_item_id()))

    for block_id in ph.get_block_ids():

        block_msg = ph.get_block_msg(block_id)
        ids = []

        for item_id in ph.get_items_ids(block_id):

            nn = get_node_name(block_id, item_id)
            item_msg = ph.get_item_msg(block_id, item_id)

            osn = get_node_name(*ph.get_id_on_success(block_id, item_id))
            ofn = get_node_name(*ph.get_id_on_failure(block_id, item_id))

            A.add_edge(nn, osn, color="green", constraint=True)
            A.add_edge(nn, ofn, color="red", constraint=True)

            for ref in item_msg.ref_id:

                A.add_edge(get_node_name(block_id, ref),
                           nn,
                           color="gray",
                           style="dashed",
                           key="ref_" + nn + "_" +
                           get_node_name(block_id, ref))

            A.get_node(nn).attr['label'] = 'Item ID: ' + str(
                item_id) + '\n' + get_type_string(item_msg)
            A.get_node(nn).attr['shape'] = 'box'
            A.get_node(nn).attr['style'] = 'rounded'
            ids.append(nn)

        sg = A.subgraph(name="cluster_" + str(block_id),
                        label="<<b>Group ID: " + str(block_id) + "<br/>" +
                        escape(block_msg.name) + "</b>>",
                        color="gray")
        sg.add_nodes_from(ids)

    A.layout(prog='dot')
    A.draw('output.pdf')
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")