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 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!")
