def test_alt_prog_item(self):
prog = Program()
prog.header.id = 123
pb = ProgramBlock()
pb.id = 10
prog.blocks.append(pb)
pi = ProgramItem()
pi.id = 1
pi.type = ProgramItem.PICK_FROM_FEEDER
pi.object.append("obj1")
pi.pose.append(PoseStamped())
pi.on_success = 2
pb.items.append(pi)
pi = ProgramItem()
pi.id = 2
pi.type = ProgramItem.PLACE_TO_GRID
pi.polygon.append(PolygonStamped())
pi.ref_id.append(1)
for i in range(0, 3):
pi.pose.append(PoseStamped())
pi.on_success = 1
pi.on_failure = 3
pb.items.append(pi)
pi = ProgramItem()
pi.id = 3
pi.type = ProgramItem.DRILL_POINTS
pi.ref_id.append(2)
pi.pose.append(PoseStamped())
pi.on_success = 3
pi.on_failure = 1
pb.items.append(pi)
res = self.ph.load(prog)
self.assertEquals(res, True, "alt program")
ret = self.ph.get_object(10, 3)
self.assertEquals(ret[0][0], "obj1", "ref_id object")
self.assertEquals(ret[1], 1, "ref_id object - source item id")
ret = self.ph.get_polygon(10, 3)
self.assertEquals(len(ret[0]), 1, "ref_id polygon")
self.assertEquals(ret[1], 2, "ref_id polygon - source item id")
self.assertRaises(ValueError, self.ph.get_polygon, 10, 1)
def test_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)
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
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)
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))
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)
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!")
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)
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
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")
def setUp(self):
# TODO add more blocks
self.prog = Program()
self.ph = ProgramHelper()
self.prog.header.id = 666
self.prog.header.name = "Basic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
self.prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = ProgramItem.WAIT_FOR_USER
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 31
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_FEEDER
p.object.append("profile")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 31
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.VISUAL_INSPECTION
p.ref_id.append(3)
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
pf.pose.position.z = 0.1
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 5
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(3)
p.ref_id.append(5)
pp = PoseStamped()
pp.header.frame_id = "marker"
pp.pose.position.x = 0.75
pp.pose.position.y = 0.5
p.pose.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 5
p.on_success = 6
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_FEEDER
p.object.append("profile")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 6
p.on_success = 7
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 7
p.on_success = 8
p.on_failure = 0
p.type = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 8
p.on_success = 9
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_POLYGON
p.object.append("profile")
pp = PolygonStamped()
pp.header.frame_id = "marker"
pp.polygon.points.append(Point32(0.4, 0.1, 0))
pp.polygon.points.append(Point32(1.0, 0.1, 0))
pp.polygon.points.append(Point32(1.0, 0.6, 0))
pp.polygon.points.append(Point32(0.4, 0.6, 0))
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 9
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(8)
pp = PoseStamped()
pp.header.frame_id = "marker"
pp.pose.position.x = 0.75
pp.pose.position.y = 0.5
p.pose.append(pp)
pb.items.append(deepcopy(p))
def test_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")
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
def setUp(self):
# TODO add more blocks
self.prog = Program()
self.ph = ProgramHelper()
self.prog.id = 666
self.prog.name = "Basic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
self.prog.blocks.append(pb)
p0 = ProgramItem()
p0.id = 1
p0.on_success = 2
p0.on_failure = 0
p0.type = ProgramItem.GET_READY
pb.items.append(p0)
p1 = ProgramItem()
p1.id = 2
p1.on_success = 3
p1.on_failure = 0
p1.type = ProgramItem.WAIT
p1.spec = ProgramItem.WAIT_FOR_USER
pb.items.append(p1)
p2 = ProgramItem()
p2.id = 3
p2.on_success = 4
p2.on_failure = 0
p2.type = ProgramItem.MANIP_PICK_PLACE
p2.spec = ProgramItem.MANIP_TYPE
p2.object = "profile"
p2.place_pose.header.frame_id = "marker"
p2.place_pose.pose.position.x = 0.75
p2.place_pose.pose.position.y = 0.5
pb.items.append(p2)
p3 = ProgramItem()
p3.id = 4
p3.on_success = 5
p3.on_failure = 0
p3.type = ProgramItem.WAIT
p3.spec = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(p3)
p4 = ProgramItem()
p4.id = 5
p4.on_success = 0
p4.on_failure = 0
p4.type = ProgramItem.MANIP_PICK_PLACE
p4.spec = ProgramItem.MANIP_TYPE
p4.object = "profile"
p4.pick_pose = p2.place_pose
p4.place_pose.header.frame_id = "marker"
p4.place_pose.pose.position.x = 0.25
p4.place_pose.pose.position.y = 0.5
pb.items.append(p4)
def 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!")
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")