class TestProgramHelper(unittest.TestCase):
def setUp(self):
# TODO add more blocks
self.prog = Program()
self.ph = ProgramHelper()
self.prog.header.id = 666
self.prog.header.name = "Basic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
self.prog.blocks.append(pb)
p = ProgramItem()
p.id = 1
p.on_success = 2
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 2
p.on_success = 3
p.on_failure = 0
p.type = ProgramItem.WAIT_FOR_USER
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 3
p.on_success = 31
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_FEEDER
p.object.append("profile")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 31
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.VISUAL_INSPECTION
p.ref_id.append(3)
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
pf.pose.position.z = 0.1
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 4
p.on_success = 5
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(3)
p.ref_id.append(5)
pp = PoseStamped()
pp.header.frame_id = "marker"
pp.pose.position.x = 0.75
pp.pose.position.y = 0.5
p.pose.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 5
p.on_success = 6
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_FEEDER
p.object.append("profile")
pf = PoseStamped()
pf.header.frame_id = "marker"
pf.pose.position.x = 0.75
pf.pose.position.y = 0.5
p.pose.append(pf)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 6
p.on_success = 7
p.on_failure = 0
p.type = ProgramItem.GET_READY
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 7
p.on_success = 8
p.on_failure = 0
p.type = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 8
p.on_success = 9
p.on_failure = 0
p.type = ProgramItem.PICK_FROM_POLYGON
p.object.append("profile")
pp = PolygonStamped()
pp.header.frame_id = "marker"
pp.polygon.points.append(Point32(0.4, 0.1, 0))
pp.polygon.points.append(Point32(1.0, 0.1, 0))
pp.polygon.points.append(Point32(1.0, 0.6, 0))
pp.polygon.points.append(Point32(0.4, 0.6, 0))
p.polygon.append(pp)
pb.items.append(deepcopy(p))
p = ProgramItem()
p.id = 9
p.on_success = 4
p.on_failure = 0
p.type = ProgramItem.PLACE_TO_POSE
p.ref_id.append(8)
pp = PoseStamped()
pp.header.frame_id = "marker"
pp.pose.position.x = 0.75
pp.pose.position.y = 0.5
p.pose.append(pp)
pb.items.append(deepcopy(p))
def test_alt_prog_item(self):
prog = Program()
prog.header.id = 123
pb = ProgramBlock()
pb.id = 10
prog.blocks.append(pb)
pi = ProgramItem()
pi.id = 1
pi.type = ProgramItem.PICK_FROM_FEEDER
pi.object.append("obj1")
pi.pose.append(PoseStamped())
pi.on_success = 2
pb.items.append(pi)
pi = ProgramItem()
pi.id = 2
pi.type = ProgramItem.PLACE_TO_GRID
pi.polygon.append(PolygonStamped())
pi.ref_id.append(1)
for i in range(0, 3):
pi.pose.append(PoseStamped())
pi.on_success = 1
pi.on_failure = 3
pb.items.append(pi)
pi = ProgramItem()
pi.id = 3
pi.type = ProgramItem.DRILL_POINTS
pi.ref_id.append(2)
pi.pose.append(PoseStamped())
pi.on_success = 3
pi.on_failure = 1
pb.items.append(pi)
res = self.ph.load(prog)
self.assertEquals(res, True, "alt program")
ret = self.ph.get_object(10, 3)
self.assertEquals(ret[0][0], "obj1", "ref_id object")
self.assertEquals(ret[1], 1, "ref_id object - source item id")
ret = self.ph.get_polygon(10, 3)
self.assertEquals(len(ret[0]), 1, "ref_id polygon")
self.assertEquals(ret[1], 2, "ref_id polygon - source item id")
self.assertRaises(ValueError, self.ph.get_polygon, 10, 1)
def test_empty_program(self):
res = self.ph.load(Program())
self.assertEquals(res, False, "empty program")
def test_empty_block(self):
prog = Program()
prog.blocks.append(ProgramBlock())
res = self.ph.load(prog)
self.assertEquals(res, False, "empty block")
def test_invalid_block_id(self):
prog = deepcopy(self.prog)
prog.blocks[0].id = 0
res = self.ph.load(prog)
self.assertEquals(res, False, "invalid block id")
def test_invalid_item_id(self):
prog = deepcopy(self.prog)
prog.blocks[0].items[0].id = 0
res = self.ph.load(prog)
self.assertEquals(res, False, "invalid item id")
def test_invalid_item_on_success(self):
prog = deepcopy(self.prog)
prog.blocks[0].items[1].on_success = 1234
res = self.ph.load(prog)
self.assertEquals(res, False, "invalid item on_success")
def test_invalid_item_on_failure(self):
prog = deepcopy(self.prog)
prog.blocks[0].items[1].on_failure = 1234
res = self.ph.load(prog)
self.assertEquals(res, False, "invalid item on_failure")
def test_valid_program(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "valid program")
def test_on_success(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "on_success")
(block_id, item_id) = self.ph.get_id_on_success(1, 2)
self.assertEquals(block_id, 1, "on_success - block_id")
self.assertEquals(item_id, 3, "on_success - item_id")
def test_on_failure(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "on_failure")
(block_id, item_id) = self.ph.get_id_on_failure(1, 4)
self.assertEquals(block_id, 0, "on_failure - block_id")
self.assertEquals(item_id, 0, "on_failure - item_id")
def test_get_item_msg(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_item_msg")
msg = self.ph.get_item_msg(1, 4)
self.assertEquals(isinstance(msg, ProgramItem), True,
"get_item_msg - type")
self.assertEquals(msg.id, 4, "get_item_msg - id")
def test_get_block_msg(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_block_msg")
msg = self.ph.get_block_msg(1)
self.assertEquals(isinstance(msg, ProgramBlock), True,
"get_block_msg - type")
self.assertEquals(msg.id, 1, "get_block_msg - id")
def test_get_first_block_id(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_first_block_id")
block_id = self.ph.get_first_block_id()
self.assertEquals(block_id, self.prog.blocks[0].id,
"get_first_block_id")
def test_get_first_item_id(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_first_block_id")
(block_id, item_id) = self.ph.get_first_item_id()
self.assertEquals(block_id, self.prog.blocks[0].id,
"get_first_item_id - block")
self.assertEquals(item_id, self.prog.blocks[0].items[0].id,
"get_first_item_id - item")
def test_get_program_id(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_program_id")
prog_id = self.ph.get_program_id()
self.assertEquals(prog_id, 666, "get_program_id - id")
def test_template(self):
self.ph.load(self.prog)
self.assertEquals(self.ph.program_learned(), True, "test_template")
self.ph.load(self.prog, True)
self.assertEquals(self.ph.program_learned(), False, "test_template")
def test_invalid_ref_id(self):
prog = deepcopy(self.prog)
prog.blocks[0].items[0].ref_id.append(6587)
res = self.ph.load(prog)
self.assertEquals(res, False, "test_invalid_ref_id")
def main(args):
if len(args) < 2:
print "This script needs program id as argument."
return
rospy.init_node('plot_program', anonymous=True)
art = ArtApiHelper()
ph = ProgramHelper()
prog = art.load_program(int(args[1]))
if prog is None:
print "Failed to load program"
return
if not ph.load(prog):
print "Faulty program"
return
A = pgv.AGraph(label="<<B>Program ID: " + str(prog.header.id) + "<br/>" +
escape(prog.header.name) + "</B>>",
directed=True,
strict=False)
A.graph_attr['outputorder'] = 'edgesfirst'
A.add_node("start", label="Program start")
A.add_node("b0i0", label="Program end")
A.add_edge("start", get_node_name(*ph.get_first_item_id()))
for block_id in ph.get_block_ids():
block_msg = ph.get_block_msg(block_id)
ids = []
for item_id in ph.get_items_ids(block_id):
nn = get_node_name(block_id, item_id)
item_msg = ph.get_item_msg(block_id, item_id)
osn = get_node_name(*ph.get_id_on_success(block_id, item_id))
ofn = get_node_name(*ph.get_id_on_failure(block_id, item_id))
A.add_edge(nn, osn, color="green", constraint=True)
A.add_edge(nn, ofn, color="red", constraint=True)
for ref in item_msg.ref_id:
A.add_edge(get_node_name(block_id, ref),
nn,
color="gray",
style="dashed",
key="ref_" + nn + "_" +
get_node_name(block_id, ref))
A.get_node(nn).attr['label'] = 'Item ID: ' + str(
item_id) + '\n' + get_type_string(item_msg)
A.get_node(nn).attr['shape'] = 'box'
A.get_node(nn).attr['style'] = 'rounded'
ids.append(nn)
sg = A.subgraph(name="cluster_" + str(block_id),
label="<<b>Group ID: " + str(block_id) + "<br/>" +
escape(block_msg.name) + "</b>>",
color="gray")
sg.add_nodes_from(ids)
A.layout(prog='dot')
A.draw('output.pdf')
class TestProgramHelper(unittest.TestCase):
def setUp(self):
# TODO add more blocks
self.prog = Program()
self.ph = ProgramHelper()
self.prog.id = 666
self.prog.name = "Basic pick&place"
pb = ProgramBlock()
pb.id = 1 # can't be zero
pb.name = "First block"
pb.on_success = 1
pb.on_failure = 0
self.prog.blocks.append(pb)
p0 = ProgramItem()
p0.id = 1
p0.on_success = 2
p0.on_failure = 0
p0.type = ProgramItem.GET_READY
pb.items.append(p0)
p1 = ProgramItem()
p1.id = 2
p1.on_success = 3
p1.on_failure = 0
p1.type = ProgramItem.WAIT
p1.spec = ProgramItem.WAIT_FOR_USER
pb.items.append(p1)
p2 = ProgramItem()
p2.id = 3
p2.on_success = 4
p2.on_failure = 0
p2.type = ProgramItem.MANIP_PICK_PLACE
p2.spec = ProgramItem.MANIP_TYPE
p2.object = "profile"
p2.place_pose.header.frame_id = "marker"
p2.place_pose.pose.position.x = 0.75
p2.place_pose.pose.position.y = 0.5
pb.items.append(p2)
p3 = ProgramItem()
p3.id = 4
p3.on_success = 5
p3.on_failure = 0
p3.type = ProgramItem.WAIT
p3.spec = ProgramItem.WAIT_UNTIL_USER_FINISHES
pb.items.append(p3)
p4 = ProgramItem()
p4.id = 5
p4.on_success = 0
p4.on_failure = 0
p4.type = ProgramItem.MANIP_PICK_PLACE
p4.spec = ProgramItem.MANIP_TYPE
p4.object = "profile"
p4.pick_pose = p2.place_pose
p4.place_pose.header.frame_id = "marker"
p4.place_pose.pose.position.x = 0.25
p4.place_pose.pose.position.y = 0.5
pb.items.append(p4)
def test_empty_program(self):
res = self.ph.load(Program())
self.assertEquals(res, False, "empty program")
def test_empty_block(self):
prog = Program()
prog.blocks.append(ProgramBlock())
res = self.ph.load(prog)
self.assertEquals(res, False, "empty block")
def test_invalid_block_id(self):
self.prog.blocks[0].id = 0
res = self.ph.load(self.prog)
self.assertEquals(res, False, "invalid block id")
def test_invalid_item_id(self):
self.prog.blocks[0].items[0].id = 0
res = self.ph.load(self.prog)
self.assertEquals(res, False, "invalid item id")
def test_invalid_item_on_success(self):
self.prog.blocks[0].items[1].on_success = 1234
res = self.ph.load(self.prog)
self.assertEquals(res, False, "invalid item on_success")
def test_invalid_item_on_failure(self):
self.prog.blocks[0].items[1].on_failure = 1234
res = self.ph.load(self.prog)
self.assertEquals(res, False, "invalid item on_failure")
def test_valid_program(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "valid program")
def test_on_success(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "on_success")
(block_id, item_id) = self.ph.get_id_on_success(1, 2)
self.assertEquals(block_id, 1, "on_success - block_id")
self.assertEquals(item_id, 3, "on_success - item_id")
def test_on_failure(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "on_failure")
(block_id, item_id) = self.ph.get_id_on_failure(1, 4)
self.assertEquals(block_id, 0, "on_failure - block_id")
self.assertEquals(item_id, 0, "on_failure - item_id")
def test_get_item_msg(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_item_msg")
msg = self.ph.get_item_msg(1, 4)
self.assertEquals(isinstance(msg, ProgramItem), True, "get_item_msg - type")
self.assertEquals(msg.id, 4, "get_item_msg - id")
def test_get_block_msg(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_block_msg")
msg = self.ph.get_block_msg(1)
self.assertEquals(isinstance(msg, ProgramBlock), True, "get_block_msg - type")
self.assertEquals(msg.id, 1, "get_block_msg - id")
def test_get_first_block_id(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_first_block_id")
block_id = self.ph.get_first_block_id()
self.assertEquals(block_id, self.prog.blocks[0].id, "get_first_block_id")
def test_get_first_item_id(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_first_block_id")
(block_id, item_id) = self.ph.get_first_item_id()
self.assertEquals(block_id, self.prog.blocks[0].id, "get_first_item_id - block")
self.assertEquals(item_id, self.prog.blocks[0].items[0].id, "get_first_item_id - item")
def test_get_program_id(self):
res = self.ph.load(self.prog)
self.assertEquals(res, True, "get_program_id")
prog_id = self.ph.get_program_id()
self.assertEquals(prog_id, 666, "get_program_id - id")
