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