"""ЗАМКНУТЫЙ КОНТУР: товары едут, CV определяет класс на лету, пушер и плуг реагируют. Отличие от прежнего прогона с заранее заданными классами: класс здесь НЕ читается из разметки, а приходит от стереопайплайна во время движения. Разметка используется только для подсчёта ошибки в конце. Как замыкается контур. torch внутри Isaac роняет процесс, поэтому CV живёт отдельным процессом (cv_worker.py), а обмен идёт через каталог runtime/: при пересечении ворот осмотра сцена рендерит шесть кадров и кладёт заявку, работник отвечает файлом с классом. Времени хватает с большим запасом: от ворот (x = -0.750) до пушера (-3.900) товар едет 3.15 с, до плуга (-7.85) - 7.1 с, инференс - 469 мс. Механика та же, что проверена раньше: * пушер берёт класс D - нож двигается записью трансформа (Cell.blade_to), его призматический сустав выключен, иначе сустав и скрипт тянут нож в разные стороны; * плуг ставится ЗАРАНЕЕ по классу (B - в одну сторону, C - в другую), лезвие кинематическое, шарнир выключен: силовой привод в этой сборке не держал угол и звенел. """ import sys, os, math, json, time, glob CT = "/home/dasha/robozon-sorter/control_test" REPO = "/home/dasha/robozon-sorter" for p in (CT, REPO): if p not in sys.path: sys.path.insert(0, p) import numpy as np import omni.usd, omni.timeline import omni.kit.viewport.utility as vp import isaacsim.core.experimental.utils.app as app_utils from pxr import Gf, Usd, UsdGeom, UsdPhysics, PhysxSchema, UsdShade from isaacsim.core.experimental.prims import RigidPrim import importlib import cell as cellmod importlib.reload(cellmod) import classify as CL import cam_configs as CC from robozon_sorter import config as C from robozon_sorter.sim.mechanics import Cell from robozon_sorter.sim.plow import Plow SPEED, PITCH = 1.0, 0.70 GATE_X = float(CC.TARGET[0]) RT = f"{CT}/runtime" REQ, RES, SHOTS = f"{RT}/req", f"{RT}/res", f"{RT}/shots" for d in (RT, REQ, RES, SHOTS): os.makedirs(d, exist_ok=True) for f in glob.glob(f"{REQ}/*") + glob.glob(f"{RES}/*"): os.remove(f) ITEMS = ["bag", "backpack", "lunchbox", "helmet", "pillow", "detergent", "bucket", "box_400x400x300", "box_300x200x200"] RIGS = ["RealSense_D435", "Orbbec_Gemini305", "Orbbec_Gemini345"] tl = omni.timeline.get_timeline_interface() if tl.is_playing(): tl.stop(); await app_utils.update_app_async(steps=10) omni.usd.get_context().open_stage(str(cellmod.SCENE)) await app_utils.update_app_async(steps=60) stage = omni.usd.get_context().get_stage() info = await cellmod.prepare(stage, belt_speed=SPEED, script_control=True) print(f"ячейка готова: парковок обезврежено {info.get('parks_cleared')}, лент {len(info['belts'])}") fps = stage.GetTimeCodesPerSecond() or 60.0 stage.SetEndTimeCode(stage.GetStartTimeCode() + fps * 180.0) tl.set_end_time(float(stage.GetEndTimeCode()) / fps) tl.set_looping(False) lib = {i["name"]: i for i in CL.load_library(f"{CT}/items_flow") if "cls" in i} bb = UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_, UsdGeom.Tokens.render]) w = vp.get_active_viewport(); orig_cam = w.camera_path ROOT = "/World/_CV" if stage.GetPrimAtPath(ROOT).IsValid(): stage.RemovePrim(ROOT) stage.DefinePrim(ROOT, "Xform") def spawn(name, i): """товар из items_flow по ПРОВЕРЕННОМУ пути загрузки (как в flow_capture/run_pipeline). Три вещи, которые я сделал неверно в первой версии и из-за которых поток не поехал: меши items_flow УЖЕ в каталожном масштабе и УЖЕ посажены на z=0 (scale_items.py), так что домасштабирование лишнее; коллайдеры в них УЖЕ есть, свои накладывать поверх нельзя; и рождаться товар должен на парковке, а к ленте переноситься в момент выпуска. """ path = f"{ROOT}/{name}" if stage.GetPrimAtPath(path).IsValid(): stage.RemovePrim(path) prim = UsdGeom.Xform.Define(stage, path).GetPrim() prim.GetReferences().ClearReferences() prim.GetReferences().AddReference(lib[name]["path"]) xf = UsdGeom.Xformable(prim); xf.ClearXformOpOrder() op = xf.AddTranslateOp(precision=UsdGeom.XformOp.PrecisionDouble) op.Set(Gf.Vec3d(9.0 + 1.5 * i, 5.0, 0.4)) # парковка вне линии UsdPhysics.RigidBodyAPI.Apply(prim) UsdPhysics.RigidBodyAPI(prim).CreateKinematicEnabledAttr().Set(False) UsdPhysics.MassAPI.Apply(prim).CreateMassAttr().Set(0.6) px = PhysxSchema.PhysxRigidBodyAPI.Apply(prim) px.CreateEnableCCDAttr().Set(True) px.CreateSolverPositionIterationCountAttr().Set(24) px.CreateSolverVelocityIterationCountAttr().Set(8) px.CreateSleepThresholdAttr().Set(0.0) px.CreateMaxDepenetrationVelocityAttr().Set(C.MAX_DEPENETRATION) for d in Usd.PrimRange(prim): if d.HasAPI(UsdPhysics.CollisionAPI): pc = PhysxSchema.PhysxCollisionAPI.Apply(d) pc.CreateContactOffsetAttr().Set(0.004) pc.CreateRestOffsetAttr().Set(0.001) im = UsdGeom.Imageable(d) if im: im.MakeVisible() return prim, op async def compose(prim): for _ in range(4): await app_utils.update_app_async(steps=2) for d in Usd.PrimRange(prim): im = UsdGeom.Imageable(d) if im: im.MakeVisible() async def shoot(name): """шесть кадров в кроп-разрешении сцены; таймлайн на это время ставится на паузу""" files = {} was = tl.is_playing() if was: tl.pause() for rig in RIGS: for eye in ("Left", "Right"): cam = f"/RigRS/{rig}_{eye}" if not stage.GetPrimAtPath(cam).IsValid(): continue w.camera_path = cam await app_utils.update_app_async(steps=6) f = f"{RT}/frames/{name}__{rig}_{eye}.png" os.makedirs(os.path.dirname(f), exist_ok=True) vp.capture_viewport_to_file(w, file_path=f) await app_utils.update_app_async(steps=4) files[f"{rig}_{eye}"] = f w.camera_path = orig_cam await app_utils.update_app_async(steps=3) if was: tl.play() return files print("ожидаю работника CV...") for _ in range(600): if os.path.exists(f"{RT}/worker_ready"): break await app_utils.update_app_async(steps=6) print("работник готов:", os.path.exists(f"{RT}/worker_ready")) cell = Cell(stage, items={}) plow = Plow(stage); plow.target(C.PLOW_PRESET["D"]) tl.play(); await app_utils.update_app_async(steps=20) t0 = float(tl.get_current_time()) state = {} # name -> dict(prim, rp, shot, cls, pushed, plowed, gt) order, nxt = [], 0.0 pusher_busy = -1.0 print(f"\nпоток: шаг {PITCH*1000:.0f} мм при {SPEED} м/с, ворота x={GATE_X:+.3f}, " f"пушер x={C.PUSH_X:+.3f}, плуг x={C.PLOW_X:+.3f}\n") while float(tl.get_current_time()) - t0 < 90.0: t = float(tl.get_current_time()) - t0 if len(order) < len(ITEMS) and t >= nxt: nm = ITEMS[len(order)] prim, op = spawn(nm, len(order)) for _ in range(3): await app_utils.update_app_async(steps=2) # ссылка компонуется не в тот же тик op.Set(Gf.Vec3d(cellmod.ENTRY_X, cellmod.ENTRY_Y, C.BELT_Z + 0.02)) rp = RigidPrim(paths=[str(prim.GetPath())]) state[nm] = dict(prim=prim, rp=rp, shot=False, cls=None, pushed=False, plowed=False, gt=lib[nm]["cls"], t_gate=None, t_cls=None) order.append(nm); nxt += PITCH / SPEED print(f" {t:5.2f}s выпущен {nm}") # ворота осмотра: снять кадры и отправить заявку for nm in order: st = state[nm] if st["shot"]: continue x = float(st["rp"].get_world_poses()[0].numpy()[0][0]) if x <= GATE_X + 0.02: st["shot"] = True; st["t_gate"] = t files = await shoot(nm) json.dump(dict(name=nm, files=files), open(f"{REQ}/{nm}.json", "w")) print(f" {t:5.2f}s {nm} на воротах x={x:+.3f} - заявка отправлена") # ответы от работника for f in glob.glob(f"{RES}/*.json"): try: a = json.load(open(f)) except Exception: continue nm = a["name"] if nm in state and state[nm]["cls"] is None: state[nm]["cls"] = a.get("cls") state[nm]["dims"] = a.get("dims"); state[nm]["k"] = a.get("k") state[nm]["t_cls"] = t lat = (t - (state[nm]["t_gate"] or t)) print(f" {t:5.2f}s {nm} -> класс {a.get('cls')} (эталон {state[nm]['gt']}), " f"dims={a.get('dims')}, задержка {lat:.2f}с, инференс {a.get('ms')}мс") os.remove(f) # пушер берёт класс D if t > pusher_busy: for nm in order: st = state[nm] if st["cls"] != "D" or st["pushed"]: continue x = float(st["rp"].get_world_poses()[0].numpy()[0][0]) if x <= C.PUSH_X + 0.06: print(f" {t:5.2f}s ПУШЕР берёт {nm} (класс D) на x={x:+.3f}") await cell.stroke(app_utils, out=True, speed=1.2) await cell.stroke(app_utils, out=False, speed=1.5) st["pushed"] = True pusher_busy = float(tl.get_current_time()) - t0 + 0.2 break # плуг ставится заранее по классу B/C for nm in order: st = state[nm] if st["cls"] not in ("B", "C") or st["plowed"]: continue x = float(st["rp"].get_world_poses()[0].numpy()[0][0]) if x <= C.PLOW_X + 1.2: ang = C.PLOW_PRESET[st["cls"]] plow.target(ang) st["plowed"] = True print(f" {t:5.2f}s ПЛУГ на {ang:+.0f}° под {nm} (класс {st['cls']}) x={x:+.3f}") await app_utils.update_app_async(steps=3) # итог print(f"\n {'товар':18s} {'эталон':>7s} {'предсказано':>12s} {'габариты, мм':>20s} " f"{'пушер':>6s} {'плуг':>5s} {'конец X,Y':>16s}") print(" " + "-" * 96) rows = [] for nm in order: st = state[nm] q = st["rp"].get_world_poses()[0].numpy()[0] ok = "верно" if st["cls"] == st["gt"] else "ОШИБКА" print(f" {nm:18s} {st['gt']:>7s} {str(st['cls'])+' '+ok:>12s} " f"{str(st.get('dims')):>20s} {'да' if st['pushed'] else '-':>6s} " f"{'да' if st['plowed'] else '-':>5s} ({float(q[0]):+6.2f},{float(q[1]):+6.2f})") rows.append(dict(name=nm, gt=st["gt"], pred=st["cls"], dims=st.get("dims"), k=st.get("k"), pushed=st["pushed"], plowed=st["plowed"], end=[round(float(q[0]), 2), round(float(q[1]), 2)], lat=None if st["t_cls"] is None or st["t_gate"] is None else round(st["t_cls"] - st["t_gate"], 2))) got = [r for r in rows if r["pred"]] acc = sum(1 for r in got if r["pred"] == r["gt"]) print(f"\n классов получено {len(got)}/{len(rows)}, верно {acc}/{len(got)}") lats = [r["lat"] for r in got if r["lat"] is not None] if lats: print(f" задержка от ворот до класса: медиана {np.median(lats):.2f} с " f"(до пушера {abs(C.PUSH_X-GATE_X)/SPEED:.2f} с, до плуга {abs(C.PLOW_X-GATE_X)/SPEED:.2f} с)") json.dump(rows, open(f"{RT}/sorting_cv.json", "w"), indent=1, ensure_ascii=False) # обзорный кадр w.camera_path = orig_cam await app_utils.update_app_async(steps=30) vp.capture_viewport_to_file(w, file_path=f"{SHOTS}/overview.png") await app_utils.update_app_async(steps=30) tl.stop(); await app_utils.update_app_async(steps=5) print(f"\n -> {RT}/sorting_cv.json, кадры в {SHOTS}")