Exemplo n.º 1
0
    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)
Exemplo n.º 2
0
    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 = "PickFromFeeder"
        pi.object.append("obj1")
        pi.pose.append(PoseStamped())
        pi.on_success = 2
        pb.items.append(pi)

        pi = ProgramItem()
        pi.id = 2
        pi.type = "PlaceToGrid"
        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 = "DrillPoints"
        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(ProgramHelperException, self.ph.get_polygon, 10, 1)
Exemplo n.º 3
0
def item(it_id, it_type, on_success=it_id + 1, on_failure=0, ref_id=[]):

    p = ProgramItem()
    p.id = it_id
    p.type = it_type
    p.on_success = on_success
    p.on_failure = on_failure
    p.ref_id = ref_id

    return p
Exemplo n.º 4
0
def main(args):

    global p

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

    # -------------------------------------------------------------------------------------------
    # PROGRAMS
    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 0
    prog.header.name = "Advanced pick&place"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    p = ProgramItem()
    p.id = 1
    p.on_success = 2
    p.on_failure = 0
    p.type = "GetReady"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 2
    p.on_success = 3
    p.on_failure = 0
    p.type = "WaitForUser"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 3
    p.on_success = 4
    p.on_failure = 0
    p.type = "PickFromFeeder"
    p.object.append("")
    pf = PoseStamped()
    p.pose.append(pf)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 4
    p.on_success = 6
    p.on_failure = 0
    p.type = "PlaceToPose"
    p.ref_id.append(3)
    pp = PoseStamped()
    pp.header.frame_id = "marker"
    p.pose.append(pp)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 6
    p.on_success = 7
    p.on_failure = 0
    p.type = "GetReady"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 7
    p.on_success = 8
    p.on_failure = 0
    p.type = "WaitUntilUserFinishes"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 8
    p.on_success = 9
    p.on_failure = 0
    p.type = "PickFromFeeder"
    p.object.append("")
    pp.header.frame_id = "marker"
    pf = PoseStamped()
    p.pose.append(pf)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 9
    p.on_success = 1
    p.on_failure = 0
    p.type = "PlaceToPose"
    p.ref_id.append(8)
    pp = PoseStamped()
    p.pose.append(pp)
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 2
    prog.header.name = "Classic pick&place"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    p = ProgramItem()
    p.id = 1
    p.on_success = 2
    p.on_failure = 0
    p.type = "GetReady"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 2
    p.on_success = 3
    p.on_failure = 0
    p.type = "WaitForUser"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 3
    p.on_success = 4
    p.on_failure = 0
    p.type = "PickFromPolygon"
    p.object.append("")
    pp = PolygonStamped()
    pp.header.frame_id = "marker"
    p.polygon.append(pp)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 4
    p.on_success = 5
    p.on_failure = 0
    p.type = "PlaceToPose"
    p.ref_id.append(3)
    pp = PoseStamped()
    pp.header.frame_id = "marker"
    p.pose.append(pp)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 5
    p.on_success = 6
    p.on_failure = 0
    p.type = "GetReady"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 6
    p.on_success = 7
    p.on_failure = 0
    p.type = "WaitUntilUserFinishes"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 7
    p.on_success = 8
    p.on_failure = 0
    p.type = "PickFromPolygon"
    p.object.append("")
    p.object.append("profile_20_60")
    pp = PolygonStamped()
    pp.header.frame_id = "marker"
    p.polygon.append(pp)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 8
    p.on_success = 1
    p.on_failure = 0
    p.type = "PlaceToPose"
    p.ref_id.append(7)
    pp = PoseStamped()
    pp.header.frame_id = "marker"
    p.pose.append(pp)
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 3
    prog.header.name = "Welding seam"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    p = ProgramItem()
    p.id = 1
    p.on_success = 2
    p.on_failure = 0
    p.type = "GetReady"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 2
    p.on_success = 2
    p.on_failure = 0
    p.type = "WeldingSeam"
    p.object.append("")
    pf = PoseStamped()
    pf.header.frame_id = "marker"
    pf.pose.position.x = 0.4
    pf.pose.position.y = 0.4
    pf.pose.position.z = 0.30
    pf.pose.orientation.x = -0.707
    pf.pose.orientation.y = 0
    pf.pose.orientation.z = 0.707
    pf.pose.orientation.w = 0
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.5
    p.pose.append(deepcopy(pf))
    pf.pose.position.y = 0.2
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.4
    p.pose.append(deepcopy(pf))
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 4
    prog.header.name = "Welding points"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    p = ProgramItem()
    p.id = 1
    p.on_success = 2
    p.on_failure = 0
    p.type = "GetReady"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 2
    p.on_success = 2
    p.on_failure = 0
    p.type = "WeldingPoints"
    p.object.append("")
    pf = PoseStamped()
    pf.header.frame_id = "marker"
    pf.pose.position.x = 0.4
    pf.pose.position.y = 0.4
    pf.pose.position.z = 0.30
    pf.pose.orientation.x = -0.707
    pf.pose.orientation.y = 0
    pf.pose.orientation.z = 0.707
    pf.pose.orientation.w = 0
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.5
    p.pose.append(deepcopy(pf))
    pf.pose.position.y = 0.2
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.4
    p.pose.append(deepcopy(pf))
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 5
    prog.header.name = "Drill points"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    p = ProgramItem()
    p.id = 1
    p.on_success = 2
    p.on_failure = 0
    p.type = "GetReady"
    p.flags = [KeyValue]
    p.flags[0].key = "CLEAR_OBJECT_FLAGS"
    p.flags[0].value = "true"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 2
    p.on_success = 2
    p.on_failure = 3
    p.type = "DrillPoints"
    p.object.append("profile_20_60")
    pf = PoseStamped()
    pf.header.frame_id = "object_id_profile_20_60"
    pf.pose.position.x = 0.4
    pf.pose.position.y = 0.4
    pf.pose.position.z = 0.30
    pf.pose.orientation.x = -0.707
    pf.pose.orientation.y = 0
    pf.pose.orientation.z = 0.707
    pf.pose.orientation.w = 0
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.5
    p.pose.append(deepcopy(pf))
    pf.pose.position.y = 0.2
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.4
    p.pose.append(deepcopy(pf))

    dp = PolygonStamped()
    dp.header.frame_id = "marker"
    p.polygon.append(dp)

    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 3
    p.on_success = 3
    p.on_failure = 0
    p.type = "DrillPoints"
    p.object.append("profile_20_60")
    pf = PoseStamped()
    pf.header.frame_id = "object_id_profile_20_60"
    pf.pose.position.x = 0.4
    pf.pose.position.y = 0.4
    pf.pose.position.z = 0.30
    pf.pose.orientation.x = -0.707
    pf.pose.orientation.y = 0
    pf.pose.orientation.z = 0.707
    pf.pose.orientation.w = 0
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.5
    p.pose.append(deepcopy(pf))
    pf.pose.position.y = 0.2
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.4
    p.pose.append(deepcopy(pf))

    dp = PolygonStamped()
    dp.header.frame_id = "marker"
    p.polygon.append(dp)

    pb.items.append(deepcopy(p))

    store_program(prog)
    # -------------------------------------------------------------------------------------------
    """
    prog = Program()
    prog.header.id = 9
    prog.header.name = "Place to grid"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First Block"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    p = ProgramItem()
    p.id = 1
    p.on_success = 2
    p.on_failure = 0
    p.type = "GetReady"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 2
    p.on_success = 3
    p.on_failure = 0
    p.type = "WaitForUser"
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 3
    p.on_success = 4
    p.on_failure = 0
    p.type = "PickFromPolygon"
    p.object.append("")
    p.object.append("profile_20_60")
    pp = PolygonStamped()
    pp.header.frame_id = "marker"
    p.polygon.append(pp)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 4
    p.on_success = 3
    p.on_failure = 0
    p.type = ProgramItem.PLACE_TO_GRID
    pp = PolygonStamped()
    p.polygon.append(pp)
    p.ref_id.append(3)
    p.pose.append(PoseStamped())
    p.pose.append(PoseStamped())
    p.pose.append(PoseStamped())
    p.pose.append(PoseStamped())
    pb.items.append(deepcopy(p))

    store_program(prog)
    """

    # -------------------------------------------------------------------------------------------
    # OBJECT TYPES
    # -------------------------------------------------------------------------------------------

    ot = ObjectType()
    ot.name = "profile_20_60"
    ot.bbox.type = SolidPrimitive.BOX
    ot.bbox.dimensions.append(0.05)
    ot.bbox.dimensions.append(0.05)
    ot.bbox.dimensions.append(0.16)
    store_object_type(ot)

    ot = ObjectType()
    ot.name = "placka"
    ot.bbox.type = SolidPrimitive.BOX
    ot.bbox.dimensions.append(0.1)
    ot.bbox.dimensions.append(0.1)
    ot.bbox.dimensions.append(0.008)
    store_object_type(ot)

    ot = ObjectType()
    ot.name = "wood_46_150"
    ot.bbox.type = SolidPrimitive.BOX
    ot.bbox.dimensions.append(0.046)
    ot.bbox.dimensions.append(0.046)
    ot.bbox.dimensions.append(0.154)
    store_object_type(ot)

    ot = ObjectType()
    ot.name = "wood_46_300"
    ot.bbox.type = SolidPrimitive.BOX
    ot.bbox.dimensions.append(0.046)
    ot.bbox.dimensions.append(0.046)
    ot.bbox.dimensions.append(0.298)
    store_object_type(ot)

    ot = ObjectType()
    ot.name = "karta"
    ot.bbox.type = SolidPrimitive.BOX
    ot.bbox.dimensions.append(0.08)
    ot.bbox.dimensions.append(0.04)
    ot.bbox.dimensions.append(0.002)

    store_object_type(ot)

    ot.name = "profile_20_80"

    store_object_type(ot)
Exemplo n.º 5
0
    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 = "GetReady"
        pb.items.append(deepcopy(p))

        p = ProgramItem()
        p.id = 2
        p.on_success = 3
        p.on_failure = 0
        p.type = "WaitForUser"
        pb.items.append(deepcopy(p))

        p = ProgramItem()
        p.id = 3
        p.on_success = 31
        p.on_failure = 0
        p.type = "PickFromFeeder"
        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 = "VisualInspection"
        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 = "PlaceToPose"
        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 = "PickFromFeeder"
        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 = "GetReady"
        pb.items.append(deepcopy(p))

        p = ProgramItem()
        p.id = 7
        p.on_success = 8
        p.on_failure = 0
        p.type = "WaitUntilUserFinishes"
        pb.items.append(deepcopy(p))

        p = ProgramItem()
        p.id = 8
        p.on_success = 9
        p.on_failure = 0
        p.type = "PickFromPolygon"
        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 = "PlaceToPose"
        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))
Exemplo n.º 6
0
def main(args):

    global p

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

    # -------------------------------------------------------------------------------------------
    # PROGRAMS
    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 0
    prog.header.name = "Advanced pick&place"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    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 = 4
    p.on_failure = 0
    p.type = ProgramItem.PICK_FROM_FEEDER
    p.object.append("profile_20_60")
    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 = 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_20_60")
    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_20_60")
    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))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 3
    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
    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 = 4
    p.on_failure = 0
    p.type = ProgramItem.PICK_OBJECT_ID
    p.object.append("")
    pf = PoseStamped()
    pf.header.frame_id = "marker"
    p.pose.append(pf)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 4
    p.on_success = 3
    p.on_failure = 0
    p.type = ProgramItem.PLACE_TO_POSE
    p.ref_id.append(3)
    pp = PoseStamped()
    pp.header.frame_id = "marker"
    p.pose.append(pp)
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 2
    prog.header.name = "Classic pick&place"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    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 = 4
    p.on_failure = 0
    p.type = ProgramItem.PICK_FROM_POLYGON
    p.object.append("")
    pp = PolygonStamped()
    pp.header.frame_id = "marker"
    p.polygon.append(pp)
    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)
    pp = PoseStamped()
    pp.header.frame_id = "marker"
    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.GET_READY
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 6
    p.on_success = 7
    p.on_failure = 0
    p.type = ProgramItem.WAIT_UNTIL_USER_FINISHES
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 7
    p.on_success = 8
    p.on_failure = 0
    p.type = ProgramItem.PICK_FROM_POLYGON
    p.object.append("")
    p.object.append("profile_20_60")
    pp = PolygonStamped()
    pp.header.frame_id = "marker"
    p.polygon.append(pp)
    pb.items.append(deepcopy(p))

    p = ProgramItem()
    p.id = 8
    p.on_success = 1
    p.on_failure = 0
    p.type = ProgramItem.PLACE_TO_POSE
    p.ref_id.append(7)
    pp = PoseStamped()
    pp.header.frame_id = "marker"
    p.pose.append(pp)
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 3
    prog.header.name = "Welding seam"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    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 = 2
    p.on_failure = 0
    p.type = ProgramItem.WELDING_SEAM
    p.object.append("")
    pf = PoseStamped()
    pf.header.frame_id = "marker"
    pf.pose.position.x = 0.4
    pf.pose.position.y = 0.4
    pf.pose.position.z = 0.30
    pf.pose.orientation.x = -0.707
    pf.pose.orientation.y = 0
    pf.pose.orientation.z = 0.707
    pf.pose.orientation.w = 0
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.5
    p.pose.append(deepcopy(pf))
    pf.pose.position.y = 0.2
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.4
    p.pose.append(deepcopy(pf))
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 4
    prog.header.name = "Welding points"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    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 = 2
    p.on_failure = 0
    p.type = ProgramItem.WELDING_POINTS
    p.object.append("")
    pf = PoseStamped()
    pf.header.frame_id = "marker"
    pf.pose.position.x = 0.4
    pf.pose.position.y = 0.4
    pf.pose.position.z = 0.30
    pf.pose.orientation.x = -0.707
    pf.pose.orientation.y = 0
    pf.pose.orientation.z = 0.707
    pf.pose.orientation.w = 0
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.5
    p.pose.append(deepcopy(pf))
    pf.pose.position.y = 0.2
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.4
    p.pose.append(deepcopy(pf))
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    prog = Program()
    prog.header.id = 5
    prog.header.name = "Drill points"

    pb = ProgramBlock()
    pb.id = 1  # can't be zero
    pb.name = "First block"
    pb.on_success = 1
    pb.on_failure = 0
    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 = 2
    p.on_failure = 0
    p.type = ProgramItem.DRILL_POINTS
    p.object.append("")
    pf = PoseStamped()
    pf.header.frame_id = "marker"
    pf.pose.position.x = 0.4
    pf.pose.position.y = 0.4
    pf.pose.position.z = 0.30
    pf.pose.orientation.x = -0.707
    pf.pose.orientation.y = 0
    pf.pose.orientation.z = 0.707
    pf.pose.orientation.w = 0
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.5
    p.pose.append(deepcopy(pf))
    pf.pose.position.y = 0.2
    p.pose.append(deepcopy(pf))
    pf.pose.position.x = 0.4
    p.pose.append(deepcopy(pf))
    pb.items.append(deepcopy(p))

    store_program(prog)

    # -------------------------------------------------------------------------------------------
    # OBJECT TYPES
    # -------------------------------------------------------------------------------------------

    ot = ObjectType()
    ot.name = "profile_20_60"
    ot.bbox.type = SolidPrimitive.BOX
    ot.bbox.dimensions.append(0.04)
    ot.bbox.dimensions.append(0.04)
    ot.bbox.dimensions.append(0.13)

    store_object_type(ot)

    ot.name = "profile_20_80"

    store_object_type(ot)
Exemplo n.º 7
0
def main(args):

    global setup

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

    try:
        setup = os.environ["ARTABLE_SETUP"]
    except KeyError:
        rospy.logerr("ARTABLE_SETUP has to be set!")
        return

    api = ArtApiHelper()
    api.wait_for_db_api()

    rospy.loginfo("Refreshing collision environment...")
    api.clear_collision_primitives(setup)

    api.store_object_type(obj_type("desticka", 0.08, 0.08, 0.0125))
    api.store_object_type(
        obj_type("Modry_kontejner", 0.11, 0.165, 0.075, container=True))

    # delete all created programs
    ph = api.get_program_headers()
    if ph:
        for h in ph:
            api.program_clear_ro(h.id)
            api.delete_program(h.id)

    # TODO add parameters
    prog = Program()
    prog.header.id = 1
    prog.header.name = "MSV DEMO"

    pb = ProgramBlock()
    pb.id = 1
    pb.name = "Pick and place"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    pi = ProgramItem()
    pi.id = 1
    pi.on_success = 2
    pi.on_failure = 0
    pi.type = "PickFromPolygon"
    pi.object.append("")
    pi.polygon.append(PolygonStamped())
    pb.items.append(pi)

    pi = ProgramItem()
    pi.id = 2
    pi.on_success = 1
    pi.on_failure = 0
    pi.type = "PlaceToContainer"
    pi.object.append("")
    pi.polygon.append(PolygonStamped())
    pi.ref_id.append(1)
    pb.items.append(pi)

    api.store_program(prog)

    rospy.loginfo("Done!")
Exemplo n.º 8
0
def main(args):

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

    art = ArtApiHelper()
    # art.wait_for_api()

    # delete all created programs
    ph = art.get_program_headers()
    if ph:
        for h in ph:

            art.program_clear_ro(h.id)
            art.delete_program(h.id)

    # -------------------------------------------------------------------------------------------
    # Training program 1
    # -------------------------------------------------------------------------------------------

    prog = Program()
    prog.header.id = 1
    prog.header.name = "Trenink - oblast"

    pb = ProgramBlock()
    pb.id = 1
    pb.name = "Zvedni ze stolu a poloz"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    pb.items.append(polygon_item(1))
    pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0))
    pb.items.append(item(3, "GetReady", on_success=4, on_failure=0))
    pb.items.append(wait_item(4, ref_id=[2], on_success=1, on_failure=0))

    art.store_program(prog)
    art.program_set_ro(prog.header.id)

    # -------------------------------------------------------------------------------------------
    # Training program 2
    # -------------------------------------------------------------------------------------------

    prog = Program()
    prog.header.id = 2
    prog.header.name = "Trenink - podavac"

    pb = ProgramBlock()
    pb.id = 1
    pb.name = "Zvedni z podavace a poloz"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    pb.items.append(feeder_item(1))
    pb.items.append(place_item(2, ref_id=[1], on_success=3, on_failure=0))
    pb.items.append(feeder_item(3, ref_id=[1]))
    pb.items.append(place_item(4, ref_id=[3], on_success=5, on_failure=0))
    pb.items.append(item(5, "GetReady", on_success=6, on_failure=0))
    pb.items.append(wait_item(6, ref_id=[4], on_success=1, on_failure=0))

    art.store_program(prog)
    art.program_set_ro(prog.header.id)

    # -------------------------------------------------------------------------------------------
    # Training program 3
    # -------------------------------------------------------------------------------------------

    prog = Program()
    prog.header.id = 3
    prog.header.name = "Trenink - lepidlo"

    pb = ProgramBlock()
    pb.id = 1
    pb.name = "Aplikace lepidla"
    pb.on_success = 0
    pb.on_failure = 0
    prog.blocks.append(pb)

    pb.items.append(drill_item(1, on_success=1, on_failure=2, obj_type=[""]))
    pb.items.append(item(2, "GetReady", on_success=3, on_failure=0))
    pb.items.append(drill_item(3, on_success=3, on_failure=4, obj_type=[""]))
    pb.items.append(item(4, "GetReady", on_success=0, on_failure=0))

    art.store_program(prog)
    art.program_set_ro(prog.header.id)

    # -------------------------------------------------------------------------------------------
    # Training program 4
    # -------------------------------------------------------------------------------------------

    prog = Program()
    prog.header.id = 4
    prog.header.name = "Trenink - inspekce"

    pb = ProgramBlock()
    pb.id = 1
    pb.name = "Vizualni inspekce"
    pb.on_success = 0
    pb.on_failure = 0
    prog.blocks.append(pb)

    pb.items.append(polygon_item(1))
    pb.items.append(
        visual_inspection_item(2, ref_id=[1], on_success=3, on_failure=5))
    pb.items.append(
        visual_inspection_item(3, ref_id=[1], on_success=4, on_failure=5))
    pb.items.append(
        place_item(4, ref_id=[1], on_success=1, on_failure=0, name="OK parts"))
    pb.items.append(
        place_item(5, ref_id=[1], on_success=1, on_failure=0,
                   name="NOK parts"))

    art.store_program(prog)
    art.program_set_ro(prog.header.id)

    # -------------------------------------------------------------------------------------------
    # Training program 5
    # -------------------------------------------------------------------------------------------

    prog = Program()
    prog.header.id = 5
    prog.header.name = "Trenink - krabice"

    pb = ProgramBlock()
    pb.id = 1
    pb.name = "Polozeni do krabice"
    pb.on_success = 0
    pb.on_failure = 0
    prog.blocks.append(pb)

    pb.items.append(polygon_item(1))

    p = ProgramItem()
    p.id = 2
    p.type = "PlaceToContainer"
    p.polygon.append(PolygonStamped())
    p.object.append("")
    p.on_success = 1
    p.on_failure = 0
    p.ref_id.append(1)

    pb.items.append(p)

    art.store_program(prog)
    art.program_set_ro(prog.header.id)

    # -------------------------------------------------------------------------------------------
    # Simplified trolley assembly: object types
    # -------------------------------------------------------------------------------------------

    art.store_object_type(obj_type("Spojka", 0.046, 0.046, 0.154))
    art.store_object_type(
        obj_type("Kratka_noha", 0.046, 0.046, 0.298, container=True))
    art.store_object_type(obj_type("Dlouha_noha", 0.046, 0.046, 0.398))
    art.store_object_type(
        obj_type("Modry_kontejner", 0.1, 0.14, 0.08, container=True))

    # -------------------------------------------------------------------------------------------
    # Simplified trolley assembly: program
    # -------------------------------------------------------------------------------------------

    prog = Program()
    prog.header.id = 20
    prog.header.name = "Montaz stolicky"

    # --- left side of the trolley ------------------------------------------------------
    pb = ProgramBlock()
    pb.id = 1
    pb.name = "Bocnice 1"
    pb.on_success = 2
    pb.on_failure = 0
    prog.blocks.append(pb)

    pb.items.append(wait_item(2, on_failure=2))

    # each side consists of four profiles (of two types)
    pb.items.append(feeder_item(3, obj_type=""))
    pb.items.append(place_item(4, ref_id=[3], on_failure=4))

    pb.items.append(feeder_item(5, ref_id=[3]))
    pb.items.append(place_item(6, ref_id=[5], on_failure=6))

    pb.items.append(feeder_item(7, obj_type=""))
    pb.items.append(place_item(8, ref_id=[7], on_failure=8))

    pb.items.append(feeder_item(9, ref_id=[7]))
    pb.items.append(place_item(10, ref_id=[9], on_failure=10))

    # after p&p, let's drill holes
    pb.items.append(drill_item(11, on_success=11, on_failure=13,
                               obj_type=[""]))

    pb.items.append(item(13, "GetReady", on_success=0, on_failure=13))

    # --- right side of the trolley ------------------------------------------------------
    pb = deepcopy(pb)
    pb.id = 2
    pb.name = "Bocnice 2"
    pb.on_success = 3
    pb.on_failure = 0
    prog.blocks.append(pb)

    # --- connecting profiles ------------------------------------------------------------
    pb = ProgramBlock()
    pb.id = 3
    pb.name = "Spojovaci dily"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    pb.items.append(wait_item(1, on_success=10, on_failure=1))

    pb.items.append(feeder_item(10, obj_type=""))
    pb.items.append(place_item(11, ref_id=[10], on_failure=11))

    pb.items.append(feeder_item(12, ref_id=[10]))
    pb.items.append(place_item(13, ref_id=[12], on_failure=13))

    pb.items.append(feeder_item(14, ref_id=[10]))
    pb.items.append(place_item(15, ref_id=[14], on_failure=15))

    pb.items.append(feeder_item(16, ref_id=[10]))
    pb.items.append(place_item(17, ref_id=[16], on_failure=17))

    pb.items.append(item(18, "GetReady", on_success=0, on_failure=18))

    art.store_program(prog)
    art.program_set_ro(prog.header.id)
Exemplo n.º 9
0
    def srv_get_program_cb(self, request):
        resp = getProgramResponse()
        prog = Program()
        prog.header.id = request.id
        prog.header.name = "Demo"

        pb = ProgramBlock()
        pb.id = 1
        pb.name = "Demo"
        pb.on_success = 1
        pb.on_failure = 0
        prog.blocks.append(pb)

        pi = ProgramItem()
        pi.type = "PickFromBin"
        pi.object.append("bin")
        pi.id = 1
        pi.on_success = 2
        pi.on_failure = 1  # nebo GetReady a konec?
        pi.do_not_clear.append("object")
        pb.items.append(pi)

        pi = ProgramItem()
        pi.type = "VisualInspection"
        pi.ref_id.append(1)
        pi.pose.append(
            PoseStamped(header=Header(frame_id="marker"),
                        pose=Pose(position=Point(x=1),
                                  orientation=Quaternion(w=1))))
        pi.id = 2
        pi.on_success = 3
        pi.on_failure = 4
        pb.items.append(pi)

        pi = ProgramItem()
        pi.type = "PlaceToContainer"
        pi.name = "Place OK"
        pi.id = 3
        pi.on_success = 1
        pi.object.append("blue_container")
        pi.polygon.append(
            PolygonStamped(header=Header(frame_id="marker"),
                           polygon=Polygon(points=[
                               Point32(x=0, y=0),
                               Point32(x=1, y=0),
                               Point32(x=1, y=1),
                               Point32(x=0, y=1)
                           ])))
        pi.ref_id.append(1)
        pb.items.append(pi)

        pi = ProgramItem()
        pi.type = "PlaceToContainer"
        pi.name = "Place NOK"
        pi.id = 4
        pi.on_success = 1
        pi.object.append("blue_container")
        pi.polygon.append(
            PolygonStamped(header=Header(frame_id="marker"),
                           polygon=Polygon(points=[
                               Point32(x=0, y=0),
                               Point32(x=1, y=0),
                               Point32(x=1, y=1),
                               Point32(x=0, y=1)
                           ])))
        pi.ref_id.append(1)
        pb.items.append(pi)
        resp.program = prog
        resp.success = True
        return resp
Exemplo n.º 10
0
    def test_program(self):

        prog = Program()
        prog.header.id = 999
        prog.header.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
        prog.blocks.append(pb)

        p = ProgramItem()
        p.id = 1
        p.on_success = 2
        p.on_failure = 0
        p.type = "GetReady"
        pb.items.append(deepcopy(p))

        p = ProgramItem()
        p.id = 2
        p.on_success = 3
        p.on_failure = 0
        p.type = "WaitForUser"
        pb.items.append(deepcopy(p))

        p = ProgramItem()
        p.id = 3
        p.on_success = 4
        p.on_failure = 0
        p.type = "PickFromFeeder"
        p.object.append("profile_20_60")
        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 = 4
        p.on_success = 5
        p.on_failure = 0
        p.type = "PlaceToPose"
        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 = "PickFromFeeder"
        p.object.append("profile_20_60")
        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 = "GetReady"
        pb.items.append(deepcopy(p))

        p = ProgramItem()
        p.id = 7
        p.on_success = 8
        p.on_failure = 0
        p.type = "WaitUntilUserFinishes"
        pb.items.append(deepcopy(p))

        p = ProgramItem()
        p.id = 8
        p.on_success = 9
        p.on_failure = 0
        p.type = "PickFromPolygon"
        p.object.append("profile_20_60")
        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 = "PlaceToPose"
        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))

        try:
            resp_store = self.store_program_srv(program=prog)
        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.header.id, 999, "program_get")

        try:
            resp_headers = self.get_program_headers_srv(ids=[999])
        except rospy.ServiceException:
            pass

        self.assertEquals(len(resp_headers.headers), 1, "program_headers_len")
        self.assertEquals(resp_headers.headers[0].id, 999, "program_headers_id")
Exemplo n.º 11
0
    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))
Exemplo n.º 12
0
    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")
Exemplo n.º 13
0
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
Exemplo n.º 14
0
    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)
Exemplo n.º 15
0
def main(args):

    global setup

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

    try:
        setup = os.environ["ARTABLE_SETUP"]
    except KeyError:
        rospy.logerr("ARTABLE_SETUP has to be set!")
        return

    api = ArtApiHelper()
    api.wait_for_db_api()

    rospy.loginfo("Refreshing collision environment...")
    api.clear_collision_primitives(setup)

    table_width = 1.5
    table_depth = 0.7
    api.add_collision_primitive(
        makePrimitive("table", [table_width, table_depth, 0.78], z=-0.78 / 2))

    feeder_depth = 0.35
    feeder_thickness = 0.001
    feeder_front_to_table = 0.15

    # left feeder (1)
    api.add_collision_primitive(
        makePrimitive("lf-front", [feeder_depth, feeder_thickness, 0.175],
                      x=-feeder_depth / 2 - feeder_front_to_table,
                      y=table_depth - 0.495))
    api.add_collision_primitive(
        makePrimitive("lf-middle", [feeder_depth, feeder_thickness, 0.35],
                      x=-feeder_depth / 2 - feeder_front_to_table,
                      y=table_depth - 0.18))
    api.add_collision_primitive(
        makePrimitive("lf-rear", [feeder_depth, feeder_thickness, 0.35],
                      x=-feeder_depth / 2 - feeder_front_to_table,
                      y=table_depth))

    # right feeder (2)
    api.add_collision_primitive(
        makePrimitive("rf-front", [feeder_depth, feeder_thickness, 0.175],
                      x=table_width + feeder_depth / 2 + feeder_front_to_table,
                      y=table_depth - 0.495))
    api.add_collision_primitive(
        makePrimitive("rf-middle", [feeder_depth, feeder_thickness, 0.35],
                      x=table_width + feeder_depth / 2 + feeder_front_to_table,
                      y=table_depth - 0.18))
    api.add_collision_primitive(
        makePrimitive("rf-rear", [feeder_depth, feeder_thickness, 0.35],
                      x=table_width + feeder_depth / 2 + feeder_front_to_table,
                      y=table_depth))

    api.add_collision_primitive(
        makePrimitive("kinect-n1", [0.3, 0.3, 0.3],
                      x=0,
                      y=0,
                      z=0,
                      frame_id="n1_kinect2_link"))
    api.add_collision_primitive(
        makePrimitive("kinect-n2", [0.3, 0.3, 0.3],
                      x=0,
                      y=0,
                      z=0,
                      frame_id="n2_kinect2_link"))

    api.store_object_type(obj_type("Stretcher", 0.046, 0.046, 0.154))
    api.store_object_type(obj_type("ShortLeg", 0.046, 0.046, 0.298))
    api.store_object_type(obj_type("LongLeg", 0.046, 0.046, 0.398))

    api.store_object_type(obj_type("Spojka", 0.046, 0.046, 0.154))
    api.store_object_type(obj_type("Kratka_noha", 0.046, 0.046, 0.298))
    api.store_object_type(obj_type("Dlouha_noha", 0.046, 0.046, 0.398))
    api.store_object_type(
        obj_type("Modry_kontejner", 0.11, 0.165, 0.075, container=True))
    api.store_object_type(
        obj_type("Bily_kontejner_velky", 0.245, 0.37, 0.15, container=True))
    api.store_object_type(
        obj_type("Bily_kontejner_maly", 0.18, 0.245, 0.15, container=True))

    # delete all created programs
    ph = api.get_program_headers()
    if ph:
        for h in ph:
            api.program_clear_ro(h.id)
            api.delete_program(h.id)

    # TODO add parameters
    prog = Program()
    prog.header.id = 1
    prog.header.name = "MSV DEMO"

    pb = ProgramBlock()
    pb.id = 1
    pb.name = "Pick and place"
    pb.on_success = 2
    pb.on_failure = 0
    prog.blocks.append(pb)

    pi = ProgramItem()
    pi.id = 1
    pi.on_success = 2
    pi.on_failure = 0
    pi.type = "PickFromFeeder"
    pi.object.append("")
    pi.pose.append(PoseStamped())
    pb.items.append(pi)

    pi = ProgramItem()
    pi.id = 2
    pi.on_success = 3
    pi.on_failure = 0
    pi.type = "PlaceToPose"
    pi.pose.append(PoseStamped())
    pi.ref_id.append(1)
    pb.items.append(pi)

    pi = ProgramItem()
    pi.id = 3
    pi.on_success = 0
    pi.on_failure = 0
    pi.type = "GetReady"
    pb.items.append(pi)

    pb = ProgramBlock()
    pb.id = 2
    pb.name = "Inspect and sort"
    pb.on_success = 1
    pb.on_failure = 0
    prog.blocks.append(pb)

    pi = ProgramItem()
    pi.id = 1
    pi.on_success = 2
    pi.on_failure = 0
    pi.type = "PickFromPolygon"
    pi.object.append("")
    pi.polygon.append(PolygonStamped())
    pb.items.append(pi)

    pi = ProgramItem()
    pi.id = 2
    pi.on_success = 3
    pi.on_failure = 4
    pi.type = "VisualInspection"
    pi.pose.append(PoseStamped())
    pi.ref_id.append(1)
    pb.items.append(pi)

    pi = ProgramItem()
    pi.id = 3
    pi.on_success = 5
    pi.on_failure = 0
    pi.type = "PlaceToContainer"
    pi.name = "OK"
    pi.object.append("")
    pi.polygon.append(PolygonStamped())
    pi.ref_id.append(1)
    pb.items.append(pi)

    pi = ProgramItem()
    pi.id = 4
    pi.on_success = 5
    pi.on_failure = 0
    pi.type = "PlaceToContainer"
    pi.name = "NOK"
    pi.object.append("")
    pi.polygon.append(PolygonStamped())
    pi.ref_id.append(1)
    pb.items.append(pi)

    pi = ProgramItem()
    pi.id = 5
    pi.on_success = 0
    pi.on_failure = 0
    pi.type = "GetReady"
    pb.items.append(pi)

    api.store_program(prog)

    rospy.loginfo("Done!")
Exemplo n.º 16
0
    def test_program(self):

        prog = Program()
        prog.header.id = 999
        prog.header.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
        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 = 4
        p.on_failure = 0
        p.type = ProgramItem.PICK_FROM_FEEDER
        p.object.append("profile_20_60")
        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 = 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_20_60")
        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_20_60")
        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))

        try:
            resp_store = self.store_program_srv(program=prog)
        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.header.id, 999, "program_get")

        try:
            resp_headers = self.get_program_headers_srv(ids=[999])
        except rospy.ServiceException:
            pass

        self.assertEquals(len(resp_headers.headers), 1, "program_headers_len")
        self.assertEquals(resp_headers.headers[0].id, 999, "program_headers_id")