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)
