Exemplo n.º 1
0
def obj_type(type_name, bbx, bby, bbz):

    ot = ObjectType()
    ot.name = type_name
    ot.bbox.type = SolidPrimitive.BOX
    ot.bbox.dimensions = [bbx, bby, bbz]
    return ot
Exemplo n.º 2
0
def obj_type(type_name, bbx, bby, bbz, container=False):

    ot = ObjectType()
    ot.name = type_name
    ot.bbox.type = SolidPrimitive.BOX
    ot.bbox.dimensions = [bbx, bby, bbz]
    ot.container = container
    return ot
Exemplo n.º 3
0
    def setUp(self):

        self.ui_core = UICore(0, 0, 2, 1, 1000, 1234)

        self.type1 = ObjectType()
        self.type1.name = "type1"
        self.type1.bbox.type = SolidPrimitive.BOX
        self.type1.bbox.dimensions = [0.1, 0.1, 0.1]

        self.type2 = ObjectType()
        self.type2.name = "type2"
        self.type2.bbox.type = SolidPrimitive.BOX
        self.type2.bbox.dimensions = [0.1, 0.1, 0.1]

        self.ps = PoseStamped()
        self.ps.pose.orientation.w = 1.0
Exemplo n.º 4
0
 def srv_get_object_cb(self, request):
     return getObjectTypeResponse(success=True,
                                  object_type=ObjectType(
                                      name=request.name,
                                      container=True,
                                      bbox=SolidPrimitive(
                                          type=1,
                                          dimensions=[0.1, 0.1, 0.1])))
Exemplo n.º 5
0
    def test_object_type(self):

        ot = ObjectType()
        ot.name = "profile_test_1"
        ot.bbox.type = SolidPrimitive.BOX
        ot.bbox.dimensions.append(0.1)
        ot.bbox.dimensions.append(0.1)
        ot.bbox.dimensions.append(0.1)

        try:
            resp_store = self.store_object_srv(ot)
        except rospy.ServiceException:
            pass

        self.assertEquals(resp_store.success, True, "object_type_store")

        try:
            resp_get = self.get_object_srv(name="profile_test_1")
        except rospy.ServiceException:
            pass

        self.assertEquals(resp_get.success, True, "object_type_get")
        self.assertEquals(resp_get.object_type.name, "profile_test_1", "object_type_get")
Exemplo n.º 6
0
    def test_object_type(self):

        ot = ObjectType()
        ot.name = "profile_test_1"
        ot.bbox.type = SolidPrimitive.BOX
        ot.bbox.dimensions.append(0.1)
        ot.bbox.dimensions.append(0.1)
        ot.bbox.dimensions.append(0.1)

        try:
            resp_store = self.store_object_srv(ot)
        except rospy.ServiceException:
            pass

        self.assertEquals(resp_store.success, True, "object_type_store")

        try:
            resp_get = self.get_object_srv(name="profile_test_1")
        except rospy.ServiceException:
            pass

        self.assertEquals(resp_get.success, True, "object_type_get")
        self.assertEquals(resp_get.object_type.name, "profile_test_1", "object_type_get")
Exemplo n.º 7
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.º 8
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.º 9
0
        print "Service call failed: " + str(e)
        return

    rospy.wait_for_service('/art/db/program/get')

    try:
        get_program_srv = rospy.ServiceProxy('/art/db/program/get', getProgram)
        resp = get_program_srv(id=0)
        print resp
    except rospy.ServiceException, e:
        print "Service call failed: " + str(e)
        return

    rospy.wait_for_service('/art/db/object_type/store')

    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)

    try:
        store_object_srv = rospy.ServiceProxy('/art/db/object_type/store', storeObjectType)
        resp = store_object_srv(ot)
    except rospy.ServiceException, e:
        print "Service call failed: " + str(e)
        return

    ot.name = "profile_20_80"