From ec5e91513f2b4e87d19dbc9a331c64a63874591e Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Mon, 1 Jun 2026 18:11:25 +0900 Subject: [PATCH 01/88] Enable camera-free color discrimination checks Add an offline HSV classifier and regression gate so dispenser color logic can be tested from synthetic patches or saved image crops before camera bringup. Constraint: Current re-experiment has no live camera, and robot motion must stay out of the color-classification gate. Rejected: Testing color discrimination only through live camera/ROS nodes | unavailable and couples perception validation to hardware. Confidence: high Scope-risk: narrow Directive: Keep this path perception-only; robot execution belongs in a separate gated integration branch. Tested: python3 -m py_compile tools/perception/color_discrimination.py tools/perception/offline_color_discrimination_test.py tools/checks/check_offline_color_discrimination.py; python3 tools/checks/check_offline_color_discrimination.py Not-tested: Real dispenser images and live camera lighting conditions. --- .gitignore | 3 + docs/offline_color_discrimination.md | 71 +++++++++ .../check_offline_color_discrimination.py | 46 ++++++ tools/perception/color_discrimination.py | 146 ++++++++++++++++++ .../offline_color_discrimination_test.py | 129 ++++++++++++++++ 5 files changed, 395 insertions(+) create mode 100644 docs/offline_color_discrimination.md create mode 100755 tools/checks/check_offline_color_discrimination.py create mode 100755 tools/perception/color_discrimination.py create mode 100755 tools/perception/offline_color_discrimination_test.py diff --git a/.gitignore b/.gitignore index 8c7227b..654b369 100644 --- a/.gitignore +++ b/.gitignore @@ -35,3 +35,6 @@ tmp/ *.pth *.onnx *.tflite + +# Local offline experiment outputs +outputs/ diff --git a/docs/offline_color_discrimination.md b/docs/offline_color_discrimination.md new file mode 100644 index 0000000..efd3e2b --- /dev/null +++ b/docs/offline_color_discrimination.md @@ -0,0 +1,71 @@ +# Offline dispenser color discrimination test + +This branch adds a camera-free test path for dispenser/cocktail color classification. +It is meant for re-experimenting when the RealSense/camera is not available. + +## What it tests + +- HSV median color classification for: + - red + - orange + - yellow + - green + - blue + - purple + - black + - white +- Center-crop median HSV logic to avoid noisy borders or overlays. +- Optional saved-image crop evaluation from CSV. + +This is perception-only. It does not run ROS camera subscribers, MoveIt, gripper, or robot motion commands. + +## Quick synthetic regression + +```bash +cd /home/ssu/Azas +python3 tools/checks/check_offline_color_discrimination.py +``` + +Expected result: + +```text +[PASS] offline HSV color discrimination works without camera +``` + +Outputs: + +```text +outputs/color_discrimination/color_discrimination_results.csv +outputs/color_discrimination/preview/*.png +``` + +## Test saved images without a camera + +Create a CSV such as `outputs/color_discrimination/manual_boxes.csv`: + +```csv +image_path,expected_color,x1,y1,x2,y2 +/path/to/image.png,red,100,80,180,160 +/path/to/image.png,blue,210,80,290,160 +``` + +Run: + +```bash +python3 tools/perception/offline_color_discrimination_test.py \ + --box-csv outputs/color_discrimination/manual_boxes.csv +``` + +The output CSV includes: + +- expected color +- predicted color +- median HSV +- confidence +- preview crop path + +## Why this helps + +For the real robot project, live camera bringup and robot motion should be separate gates. +This offline test verifies the deterministic color classifier first, using synthetic +patches or saved images, before connecting any camera or robot pipeline. diff --git a/tools/checks/check_offline_color_discrimination.py b/tools/checks/check_offline_color_discrimination.py new file mode 100755 index 0000000..d36c2ff --- /dev/null +++ b/tools/checks/check_offline_color_discrimination.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +"""Static/offline regression gate for dispenser color discrimination. + +Runs without a camera and without robot hardware. It verifies synthetic color +patches classify into the expected HSV bins. +""" +from __future__ import annotations + +import csv +import subprocess +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +RESULT = ROOT / "outputs" / "color_discrimination" / "color_discrimination_results.csv" +SCRIPT = ROOT / "tools" / "perception" / "offline_color_discrimination_test.py" +EXPECTED = {"red", "orange", "yellow", "green", "blue", "purple", "black", "white"} + + +def fail(msg: str) -> int: + print(f"[FAIL] {msg}") + return 1 + + +def main() -> int: + proc = subprocess.run([sys.executable, str(SCRIPT)], cwd=str(ROOT), text=True, capture_output=True) + print(proc.stdout, end="") + if proc.stderr: + print(proc.stderr, end="", file=sys.stderr) + if proc.returncode != 0: + return fail("offline_color_discrimination_test.py returned non-zero") + if not RESULT.exists(): + return fail(f"missing result CSV: {RESULT}") + rows = list(csv.DictReader(RESULT.open(encoding="utf-8"))) + got = {r["expected_color"] for r in rows if r.get("source") == "synthetic"} + if got != EXPECTED: + return fail(f"synthetic color set mismatch: got={sorted(got)} expected={sorted(EXPECTED)}") + bad = [r for r in rows if str(r.get("pass")) != "True"] + if bad: + return fail(f"color classification failures: {bad}") + print("[PASS] offline HSV color discrimination works without camera") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/perception/color_discrimination.py b/tools/perception/color_discrimination.py new file mode 100755 index 0000000..68f803d --- /dev/null +++ b/tools/perception/color_discrimination.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 +"""Offline HSV color discrimination utilities for dispenser/cocktail perception. + +This module is intentionally perception-only: it classifies colors in image crops +or arrays and does not subscribe to cameras or command hardware. +""" +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Iterable, Mapping, Sequence + +import numpy as np + +try: + import cv2 # type: ignore +except Exception: # pragma: no cover - handled by callers + cv2 = None + + +COLOR_ORDER = ("red", "orange", "yellow", "green", "blue", "purple", "black", "white", "unknown") + + +@dataclass(frozen=True) +class HsvColorResult: + color: str + h_median: float + s_median: float + v_median: float + confidence: float + reason: str + + +def _require_cv2() -> None: + if cv2 is None: + raise RuntimeError("opencv-python is required for BGR/HSV color discrimination") + + +def center_crop_fraction(image: np.ndarray, fraction: float = 0.60) -> np.ndarray: + """Return center crop for stable median color estimation. + + The center crop avoids box borders, text overlays, and specular edges. + """ + if image.ndim < 2: + raise ValueError("image must have at least HxW dimensions") + fraction = float(fraction) + if not (0.0 < fraction <= 1.0): + raise ValueError("fraction must be in (0, 1]") + h, w = image.shape[:2] + ch, cw = max(1, int(round(h * fraction))), max(1, int(round(w * fraction))) + y1 = max(0, (h - ch) // 2) + x1 = max(0, (w - cw) // 2) + return image[y1 : y1 + ch, x1 : x1 + cw] + + +def median_hsv_from_bgr(crop_bgr: np.ndarray, center_fraction: float = 0.60) -> tuple[float, float, float]: + _require_cv2() + if crop_bgr.size == 0: + return 0.0, 0.0, 0.0 + crop = center_crop_fraction(crop_bgr, center_fraction) + hsv = cv2.cvtColor(crop, cv2.COLOR_BGR2HSV) + pixels = hsv.reshape(-1, 3).astype(np.float32) + return tuple(float(x) for x in np.median(pixels, axis=0)) # type: ignore[return-value] + + +def classify_hsv(h: float, s: float, v: float) -> HsvColorResult: + """Classify OpenCV HSV median into robot-relevant color bins. + + OpenCV hue range is [0, 179]. The thresholds are deliberately conservative: + low saturation/value becomes white/black before hue classification. + """ + h = float(h) % 180.0 + s = float(s) + v = float(v) + + if v < 45: + return HsvColorResult("black", h, s, v, 0.95, "value below black threshold") + if s < 35 and v >= 155: + return HsvColorResult("white", h, s, v, 0.90, "low saturation and high value") + if s < 28: + return HsvColorResult("unknown", h, s, v, 0.30, "low saturation but not bright enough for white") + + # hue ranges in OpenCV units. Red wraps around 0/179. + ranges: list[tuple[str, tuple[float, float] | tuple[tuple[float, float], tuple[float, float]], float]] = [ + ("red", ((0, 9), (170, 179)), 0.90), + ("orange", (10, 22), 0.85), + ("yellow", (23, 36), 0.85), + ("green", (37, 84), 0.85), + ("blue", (85, 124), 0.85), + ("purple", (125, 160), 0.80), + ] + for name, rng, conf in ranges: + if isinstance(rng[0], tuple): # type: ignore[index] + if any(lo <= h <= hi for lo, hi in rng): # type: ignore[assignment] + return HsvColorResult(name, h, s, v, conf, "hue inside wrapped range" if name == "red" else "hue inside range") + else: + lo, hi = rng # type: ignore[misc] + if lo <= h <= hi: + return HsvColorResult(name, h, s, v, conf, "hue inside range") + return HsvColorResult("unknown", h, s, v, 0.25, "hue outside configured ranges") + + +def classify_bgr_crop(crop_bgr: np.ndarray, center_fraction: float = 0.60) -> HsvColorResult: + h, s, v = median_hsv_from_bgr(crop_bgr, center_fraction=center_fraction) + return classify_hsv(h, s, v) + + +def classify_image_box(image_bgr: np.ndarray, xyxy: Sequence[float], center_fraction: float = 0.60) -> HsvColorResult: + h, w = image_bgr.shape[:2] + x1, y1, x2, y2 = [int(round(float(x))) for x in xyxy] + x1, y1 = max(0, x1), max(0, y1) + x2, y2 = min(w, x2), min(h, y2) + if x2 <= x1 or y2 <= y1: + return HsvColorResult("unknown", 0.0, 0.0, 0.0, 0.0, "empty crop") + return classify_bgr_crop(image_bgr[y1:y2, x1:x2], center_fraction=center_fraction) + + +def bgr_patch_for_color(color: str, size: int = 96) -> np.ndarray: + """Generate deterministic synthetic BGR patch for offline regression tests.""" + _require_cv2() + hsv_values = { + "red": (0, 220, 220), + "orange": (16, 220, 230), + "yellow": (30, 220, 235), + "green": (60, 210, 210), + "blue": (110, 210, 210), + "purple": (142, 190, 200), + "black": (0, 0, 25), + "white": (0, 0, 230), + } + if color not in hsv_values: + raise ValueError(f"unsupported synthetic color: {color}") + hsv = np.zeros((size, size, 3), dtype=np.uint8) + hsv[:, :] = hsv_values[color] + bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR) + # Add mild deterministic brightness gradient to mimic real nonuniform lighting. + grad = np.linspace(-12, 12, size, dtype=np.int16).reshape(1, size, 1) + return np.clip(bgr.astype(np.int16) + grad, 0, 255).astype(np.uint8) + + +def read_bgr(path: Path) -> np.ndarray: + _require_cv2() + img = cv2.imread(str(path), cv2.IMREAD_COLOR) + if img is None: + raise RuntimeError(f"failed to read image: {path}") + return img diff --git a/tools/perception/offline_color_discrimination_test.py b/tools/perception/offline_color_discrimination_test.py new file mode 100755 index 0000000..bcf7e88 --- /dev/null +++ b/tools/perception/offline_color_discrimination_test.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +"""Run color discrimination tests without a camera. + +Modes: + 1. Synthetic patches for red/orange/yellow/green/blue/purple/black/white. + 2. Optional image boxes from a CSV: image_path,expected_color,x1,y1,x2,y2. + +Outputs a CSV and preview image directory under outputs/color_discrimination/. +""" +from __future__ import annotations + +import argparse +import csv +from pathlib import Path +import sys + +import numpy as np + +ROOT = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(ROOT)) + +from tools.perception.color_discrimination import ( # noqa: E402 + COLOR_ORDER, + bgr_patch_for_color, + classify_bgr_crop, + classify_image_box, + read_bgr, +) + +try: + import cv2 # type: ignore +except Exception: + cv2 = None + +OUT_DIR = ROOT / "outputs" / "color_discrimination" +FIELDS = ["source", "expected_color", "predicted_color", "pass", "h_median", "s_median", "v_median", "confidence", "reason", "preview_path"] + + +def write_preview(path: Path, image: np.ndarray, label: str) -> None: + if cv2 is None: + return + img = image.copy() + cv2.putText(img, label, (8, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (255, 255, 255), 2, cv2.LINE_AA) + cv2.putText(img, label, (8, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 0), 1, cv2.LINE_AA) + path.parent.mkdir(parents=True, exist_ok=True) + cv2.imwrite(str(path), img) + + +def run_synthetic(preview_dir: Path) -> list[dict]: + rows = [] + for color in [c for c in COLOR_ORDER if c != "unknown"]: + patch = bgr_patch_for_color(color) + result = classify_bgr_crop(patch) + preview = preview_dir / f"synthetic_{color}_pred_{result.color}.png" + write_preview(preview, patch, f"gt={color} pred={result.color}") + rows.append({ + "source": "synthetic", + "expected_color": color, + "predicted_color": result.color, + "pass": result.color == color, + "h_median": f"{result.h_median:.2f}", + "s_median": f"{result.s_median:.2f}", + "v_median": f"{result.v_median:.2f}", + "confidence": f"{result.confidence:.2f}", + "reason": result.reason, + "preview_path": str(preview), + }) + return rows + + +def run_box_csv(path: Path, preview_dir: Path) -> list[dict]: + if not path.exists(): + return [] + rows = [] + with path.open("r", encoding="utf-8", newline="") as f: + reader = csv.DictReader(f) + for i, row in enumerate(reader, start=1): + image_path = Path(row["image_path"]) + expected = str(row["expected_color"]).strip().lower() + xyxy = [float(row[c]) for c in ["x1", "y1", "x2", "y2"]] + image = read_bgr(image_path) + result = classify_image_box(image, xyxy) + x1, y1, x2, y2 = [int(round(x)) for x in xyxy] + crop = image[max(0, y1):max(0, y2), max(0, x1):max(0, x2)] + preview = preview_dir / f"box_{i:03d}_{image_path.stem}_gt_{expected}_pred_{result.color}.png" + write_preview(preview, crop if crop.size else image, f"gt={expected} pred={result.color}") + rows.append({ + "source": str(image_path), + "expected_color": expected, + "predicted_color": result.color, + "pass": result.color == expected, + "h_median": f"{result.h_median:.2f}", + "s_median": f"{result.s_median:.2f}", + "v_median": f"{result.v_median:.2f}", + "confidence": f"{result.confidence:.2f}", + "reason": result.reason, + "preview_path": str(preview), + }) + return rows + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--box-csv", default="", help="Optional CSV with image_path,expected_color,x1,y1,x2,y2") + parser.add_argument("--output", default=str(OUT_DIR / "color_discrimination_results.csv")) + args = parser.parse_args() + + OUT_DIR.mkdir(parents=True, exist_ok=True) + preview_dir = OUT_DIR / "preview" + rows = run_synthetic(preview_dir) + if args.box_csv: + rows.extend(run_box_csv(Path(args.box_csv), preview_dir)) + + out = Path(args.output) + out.parent.mkdir(parents=True, exist_ok=True) + with out.open("w", encoding="utf-8", newline="") as f: + writer = csv.DictWriter(f, fieldnames=FIELDS) + writer.writeheader() + writer.writerows(rows) + + total = len(rows) + passed = sum(str(r["pass"]) == "True" for r in rows) + print(f"[Azas] offline color discrimination: {passed}/{total} passed") + print(out) + return 0 if passed == total else 1 + + +if __name__ == "__main__": + raise SystemExit(main()) From 20c319cdba33a3d5114dc63c4c7e32cea8b9db7e Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Mon, 1 Jun 2026 18:16:31 +0900 Subject: [PATCH 02/88] Plan post-shake human handover safely Add a dry-run-only post-shake hand tracking and handover planning sequence so the workflow can end with a human handover concept without enabling live motion near a person. Constraint: Handover near a human is an HRI safety-critical step and must not be activated by perception alone. Rejected: Adding direct robot handover motion after shaking | unsafe without hand tracking validation, operator approval, force limits, and HRI review. Confidence: high Scope-risk: moderate Directive: Keep HANDOVER_CUP_TO_HUMAN_DISABLED disabled until a separate live-executor safety review is completed. Tested: python3 -m py_compile src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py tools/checks/check_cocktail_workflow_plan.py; python3 tools/checks/check_cocktail_workflow_plan.py Not-tested: Live hand detector, camera depth, and real robot handover execution. --- docs/post_shake_human_handover_plan.md | 78 +++++++++++++++++++ .../cocktail_workflow_plan.py | 55 ++++++++++++- tools/checks/check_cocktail_workflow_plan.py | 27 ++++++- 3 files changed, 158 insertions(+), 2 deletions(-) create mode 100644 docs/post_shake_human_handover_plan.md diff --git a/docs/post_shake_human_handover_plan.md b/docs/post_shake_human_handover_plan.md new file mode 100644 index 0000000..f9566cd --- /dev/null +++ b/docs/post_shake_human_handover_plan.md @@ -0,0 +1,78 @@ +# Post-shake human hand handover plan + +This document defines a safe, staged plan for ending the cocktail workflow by +tracking a person's hand and preparing a cup handover after shaking/serving. + +## Summary + +The feature is useful for the final user experience, but it is an HRI +(human-robot interaction) step: the robot would move near a person. Therefore the +current implementation is deliberately a dry-run plan only. + +Implemented in this branch: + +1. Add post-shake hand tracking and handover planning phases to the cocktail task plan. +2. Keep hand tracking as perception-only. +3. Compute a handover pose candidate only as data. +4. Require explicit operator approval. +5. Keep the actual cup-to-human handover motion disabled until a separate safety review. + +No live robot command is added by this branch. + +## Workflow phases added + +The new final phases are appended after `POUR`: + +1. `VERIFY_HUMAN_HAND_TRACKING` + - input: `/azas/human_hand_detection`, `handover_safety.yaml` + - purpose: require a stable open hand target + - gate: `no_motion_hri_perception_only` + +2. `COMPUTE_HANDOVER_POSE` + - input: stable hand target and camera/base TF + - purpose: compute a conservative pose candidate with approach offset + - gate: `tf_required_no_motion` + +3. `WAIT_FOR_HANDOVER_APPROVAL` + - input: pose candidate, operator confirmation, still-open hand target + - purpose: prevent accidental handover execution + - gate: `operator_approval_required` + +4. `HANDOVER_CUP_TO_HUMAN_DISABLED` + - purpose: placeholder final handover step + - gate: `disabled_until_hri_safety_review` + - command: `disabled_handover_motion_placeholder` + +## Why this is staged + +A handover near a human should not be triggered by vision alone. Before any live +execution, the project needs at least: + +- hand target stability check +- depth validity check +- person distance monitor +- emergency stop observer +- low force/speed limits +- retreat path +- operator confirmation +- real-robot dry-run with no cup +- real-robot dry-run with empty cup + +## Regression check + +```bash +cd /home/ssu/Azas +python3 tools/checks/check_cocktail_workflow_plan.py +``` + +Expected: + +```text +[PASS] full cocktail workflow plan includes calibration, dispenser press, shake gates, and disabled post-shake handover planning +``` + +## Current limitation + +There is no live hand detector in this branch. The workflow expects a future +perception source such as `/azas/human_hand_detection`. That source should be +validated offline first, similar to the camera-free color discrimination test. diff --git a/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py b/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py index 5033e28..2514174 100644 --- a/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py +++ b/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py @@ -36,7 +36,7 @@ def detection_class(status: str) -> str | None: return tail.split(maxsplit=1)[0].split(":", maxsplit=1)[0].strip().lower() or None -def build_cocktail_steps(dispenser_ids: list[str]) -> list[TaskStep]: +def build_cocktail_steps(dispenser_ids: list[str], include_human_handover: bool = True) -> list[TaskStep]: steps = [ TaskStep( "VERIFY_RECIPE", @@ -159,4 +159,57 @@ def build_cocktail_steps(dispenser_ids: list[str]) -> list[TaskStep]: ), ] ) + + if include_human_handover: + steps.extend( + [ + TaskStep( + "VERIFY_HUMAN_HAND_TRACKING", + "track an open human hand after shaking/serving and require stable hand perception before any handover plan", + required_inputs=("cocktail_served", "/azas/human_hand_detection", "handover_safety.yaml"), + produces=("stable_human_hand_target",), + command="none", + hardware_gate="no_motion_hri_perception_only", + parameters={ + "min_stable_frames": 10, + "max_target_age_s": 1.0, + "required_state": "open_hand", + }, + ), + TaskStep( + "COMPUTE_HANDOVER_POSE", + "convert stable hand target to a conservative handover pose candidate with approach offset and retreat path", + required_inputs=("stable_human_hand_target", "base_link<-camera_frame TF", "handover_safety.yaml"), + produces=("handover_pose_candidate",), + command="none", + hardware_gate="tf_required_no_motion", + parameters={ + "approach_offset_m": 0.12, + "min_hand_distance_m": 0.10, + "max_handover_speed_mps": 0.05, + }, + ), + TaskStep( + "WAIT_FOR_HANDOVER_APPROVAL", + "wait for explicit operator confirmation and a still-open hand before enabling any live handover executor", + required_inputs=("handover_pose_candidate", "operator_confirmation", "stable_human_hand_target"), + produces=("handover_approved",), + command="none", + hardware_gate="operator_approval_required", + ), + TaskStep( + "HANDOVER_CUP_TO_HUMAN_DISABLED", + "placeholder final handover step; live motion is intentionally disabled until HRI safety review and force/speed limits are validated", + required_inputs=("handover_approved", "cup_or_served_drink_held"), + produces=("handover_ready_for_separate_live_executor",), + command="disabled_handover_motion_placeholder", + hardware_gate="disabled_until_hri_safety_review", + parameters={ + "requires_force_limit": True, + "requires_emergency_stop_observer": True, + "requires_person_distance_monitor": True, + }, + ), + ] + ) return steps diff --git a/tools/checks/check_cocktail_workflow_plan.py b/tools/checks/check_cocktail_workflow_plan.py index 2b319a0..af30d91 100755 --- a/tools/checks/check_cocktail_workflow_plan.py +++ b/tools/checks/check_cocktail_workflow_plan.py @@ -27,6 +27,10 @@ def main() -> int: "SHAKE_CUP", "OPEN_LID", "POUR", + "VERIFY_HUMAN_HAND_TRACKING", + "COMPUTE_HANDOVER_POSE", + "WAIT_FOR_HANDOVER_APPROVAL", + "HANDOVER_CUP_TO_HUMAN_DISABLED", ] if phases != required_order: print("[FAIL] unexpected workflow phase order") @@ -64,7 +68,28 @@ def main() -> int: print("[FAIL] SHAKE_CUP dispenser_keepout_radius_m is too small") return 1 - print("[PASS] full cocktail workflow plan includes calibration, dispenser press, and shake gates") + handover = {step.phase: step for step in steps if step.phase.startswith(("VERIFY_HUMAN", "COMPUTE_HANDOVER", "WAIT_FOR_HANDOVER", "HANDOVER_CUP"))} + expected_handover = { + "VERIFY_HUMAN_HAND_TRACKING", + "COMPUTE_HANDOVER_POSE", + "WAIT_FOR_HANDOVER_APPROVAL", + "HANDOVER_CUP_TO_HUMAN_DISABLED", + } + if set(handover) != expected_handover: + print("[FAIL] post-shake human handover dry-run steps are missing") + print(json.dumps(sorted(handover), ensure_ascii=False, indent=2)) + return 1 + if handover["VERIFY_HUMAN_HAND_TRACKING"].hardware_gate != "no_motion_hri_perception_only": + print("[FAIL] hand tracking must remain perception-only") + return 1 + if handover["HANDOVER_CUP_TO_HUMAN_DISABLED"].hardware_gate != "disabled_until_hri_safety_review": + print("[FAIL] live handover must remain disabled until HRI safety review") + return 1 + if handover["HANDOVER_CUP_TO_HUMAN_DISABLED"].command != "disabled_handover_motion_placeholder": + print("[FAIL] handover placeholder must not route to a live motion command") + return 1 + + print("[PASS] full cocktail workflow plan includes calibration, dispenser press, shake gates, and disabled post-shake handover planning") return 0 From 6667c29a70b8aa7c23efc229f43e0c4bae0e93f3 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 13:45:36 +0900 Subject: [PATCH 03/88] Refactor launch parameters and update session logs for improved clarity and functionality --- docs/robot_pipeline_control.html | 91 +++--- omx_wiki/index.md | 3 +- omx_wiki/log.md | 4 + omx_wiki/session-log-2026-06-05-8-3rrp4t.md | 18 ++ src/azas_bringup/config/calibration.yaml | 19 +- ...measured_dispenser_collision_scene_node.py | 5 +- .../cocktail_workflow_plan.py | 2 +- src/azas_voice/azas_voice/tts_node.py | 6 +- .../dsr_practice/yolo_cup_pick_node.py | 2 - .../launch/yolo_cup_pick_node.launch.py | 5 - tools/checks/check_dispenser_color_scan.py | 93 ++++++ tools/perception/dispenser_color_scan.py | 295 ++++++++++++++++++ tools/run/compare_hand_eye_in_rviz.sh | 44 +++ tools/run/robot_pipeline_control_server.py | 111 +++---- tools/run/run_rule_based_shake_real.sh | 46 +-- tools/setup/bootstrap_team_pc.sh | 1 - 16 files changed, 592 insertions(+), 153 deletions(-) create mode 100644 omx_wiki/session-log-2026-06-05-8-3rrp4t.md create mode 100644 tools/checks/check_dispenser_color_scan.py create mode 100644 tools/perception/dispenser_color_scan.py create mode 100755 tools/run/compare_hand_eye_in_rviz.sh diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index 0ce1892..ccce9fa 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -162,6 +162,14 @@ border-color: #a7f3d0; color: #047857; } + .quick-start-btn { + background: #1e40af !important; + border-color: #1e40af !important; + color: #fff !important; + font-weight: 600 !important; + letter-spacing: 0.02em; + } + .quick-start-btn:hover { background: #1d3faa !important; } .danger-light { color: var(--red) !important; background: #fff1f2 !important; border-color: #fecdd3 !important; } details.settings { @@ -612,6 +620,7 @@

Azas Robot Pipeline Control

+ @@ -676,15 +685,14 @@

Azas Robot Pipeline Control

source /home/ssu/Azas/install/setup.bash export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} -ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ - model_path:=/home/ssu/Azas/best.pt \ +ros2 launch /home/ssu/Azas/install/dsr_practice/share/dsr_practice/launch/yolo_cup_pick_node.launch.py \ + model_path:=/home/ssu/Azas/local_models/best.pt \ conf:=0.35 \ imgsz:=640 \ device:=cpu \ target_class:=cup \ - auto_pick:=false \ + auto_pick:=true \ auto_pick_interval:=3.0 \ - pick_depth_ratio:=0.55 \ depth_patch_radius:=7 \ min_depth_valid_ratio:=0.03 \ min_depth_m:=0.15 \ @@ -692,77 +700,37 @@

Azas Robot Pipeline Control

redetect_on_approach:=false \ redetect_settle_sec:=0.5 \ grasp_mode:=side \ - side_grasp_axis:=y_axis \ - side_grasp_direction:=1.0 \ - side_auto_direction_by_cup_y:=true \ side_far_stage_enabled:=false \ - side_staging_offset:=0.30 \ side_approach_offset:=0.18 \ side_short_stage_backoff_m:=0.08 \ - side_stage_y_min:=-0.35 \ - side_stage_y_max:=0.35 \ - side_grasp_offset:=0.025 \ - side_grasp_z_offset:=0.05 \ side_grasp_stop_backoff_m:=0.04 \ - side_close_underreach_m:=0.05 \ + side_close_underreach_m:=0.03 \ side_low_retry_lift_m:=0.03 \ side_low_retry_attempts:=5 \ - side_linear_approach_enabled:=false \ + side_linear_approach_enabled:=true \ side_final_slide_enabled:=false \ side_fixed_grasp_z_enabled:=true \ side_fixed_grasp_z:=0.07 \ side_project_bbox_center_to_fixed_z:=true \ - side_orientation_mode:=approach \ - side_tool_roll_deg:=0.0 \ - side_roll_deg:=0.0 \ - side_pitch_deg:=90.0 \ - side_yaw_deg:=0.0 \ - table_collision_enabled:=true \ - table_surface_z:=0.0 \ - table_thickness:=0.04 \ - table_size_x:=1.20 \ - table_size_y:=1.00 \ - table_center_x:=0.45 \ - table_center_y:=0.0 \ - dispenser_collision_enabled:=true \ - dispenser_collision_config_path:=/home/ssu/Azas/src/azas_bringup/config/measured_dispenser_collision.yaml \ - dispenser_collision_publish_period_sec:=1.0 \ - dispenser_collision_publish_objects:=true \ - dispenser_collision_publish_markers:=true \ - center_check_enabled:=false \ - center_check_settle_sec:=0.6 \ - center_check_x:=0.45 \ - center_check_y:=0.0 \ - center_check_z:=0.64 \ - side_prepose_enabled:=false \ - side_prepose_split_z:=0.18 \ + side_candidate_plan_check_enabled:=true \ side_move_to_initial_center_before_close:=false \ - pre_pick_joint1_clearance_deg:=0.0 \ verify_motion:=true \ - motion_verify_tolerance:=0.03 \ - joint_goal_tolerance_rad:=0.02 \ move_to_camera_home:=true \ move_joint_home_before_camera_home:=false \ camera_home_mode:=joint \ - camera_home_joint_1_deg:=3.0 \ - camera_home_joint_2_deg:=-12.7 \ - camera_home_joint_3_deg:=44.0 \ - camera_home_joint_4_deg:=-9.0 \ - camera_home_joint_5_deg:=133.0 \ - camera_home_joint_6_deg:=90.0 \ - camera_home_x:=0.45 \ - camera_home_y:=0.0 \ - camera_home_z:=0.64 \ - camera_home_search_max_z:=0.64 \ - camera_home_search_min_z:=0.54 \ - camera_home_search_step_z:=0.02 \ min_motion_z:=0.07 \ workspace_xy_clamp_enabled:=false \ return_home_after_task:=false \ return_to_camera_home_after_attempt:=true \ - place_x:=0.45 \ - place_y:=0.0 \ - place_z:=0.30 \ + table_collision_enabled:=true \ + table_surface_z:=0.0 \ + table_thickness:=0.04 \ + table_size_x:=1.10 \ + table_size_y:=0.65 \ + table_center_x:=0.29 \ + table_center_y:=0.0 \ + dispenser_collision_enabled:=true \ + dispenser_collision_config_path:=/home/ssu/Azas/src/azas_bringup/config/measured_dispenser_collision.yaml \ moveit_controller_name:=/dsr01/dsr_moveit_controller \ start_joint_state_relay:=true
@@ -1355,6 +1323,17 @@

파이프라인 단계

if (mode) selectByMode(mode); }); document.getElementById("focusLog").addEventListener("click", focusLog); + document.getElementById("quickStartBtn").addEventListener("click", () => { + const keys = ["connect_robot", "status_check", "lift_robot", "start_camera", "start_collision_scene", "side_grip"]; + selectedQueue = []; + addQueueItems(keys); + itemStatuses = new Map(); + updateSelectedCount(); + renderSteps(); + flowEl.closest(".flow-panel")?.scrollIntoView({behavior: "smooth", block: "start"}); + log.textContent = `⚡ 빠른시작: ${keys.length}개 단계를 큐에 올렸습니다. 실행 버튼을 누르세요.`; + focusLog(); + }); document.getElementById("run").addEventListener("click", async () => { const selected = withCollisionScenePrereq(selectedQueue); diff --git a/omx_wiki/index.md b/omx_wiki/index.md index 524a38a..b1af80a 100644 --- a/omx_wiki/index.md +++ b/omx_wiki/index.md @@ -1,9 +1,10 @@ # Wiki Index -> 3 pages | Last updated: 2026-05-28T02:01:49.699Z +> 4 pages | Last updated: 2026-06-05T03:23:17.635Z ## session-log - [Azas Real Robot Handoff 2026-05-18 Dispenser Shake Panel](azas-real-robot-handoff-2026-05-18-dispenser-shake-panel.md) — # Azas Real Robot Handoff 2026-05-18 Dispenser Shake Panel - [Session Log 2026-05-19](session-log-2026-05-19-0-4as20g.md) — # Session Log 2026-05-19 - [Session Log 2026-05-28](session-log-2026-05-28-3-adlm15.md) — # Session Log 2026-05-28 +- [Session Log 2026-06-05](session-log-2026-06-05-8-3rrp4t.md) — # Session Log 2026-06-05 diff --git a/omx_wiki/log.md b/omx_wiki/log.md index 848e34b..1cce7af 100644 --- a/omx_wiki/log.md +++ b/omx_wiki/log.md @@ -54,3 +54,7 @@ - **Pages:** session-log-2026-05-28-3-adlm15.md - **Summary:** Auto-captured session log for omx-1779933651513-adlm15 +## [2026-06-05T03:23:17.633Z] session-end +- **Pages:** session-log-2026-06-05-8-3rrp4t.md +- **Summary:** Auto-captured session log for omx-1780629727098-3rrp4t + diff --git a/omx_wiki/session-log-2026-06-05-8-3rrp4t.md b/omx_wiki/session-log-2026-06-05-8-3rrp4t.md new file mode 100644 index 0000000..7f82679 --- /dev/null +++ b/omx_wiki/session-log-2026-06-05-8-3rrp4t.md @@ -0,0 +1,18 @@ +--- +title: "Session Log 2026-06-05" +tags: ["session-log", "auto-captured"] +created: 2026-06-05T03:23:17.633Z +updated: 2026-06-05T03:23:17.633Z +sources: ["omx-1780629727098-3rrp4t"] +links: [] +category: session-log +confidence: medium +schemaVersion: 1 +--- + +# Session Log 2026-06-05 + +Auto-captured session metadata. +Session ID: omx-1780629727098-3rrp4t + +Review and promote significant findings to curated wiki pages via `wiki_ingest`. diff --git a/src/azas_bringup/config/calibration.yaml b/src/azas_bringup/config/calibration.yaml index d1948ad..f66ad26 100644 --- a/src/azas_bringup/config/calibration.yaml +++ b/src/azas_bringup/config/calibration.yaml @@ -5,15 +5,28 @@ frames: base_frame: base_link camera_frame: null # 확인 필요: 실제 depth camera frame_id - ee_link: null # 확인 필요: M0609 MoveIt EE_LINK + ee_link: link_6 # M0609 EE link (yolo_cup_pick_node.py 확인) planning_group: null # 확인 필요: M0609 MoveIt GROUP_NAME gripper_tcp: gripper_tcp # 확인 필요: 실제 TCP frame 구성 후 확정 cup_mouth_center: cup_mouth_center hand_eye: parent_frame: base_link child_frame: null # 확인 필요: camera_frame - xyz_m: null # 확인 필요: calibrated transform - rpy_rad: null # 확인 필요: calibrated transform + npy_path: src/dsr_practice/config/T_gripper2camera.npy # measured (May 15), used by yolo_cup_pick_node + npy_path_alt: src/azas_perception/config/T_gripper2camera.npy # re-calibrated (May 20) + xyz_m: null # 확인 필요: fill from npy after deciding canonical file + rpy_rad: null # 확인 필요: fill from npy after deciding canonical file + +# Saved robot pose for dispenser color scanning. +# Joint values from yolo_cup_pick_node.launch.py camera_home_joint_*_deg defaults (measured). +color_scan_pose: + source: yolo_cup_pick_node.launch.py camera_home defaults + ee_link: link_6 + joints_deg: [3.0, -12.7, 44.0, -9.0, 133.0, 90.0] + joints_rad: [0.0524, -0.2217, 0.7679, -0.1571, 2.3213, 1.5708] + joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] + cartesian_xyz_m: [0.45, 0.0, 0.64] + cartesian_frame: base_link cup_offsets: default: tcp_to_cup_mouth_m: null # 확인 필요: jig/dry-run으로 측정 diff --git a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py index 46c3669..2194b39 100644 --- a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py +++ b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py @@ -251,7 +251,10 @@ def _publish_scene(self) -> None: markers = self._make_markers(collision_objects) self.marker_pub.publish(markers) if self.publish_rviz_visual_tools_compat: - self.rviz_visual_tools_pub.publish(markers) + rviz_markers = MarkerArray( + markers=[m for m in markers.markers if m.action != Marker.DELETEALL] + ) + self.rviz_visual_tools_pub.publish(rviz_markers) def _make_collision_object( self, object_id: str, object_config: dict[str, Any] diff --git a/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py b/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py index 2514174..7f122c4 100644 --- a/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py +++ b/src/azas_task_manager/azas_task_manager/cocktail_workflow_plan.py @@ -41,7 +41,7 @@ def build_cocktail_steps(dispenser_ids: list[str], include_human_handover: bool TaskStep( "VERIFY_RECIPE", "accept symbolic recipe and ordered dispenser color targets", - required_inputs=("/azas/voice/recipe_decision",), + required_inputs=("/azas/voice/confirmed_recipe_decision",), produces=("ordered_dispenser_colors",), ), TaskStep( diff --git a/src/azas_voice/azas_voice/tts_node.py b/src/azas_voice/azas_voice/tts_node.py index d27b82f..ddb3b40 100644 --- a/src/azas_voice/azas_voice/tts_node.py +++ b/src/azas_voice/azas_voice/tts_node.py @@ -97,6 +97,10 @@ def _speed_adjusted_path(self, raw_path: str) -> str: ], check=True, ) + try: + os.unlink(raw_path) + except OSError: + pass return adjusted_path def shutdown(self) -> None: @@ -172,7 +176,7 @@ def _run_worker(self) -> None: except Exception as exc: self.get_logger().error(f"TTS playback failed: {exc}") self._publish_state("error", text=text, emotion="concerned") - finally: + else: self._publish_state("idle") def _publish_state(self, state: str, text: str = "", emotion: str = "neutral") -> None: diff --git a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py index 29cc8fb..82289b8 100644 --- a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -209,7 +209,6 @@ def __init__(self): self.declare_parameter("target_class", "cup") self.declare_parameter("auto_pick", False) self.declare_parameter("auto_pick_interval", 3.0) - self.declare_parameter("pick_depth_ratio", 0.55) self.declare_parameter("depth_patch_radius", 7) self.declare_parameter("min_depth_valid_ratio", 0.03) self.declare_parameter("min_depth_m", 0.15) @@ -375,7 +374,6 @@ def __init__(self): self.target_class = self.get_parameter("target_class").value self.auto_pick = parse_bool(self.get_parameter("auto_pick").value) self.auto_pick_interval = float(self.get_parameter("auto_pick_interval").value) - self.pick_depth_ratio = float(self.get_parameter("pick_depth_ratio").value) self.depth_patch_radius = int(self.get_parameter("depth_patch_radius").value) self.min_depth_valid_ratio = float( self.get_parameter("min_depth_valid_ratio").value diff --git a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py index 35942e6..af01dc8 100644 --- a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py +++ b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py @@ -102,7 +102,6 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params value_type=str, ), "auto_pick_interval": LaunchConfiguration("auto_pick_interval"), - "pick_depth_ratio": LaunchConfiguration("pick_depth_ratio"), "depth_patch_radius": LaunchConfiguration("depth_patch_radius"), "min_depth_valid_ratio": LaunchConfiguration("min_depth_valid_ratio"), "min_depth_m": LaunchConfiguration("min_depth_m"), @@ -320,9 +319,6 @@ def generate_launch_description(): auto_pick_interval_arg = DeclareLaunchArgument( "auto_pick_interval", default_value="3.0" ) - pick_depth_ratio_arg = DeclareLaunchArgument( - "pick_depth_ratio", default_value="0.55" - ) depth_patch_radius_arg = DeclareLaunchArgument( "depth_patch_radius", default_value="7" ) @@ -707,7 +703,6 @@ def generate_launch_description(): device_arg, target_class_arg, auto_pick_interval_arg, - pick_depth_ratio_arg, depth_patch_radius_arg, min_depth_valid_ratio_arg, min_depth_m_arg, diff --git a/tools/checks/check_dispenser_color_scan.py b/tools/checks/check_dispenser_color_scan.py new file mode 100644 index 0000000..f82397e --- /dev/null +++ b/tools/checks/check_dispenser_color_scan.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 +"""Offline regression gate for dispenser_color_scan.py. + +Creates synthetic per-dispenser images, runs dispenser_color_scan.py, and +verifies that all 4 dispenser IDs are present with valid color labels. +""" +from __future__ import annotations + +import json +import subprocess +import sys +import tempfile +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +SCRIPT = ROOT / "tools" / "perception" / "dispenser_color_scan.py" +VALID_COLORS = {"red", "orange", "yellow", "green", "blue", "purple", "black", "white", "unknown"} +EXPECTED_IDS = {"1", "2", "3", "4"} +# One synthetic color per dispenser slot for the offline test +DISPENSER_COLORS = {"1": "red", "2": "green", "3": "yellow", "4": "blue"} + + +def fail(msg: str) -> int: + print(f"[FAIL] {msg}") + return 1 + + +def create_synthetic_images(image_dir: Path) -> None: + sys.path.insert(0, str(ROOT)) + from tools.perception.color_discrimination import bgr_patch_for_color # noqa: E402 + try: + import cv2 # type: ignore + except ImportError as exc: + raise RuntimeError(f"opencv-python required for synthetic image creation: {exc}") from exc + + image_dir.mkdir(parents=True, exist_ok=True) + for did, color in DISPENSER_COLORS.items(): + patch = bgr_patch_for_color(color) + out_path = image_dir / f"dispenser_{did}.png" + cv2.imwrite(str(out_path), patch) + + +def main() -> int: + with tempfile.TemporaryDirectory() as tmp_dir: + image_dir = Path(tmp_dir) / "images" + output_path = Path(tmp_dir) / "dispenser_color_map.json" + + try: + create_synthetic_images(image_dir) + except Exception as exc: + return fail(f"synthetic image creation failed: {exc}") + + proc = subprocess.run( + [ + sys.executable, + str(SCRIPT), + "--image-dir", str(image_dir), + "--output", str(output_path), + ], + cwd=str(ROOT), + text=True, + capture_output=True, + ) + if proc.stdout: + print(proc.stdout, end="") + if proc.stderr: + print(proc.stderr, end="", file=sys.stderr) + + if proc.returncode != 0: + return fail(f"dispenser_color_scan.py exited with code {proc.returncode}") + + if not output_path.exists(): + return fail(f"output JSON not created: {output_path}") + + try: + color_map: dict[str, str] = json.loads(output_path.read_text(encoding="utf-8")) + except Exception as exc: + return fail(f"output JSON is not valid: {exc}") + + missing_ids = EXPECTED_IDS - set(color_map.keys()) + if missing_ids: + return fail(f"missing dispenser IDs in output: {sorted(missing_ids)}") + + invalid_colors = {did: c for did, c in color_map.items() if c not in VALID_COLORS} + if invalid_colors: + return fail(f"invalid color values in output: {invalid_colors}") + + print(f"[PASS] dispenser_color_scan produced valid map: {color_map}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/perception/dispenser_color_scan.py b/tools/perception/dispenser_color_scan.py new file mode 100644 index 0000000..dcbc379 --- /dev/null +++ b/tools/perception/dispenser_color_scan.py @@ -0,0 +1,295 @@ +#!/usr/bin/env python3 +"""Scan dispenser positions to build a color→dispenser_id map. + +Modes: + --image-dir : classify dispenser_1.png ~ dispenser_4.png from a directory + --ros : subscribe to camera + TF, project each dispenser's 3D position to pixel, + crop and classify. Requires robot connected with TF publishing. + (default) : fail with usage hint if neither flag is given + +Output: {"1": "red", "2": "blue", ...} written to --output (default: outputs/dispenser_color_map.json) +""" +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(ROOT)) + +from tools.perception.color_discrimination import ( # noqa: E402 + bgr_patch_for_color, + classify_bgr_crop, + read_bgr, +) + +try: + import cv2 # type: ignore +except Exception: + cv2 = None + +CALIBRATION_PATH = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +HAND_EYE_PATH = ROOT / "src" / "dsr_practice" / "config" / "T_gripper2camera.npy" +DEFAULT_OUTPUT = ROOT / "outputs" / "dispenser_color_map.json" +DISPENSER_IDS = ("1", "2", "3", "4") +CAMERA_TOPIC = "/camera/camera/color/image_raw" +CAMERA_INFO_TOPIC = "/camera/camera/color/camera_info" +BASE_FRAME = "base_link" +EE_LINK = "link_6" +CROP_HALF_PX = 60 # half-side of crop box around projected pixel + + +def load_dispenser_ids() -> list[str]: + """Return dispenser IDs from calibration.yaml, falling back to 1-4.""" + try: + import yaml # type: ignore + with CALIBRATION_PATH.open() as f: + data = yaml.safe_load(f) + outlets = data.get("dispenser_outlets") or {} + ids = sorted(str(k) for k in outlets.keys()) + return ids if ids else list(DISPENSER_IDS) + except Exception: + return list(DISPENSER_IDS) + + +def load_dispenser_positions() -> dict[str, list[float]]: + """Return {dispenser_id: [x, y, z]} in base_link metres from calibration.yaml.""" + try: + import yaml # type: ignore + with CALIBRATION_PATH.open() as f: + data = yaml.safe_load(f) + outlets = data.get("dispenser_outlets") or {} + result = {} + for k, v in outlets.items(): + xyz = v.get("outlet_pose_xyz_m") + if xyz: + result[str(k)] = list(xyz) + return result + except Exception: + return {} + + +def load_hand_eye() -> "np.ndarray | None": + """Load T_gripper2camera (4x4, translation in mm → convert to m).""" + try: + import numpy as np # type: ignore + T = np.load(str(HAND_EYE_PATH)).astype(float) + T[:3, 3] /= 1000.0 + return T + except Exception as exc: + print(f"[dispenser_color_scan] WARNING: could not load hand-eye: {exc}", file=sys.stderr) + return None + + +def project_base_point_to_pixel( + xyz_base: list[float], + T_base2ee: "np.ndarray", + T_gripper2cam: "np.ndarray", + fx: float, fy: float, cx: float, cy: float, +) -> tuple[int, int] | None: + """Project a 3D point in base_link to a camera pixel. + + T_base2ee: 4x4 transform from base_link to EE (link_6), i.e. FK result. + T_gripper2cam: 4x4 from gripper frame to camera frame (hand-eye). + Returns (u, v) pixel or None if point is behind camera. + """ + import numpy as np # type: ignore + p_base = np.array([xyz_base[0], xyz_base[1], xyz_base[2], 1.0]) + # base_link → link_6 frame + T_ee2base = np.linalg.inv(T_base2ee) + p_ee = T_ee2base @ p_base + # link_6 frame → camera frame + p_cam = T_gripper2cam @ p_ee + if p_cam[2] <= 0.01: + return None + u = int(round(fx * p_cam[0] / p_cam[2] + cx)) + v = int(round(fy * p_cam[1] / p_cam[2] + cy)) + return u, v + + +def classify_image_file(path: Path) -> str: + img = read_bgr(path) + result = classify_bgr_crop(img) + return result.color + + +def scan_from_image_dir(image_dir: Path) -> dict[str, str]: + dispenser_ids = load_dispenser_ids() + color_map: dict[str, str] = {} + for did in dispenser_ids: + img_path = image_dir / f"dispenser_{did}.png" + if not img_path.exists(): + # try jpg fallback + img_path = image_dir / f"dispenser_{did}.jpg" + if not img_path.exists(): + print(f"[dispenser_color_scan] WARNING: image not found for dispenser {did}: {img_path}", file=sys.stderr) + color_map[did] = "unknown" + continue + color = classify_image_file(img_path) + color_map[did] = color + print(f"[dispenser_color_scan] dispenser {did}: {color} (from {img_path.name})") + return color_map + + +def scan_from_ros() -> dict[str, str]: + try: + import rclpy # type: ignore + from rclpy.qos import qos_profile_sensor_data # type: ignore + from sensor_msgs.msg import Image, CameraInfo # type: ignore + import tf2_ros # type: ignore + import numpy as np # type: ignore + from geometry_msgs.msg import TransformStamped # type: ignore + except ImportError as exc: + print(f"[dispenser_color_scan] rclpy not available: {exc}", file=sys.stderr) + print("[dispenser_color_scan] Source the ROS2 workspace before using --ros.", file=sys.stderr) + sys.exit(1) + + import time + + T_gripper2cam = load_hand_eye() + if T_gripper2cam is None: + print("[dispenser_color_scan] ERROR: could not load T_gripper2camera.npy", file=sys.stderr) + sys.exit(1) + + dispenser_positions = load_dispenser_positions() + if not dispenser_positions: + print("[dispenser_color_scan] ERROR: no dispenser positions in calibration.yaml", file=sys.stderr) + sys.exit(1) + + frame_bgr = None + cam_info = None + + def to_bgr(msg: "Image") -> "np.ndarray": + enc = (msg.encoding or "").lower() + data = np.frombuffer(msg.data, dtype=np.uint8) + if enc in ("rgb8", "bgr8"): + image = data.reshape((msg.height, msg.width, 3)) + return cv2.cvtColor(image, cv2.COLOR_RGB2BGR) if enc == "rgb8" else image + if enc in ("rgba8", "bgra8"): + image = data.reshape((msg.height, msg.width, 4)) + return cv2.cvtColor(image, cv2.COLOR_RGBA2BGR) if enc == "rgba8" else cv2.cvtColor(image, cv2.COLOR_BGRA2BGR) + if enc == "mono8": + image = data.reshape((msg.height, msg.width)) + return cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) + raise RuntimeError(f"unsupported encoding: {msg.encoding}") + + def image_cb(msg: "Image") -> None: + nonlocal frame_bgr + if frame_bgr is None: + frame_bgr = to_bgr(msg) + + def info_cb(msg: "CameraInfo") -> None: + nonlocal cam_info + cam_info = msg + + rclpy.init() + node = rclpy.create_node("dispenser_color_scan_node") + tf_buffer = tf2_ros.Buffer() + tf2_ros.TransformListener(tf_buffer, node) + node.create_subscription(Image, CAMERA_TOPIC, image_cb, qos_profile_sensor_data) + node.create_subscription(CameraInfo, CAMERA_INFO_TOPIC, info_cb, qos_profile_sensor_data) + + deadline = time.time() + 8.0 + try: + while rclpy.ok() and time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if frame_bgr is not None and cam_info is not None: + break + finally: + pass # keep node alive for TF lookup below + + if frame_bgr is None: + node.destroy_node(); rclpy.shutdown() + print(f"[dispenser_color_scan] no frame from {CAMERA_TOPIC} within 8s", file=sys.stderr) + sys.exit(1) + if cam_info is None: + node.destroy_node(); rclpy.shutdown() + print(f"[dispenser_color_scan] no camera_info from {CAMERA_INFO_TOPIC} within 8s", file=sys.stderr) + sys.exit(1) + + # Get TF: base_link → link_6 (EE) + T_base2ee = None + try: + tf_msg: TransformStamped = tf_buffer.lookup_transform( + BASE_FRAME, EE_LINK, rclpy.time.Time(), timeout=rclpy.duration.Duration(seconds=3.0) + ) + t = tf_msg.transform.translation + q = tf_msg.transform.rotation + import numpy as np # type: ignore + # quaternion → rotation matrix + qx, qy, qz, qw = q.x, q.y, q.z, q.w + R = np.array([ + [1-2*(qy**2+qz**2), 2*(qx*qy-qz*qw), 2*(qx*qz+qy*qw)], + [2*(qx*qy+qz*qw), 1-2*(qx**2+qz**2), 2*(qy*qz-qx*qw)], + [2*(qx*qz-qy*qw), 2*(qy*qz+qx*qw), 1-2*(qx**2+qy**2)], + ]) + T_base2ee = np.eye(4) + T_base2ee[:3, :3] = R + T_base2ee[:3, 3] = [t.x, t.y, t.z] + except Exception as exc: + print(f"[dispenser_color_scan] TF lookup {BASE_FRAME}→{EE_LINK} failed: {exc}", file=sys.stderr) + finally: + node.destroy_node() + rclpy.shutdown() + + if T_base2ee is None: + print("[dispenser_color_scan] ERROR: cannot get EE pose; is robot driver running?", file=sys.stderr) + sys.exit(1) + + fx, fy = cam_info.k[0], cam_info.k[4] + cx, cy = cam_info.k[2], cam_info.k[5] + img_h, img_w = frame_bgr.shape[:2] + color_map: dict[str, str] = {} + + for did, xyz in sorted(dispenser_positions.items()): + uv = project_base_point_to_pixel(xyz, T_base2ee, T_gripper2cam, fx, fy, cx, cy) + if uv is None: + print(f"[dispenser_color_scan] dispenser {did}: projection behind camera, fallback unknown", file=sys.stderr) + color_map[did] = "unknown" + continue + u, v = uv + x1 = max(0, u - CROP_HALF_PX) + x2 = min(img_w, u + CROP_HALF_PX) + y1 = max(0, v - CROP_HALF_PX) + y2 = min(img_h, v + CROP_HALF_PX) + if x2 <= x1 or y2 <= y1: + print(f"[dispenser_color_scan] dispenser {did}: projected pixel ({u},{v}) out of frame {img_w}x{img_h}", file=sys.stderr) + color_map[did] = "unknown" + continue + crop = frame_bgr[y1:y2, x1:x2] + result = classify_bgr_crop(crop) + color_map[did] = result.color + print(f"[dispenser_color_scan] dispenser {did}: {result.color} (pixel=({u},{v}) crop=[{x1}:{x2},{y1}:{y2}])") + + return color_map + + +def main() -> int: + parser = argparse.ArgumentParser(description="Scan dispenser positions for color and output a color map JSON.") + parser.add_argument("--image-dir", default="", help="Directory with dispenser_1.png ~ dispenser_4.png") + parser.add_argument("--output", default=str(DEFAULT_OUTPUT), help="Output JSON path") + parser.add_argument("--ros", action="store_true", help="Capture from ROS camera topic") + args = parser.parse_args() + + if not args.image_dir and not args.ros: + parser.print_help() + print("\n[dispenser_color_scan] ERROR: specify --image-dir or --ros", file=sys.stderr) + return 2 + + if args.image_dir: + color_map = scan_from_image_dir(Path(args.image_dir)) + else: + color_map = scan_from_ros() + + out = Path(args.output) + out.parent.mkdir(parents=True, exist_ok=True) + out.write_text(json.dumps(color_map, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") + print(f"[dispenser_color_scan] saved: {out}") + print(json.dumps(color_map, ensure_ascii=False)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/compare_hand_eye_in_rviz.sh b/tools/run/compare_hand_eye_in_rviz.sh new file mode 100755 index 0000000..792efa8 --- /dev/null +++ b/tools/run/compare_hand_eye_in_rviz.sh @@ -0,0 +1,44 @@ +#!/usr/bin/env bash +# 두 T_gripper2camera 캘리브레이션을 TF로 동시 퍼블리시. +# RViz에서 link_6 기준으로 두 camera frame 위치를 비교. +# +# 사용법: +# source /home/ssu/Azas/install/local_setup.bash +# bash tools/run/compare_hand_eye_in_rviz.sh +# +# RViz에서 확인: +# - Fixed Frame: base_link +# - Add > TF 체크 +# - camera_color_optical_frame_may20 (azas_perception, 5월20일) +# - camera_color_optical_frame_may15 (dsr_practice, 5월15일) +# 둘을 link_6와 비교하면 카메라 장착 위치 차이를 직접 확인 가능. + +set -e + +echo "[compare_hand_eye] May20 (azas_perception): xyz=[0.0340, 0.0572, 0.0108]" +echo "[compare_hand_eye] May15 (dsr_practice) : xyz=[0.0305, 0.0731, 0.0359]" +echo "" +echo "[compare_hand_eye] TF publisher 2개 백그라운드 실행 중..." + +ros2 run tf2_ros static_transform_publisher \ + --x 0.0340 --y 0.0572 --z 0.0108 \ + --qx 0.0020 --qy 0.0031 --qz 1.0000 --qw -0.0033 \ + --frame-id link_6 \ + --child-frame-id camera_color_optical_frame_may20 & +PID1=$! + +ros2 run tf2_ros static_transform_publisher \ + --x 0.0305 --y 0.0731 --z 0.0359 \ + --qx 0.0089 --qy 0.0050 --qz 0.9999 --qw -0.0013 \ + --frame-id link_6 \ + --child-frame-id camera_color_optical_frame_may15 & +PID2=$! + +echo "[compare_hand_eye] PID $PID1 = May20, PID $PID2 = May15" +echo "[compare_hand_eye] RViz를 열고 TF를 추가하세요:" +echo " rviz2 &" +echo "" +echo " 종료: Ctrl+C" + +trap "kill $PID1 $PID2 2>/dev/null; echo 'stopped.'" EXIT +wait diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 648b0d8..7aa9e5b 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -43,7 +43,7 @@ DEFAULT_ROBOT_HOST = "192.168.1.100" DEFAULT_ROS_DOMAIN_ID = "9" DEFAULT_YOLO_MODEL_PATH = ROOT / "local_models" / "best.pt" -PR20_YOLO_MODEL_PATH = Path("/home/ssu/Azas/best.pt") +PR20_YOLO_MODEL_PATH = DEFAULT_YOLO_MODEL_PATH DEFAULT_DISPENSER_TCP_NAME = "GripperDA_v1_jarvis" FAST_MOVE_VELOCITY = "30" FAST_MOVE_ACCELERATION = "30" @@ -71,12 +71,28 @@ "j5": "135", "j6": "0", } -DISPENSER_PRESS_TARGETS = { +_DISPENSER_PRESS_TARGETS_DEFAULT: dict[str, str] = { "1": "red", "2": "green", "3": "yellow", "4": "blue", } +DISPENSER_COLOR_MAP_PATH = ROOT / "outputs" / "dispenser_color_map.json" + + +def _load_dispenser_press_targets() -> dict[str, str]: + base = dict(_DISPENSER_PRESS_TARGETS_DEFAULT) + if DISPENSER_COLOR_MAP_PATH.exists(): + try: + loaded = json.loads(DISPENSER_COLOR_MAP_PATH.read_text(encoding="utf-8")) + if isinstance(loaded, dict): + base.update({str(k): str(v) for k, v in loaded.items()}) + except Exception: + pass + return base + + +DISPENSER_PRESS_TARGETS: dict[str, str] = _load_dispenser_press_targets() @dataclass(frozen=True) @@ -1635,19 +1651,18 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "if [ \"${AZAS_SIDE_GRIP_BUILD:-0}\" = \"1\" ]; then " "colcon build --symlink-install --packages-select dsr_practice; " "fi && " - "source /home/ssu/Azas/install/setup.bash && " + f"source {shlex.quote(str(ROOT / 'install' / 'local_setup.bash'))} && " f"export PYTHONPATH={shlex.quote(str(ROOT / 'tools' / 'run' / 'python_compat'))}:${{PYTHONPATH:-}} && " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " - "ros2 launch dsr_practice yolo_cup_pick_node.launch.py " - "model_path:=/home/ssu/Azas/best.pt " + f"ros2 launch {shlex.quote(str(ROOT / 'install' / 'dsr_practice' / 'share' / 'dsr_practice' / 'launch' / 'yolo_cup_pick_node.launch.py'))} " + f"model_path:={shlex.quote(str(DEFAULT_YOLO_MODEL_PATH))} " "conf:=0.35 " "imgsz:=640 " "device:=cpu " "target_class:=cup " - "auto_pick:=false " + "auto_pick:=true " "auto_pick_interval:=3.0 " - "pick_depth_ratio:=0.55 " "depth_patch_radius:=7 " "min_depth_valid_ratio:=0.03 " "min_depth_m:=0.15 " @@ -1655,77 +1670,37 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "redetect_on_approach:=false " "redetect_settle_sec:=0.5 " "grasp_mode:=side " - "side_grasp_axis:=y_axis " - "side_grasp_direction:=1.0 " - "side_auto_direction_by_cup_y:=true " "side_far_stage_enabled:=false " - "side_staging_offset:=0.30 " "side_approach_offset:=0.18 " "side_short_stage_backoff_m:=0.08 " - "side_stage_y_min:=-0.35 " - "side_stage_y_max:=0.35 " - "side_grasp_offset:=0.025 " - "side_grasp_z_offset:=0.05 " "side_grasp_stop_backoff_m:=0.04 " - "side_close_underreach_m:=0.05 " + "side_close_underreach_m:=0.03 " "side_low_retry_lift_m:=0.03 " "side_low_retry_attempts:=5 " - "side_linear_approach_enabled:=false " + "side_linear_approach_enabled:=true " "side_final_slide_enabled:=false " "side_fixed_grasp_z_enabled:=true " "side_fixed_grasp_z:=0.07 " "side_project_bbox_center_to_fixed_z:=true " - "side_orientation_mode:=approach " - "side_tool_roll_deg:=0.0 " - "side_roll_deg:=0.0 " - "side_pitch_deg:=90.0 " - "side_yaw_deg:=0.0 " - "table_collision_enabled:=true " - "table_surface_z:=0.0 " - "table_thickness:=0.04 " - "table_size_x:=1.20 " - "table_size_y:=1.00 " - "table_center_x:=0.45 " - "table_center_y:=0.0 " - "dispenser_collision_enabled:=true " - f"dispenser_collision_config_path:={shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'config' / 'measured_dispenser_collision.yaml'))} " - "dispenser_collision_publish_period_sec:=1.0 " - "dispenser_collision_publish_objects:=true " - "dispenser_collision_publish_markers:=true " - "center_check_enabled:=false " - "center_check_settle_sec:=0.6 " - "center_check_x:=0.45 " - "center_check_y:=0.0 " - "center_check_z:=0.64 " - "side_prepose_enabled:=false " - "side_prepose_split_z:=0.18 " + "side_candidate_plan_check_enabled:=true " "side_move_to_initial_center_before_close:=false " - "pre_pick_joint1_clearance_deg:=0.0 " "verify_motion:=true " - "motion_verify_tolerance:=0.03 " - "joint_goal_tolerance_rad:=0.02 " "move_to_camera_home:=true " "move_joint_home_before_camera_home:=false " "camera_home_mode:=joint " - "camera_home_joint_1_deg:=3.0 " - "camera_home_joint_2_deg:=-12.7 " - "camera_home_joint_3_deg:=44.0 " - "camera_home_joint_4_deg:=-9.0 " - "camera_home_joint_5_deg:=133.0 " - "camera_home_joint_6_deg:=90.0 " - "camera_home_x:=0.45 " - "camera_home_y:=0.0 " - "camera_home_z:=0.64 " - "camera_home_search_max_z:=0.64 " - "camera_home_search_min_z:=0.54 " - "camera_home_search_step_z:=0.02 " "min_motion_z:=0.07 " "workspace_xy_clamp_enabled:=false " "return_home_after_task:=false " "return_to_camera_home_after_attempt:=true " - "place_x:=0.45 " - "place_y:=0.0 " - "place_z:=0.30 " + "table_collision_enabled:=true " + "table_surface_z:=0.0 " + "table_thickness:=0.04 " + "table_size_x:=1.10 " + "table_size_y:=0.65 " + "table_center_x:=0.29 " + "table_center_y:=0.0 " + "dispenser_collision_enabled:=true " + f"dispenser_collision_config_path:={shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'config' / 'measured_dispenser_collision.yaml'))} " "moveit_controller_name:=/dsr01/dsr_moveit_controller " "start_joint_state_relay:=true" ) @@ -2383,6 +2358,9 @@ def do_GET(self) -> None: data.append(item) self.send_json(data) return + if path == "/api/dispenser_color_map": + self.send_json({"map": DISPENSER_PRESS_TARGETS}) + return if path == "/api/camera_snapshot.jpg": ok, body, error = camera_snapshot_jpeg() if not ok: @@ -2416,6 +2394,21 @@ def do_POST(self) -> None: ] self.send_json({"results": results}) return + if path == "/api/dispenser_color_map": + new_map = payload.get("map") + if not isinstance(new_map, dict): + self.send_json({"error": "body must be {\"map\": {\"1\": \"red\", ...}}"}, 400) + return + validated = {str(k): str(v) for k, v in new_map.items()} + DISPENSER_PRESS_TARGETS.clear() + DISPENSER_PRESS_TARGETS.update(validated) + DISPENSER_COLOR_MAP_PATH.parent.mkdir(parents=True, exist_ok=True) + DISPENSER_COLOR_MAP_PATH.write_text( + json.dumps(validated, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + self.send_json({"map": DISPENSER_PRESS_TARGETS}) + return if path == "/api/stop": self.send_json(stop_all()) return diff --git a/tools/run/run_rule_based_shake_real.sh b/tools/run/run_rule_based_shake_real.sh index 81535ea..299acd6 100755 --- a/tools/run/run_rule_based_shake_real.sh +++ b/tools/run/run_rule_based_shake_real.sh @@ -170,29 +170,6 @@ if [[ "${REQUIRE_ROBOT_STANDBY}" == "true" ]]; then fi fi -if [[ "${SKIP_CUP_HOLDER_PICK}" != "true" ]]; then - echo "[Azas] Cup-holder pick is required before shake. Starting measured holder side-grip pickup." - echo "[Azas] Cup-holder pick Z offset: ${CUP_HOLDER_PICK_Z_OFFSET_M} m (negative lowers grasp pose; calibration unchanged)." - echo "[Azas] Cup-holder grasp: width=${CUP_HOLDER_PICK_WIDTH_M} m force=${CUP_HOLDER_PICK_FORCE_N} N; shake is conservative to reduce drop risk." - python3 "${ROOT_DIR}/tools/run/pick_from_cup_holder_side_grip.py" \ - --service-prefix "${SERVICE_PREFIX}" \ - --config "${CUP_HOLDER_PICK_CONFIG}" \ - --approach-velocity 12.0 --approach-acceleration 16.0 \ - --descend-velocity 6.0 --descend-acceleration 10.0 \ - --lift-velocity 12.0 --lift-acceleration 16.0 \ - --place-final-z-offset-m "${CUP_HOLDER_PICK_Z_OFFSET_M}" \ - --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 \ - --ikin-timeout-sec 20.0 --ikin-retries 2 \ - --gripper-grasp-width-m "${CUP_HOLDER_PICK_WIDTH_M}" \ - --gripper-force-n "${CUP_HOLDER_PICK_FORCE_N}" \ - --post-grasp-settle-sec 0.8 \ - --z-max 0.28 \ - --execute --confirm ENABLE_CUP_HOLDER_PICK - echo "[Azas] Cup-holder pick completed; continuing to shake with grasped cup." -else - echo "[Azas] Cup-holder pick skipped only because SKIP_CUP_HOLDER_PICK=true was set by a wrapper that already completed it." -fi - parse_first_array() { python3 -c ' import re @@ -288,6 +265,29 @@ if [[ "${CONFIRM}" != "ENABLE_REAL_ROBOT_MOTION" ]]; then exit 1 fi +if [[ "${SKIP_CUP_HOLDER_PICK}" != "true" ]]; then + echo "[Azas] Cup-holder pick is required before shake. Starting measured holder side-grip pickup." + echo "[Azas] Cup-holder pick Z offset: ${CUP_HOLDER_PICK_Z_OFFSET_M} m (negative lowers grasp pose; calibration unchanged)." + echo "[Azas] Cup-holder grasp: width=${CUP_HOLDER_PICK_WIDTH_M} m force=${CUP_HOLDER_PICK_FORCE_N} N; shake is conservative to reduce drop risk." + python3 "${ROOT_DIR}/tools/run/pick_from_cup_holder_side_grip.py" \ + --service-prefix "${SERVICE_PREFIX}" \ + --config "${CUP_HOLDER_PICK_CONFIG}" \ + --approach-velocity 12.0 --approach-acceleration 16.0 \ + --descend-velocity 6.0 --descend-acceleration 10.0 \ + --lift-velocity 12.0 --lift-acceleration 16.0 \ + --place-final-z-offset-m "${CUP_HOLDER_PICK_Z_OFFSET_M}" \ + --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 \ + --ikin-timeout-sec 20.0 --ikin-retries 2 \ + --gripper-grasp-width-m "${CUP_HOLDER_PICK_WIDTH_M}" \ + --gripper-force-n "${CUP_HOLDER_PICK_FORCE_N}" \ + --post-grasp-settle-sec 0.8 \ + --z-max 0.28 \ + --execute --confirm ENABLE_CUP_HOLDER_PICK + echo "[Azas] Cup-holder pick completed; continuing to shake with grasped cup." +else + echo "[Azas] Cup-holder pick skipped only because SKIP_CUP_HOLDER_PICK=true was set by a wrapper that already completed it." +fi + exec ros2 launch azas_bringup tumbler_shake_sequence.launch.py \ enable_hardware:=true \ hardware_confirm:=ENABLE_REAL_ROBOT_MOTION \ diff --git a/tools/setup/bootstrap_team_pc.sh b/tools/setup/bootstrap_team_pc.sh index 280f194..147ac82 100755 --- a/tools/setup/bootstrap_team_pc.sh +++ b/tools/setup/bootstrap_team_pc.sh @@ -113,7 +113,6 @@ for pkg in \ dsr_bringup2 \ dsr_msgs2 \ dsr_moveit_config_m0609 \ - jarvis \ realsense2_camera; do check_ros_pkg "$pkg" || missing=1 done From 705ed301f2dc1ab694fbebce70f1752de320dcdc Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 14:35:08 +0900 Subject: [PATCH 04/88] Add cocktail cycle feature and related scripts for enhanced recipe execution --- docs/robot_pipeline_control.html | 95 ++++++++ tools/run/dispenser_color_scan_ros.sh | 9 + tools/run/listen_stt_recipe.py | 89 ++++++++ tools/run/publish_collision_scene_rviz.py | 239 +++++++++++++++++++++ tools/run/robot_pipeline_control_server.py | 72 ++++++- tools/run/run_color_recipe_sequence.py | 116 ++++++++++ 6 files changed, 613 insertions(+), 7 deletions(-) create mode 100755 tools/run/dispenser_color_scan_ros.sh create mode 100644 tools/run/listen_stt_recipe.py create mode 100644 tools/run/publish_collision_scene_rviz.py create mode 100644 tools/run/run_color_recipe_sequence.py diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index ccce9fa..ea114eb 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -171,6 +171,53 @@ } .quick-start-btn:hover { background: #1d3faa !important; } .danger-light { color: var(--red) !important; background: #fff1f2 !important; border-color: #fecdd3 !important; } + #cocktailCyclePanel { + display: none; + margin-top: 8px; + padding: 10px 14px 12px; + background: #f3eaff; + border: 1px solid #c4b5fd; + border-radius: 10px; + font-size: 13px; + } + #cocktailCyclePanel .cycle-label { + font-weight: 700; + color: #5b21b6; + margin-bottom: 8px; + font-size: 12px; + text-transform: uppercase; + letter-spacing: 0.05em; + } + #cocktailCyclePanel .cycle-checks { + display: flex; + flex-wrap: wrap; + gap: 6px 16px; + margin-bottom: 10px; + } + #cocktailCyclePanel label { + display: flex; + align-items: center; + gap: 5px; + cursor: pointer; + color: #3b0764; + user-select: none; + } + #cocktailCyclePanel input[type="checkbox"] { + accent-color: #7c3aed; + width: 15px; height: 15px; + cursor: pointer; + } + #cocktailCycleApplyBtn { + background: #7c3aed !important; + color: #fff !important; + border: none !important; + padding: 0 14px !important; + min-height: 30px !important; + font-size: 13px !important; + font-weight: 700 !important; + border-radius: 8px !important; + } + #cocktailCycleApplyBtn:hover { background: #6d28d9 !important; } details.settings { border-top: 1px dashed var(--line); @@ -621,6 +668,7 @@

Azas Robot Pipeline Control

+ @@ -635,6 +683,23 @@

Azas Robot Pipeline Control

+
+
🍹 칵테일 사이클 — 실행할 단계 선택
+
+ +
+
+ + + + + + + +
+ +
+
설정 보기 / 숨기기
@@ -1335,6 +1400,36 @@

파이프라인 단계

focusLog(); }); + document.getElementById("cocktailCycleBtn").addEventListener("click", () => { + const panel = document.getElementById("cocktailCyclePanel"); + panel.style.display = panel.style.display === "none" ? "block" : "none"; + }); + + document.getElementById("cocktailCycleApplyBtn").addEventListener("click", () => { + const cycleKeys = [...document.querySelectorAll(".cycle-step:checked")].map(cb => cb.value); + if (!cycleKeys.length) { + log.textContent = "⚠️ 선택된 단계가 없습니다. 최소 하나를 체크하세요."; + focusLog(); + return; + } + const includeConnect = document.getElementById("cycleIncludeConnect").checked; + const connectKeys = includeConnect + ? ["connect_robot", "status_check", "lift_robot", "start_camera"] + : []; + const keys = [...connectKeys, ...cycleKeys]; + selectedQueue = []; + addQueueItems(keys); + itemStatuses = new Map(); + updateSelectedCount(); + renderSteps(); + document.getElementById("cocktailCyclePanel").style.display = "none"; + flowEl.closest(".flow-panel")?.scrollIntoView({behavior: "smooth", block: "start"}); + log.textContent = `🍹 ${includeConnect ? "연결+" : ""}칵테일 사이클 (${keys.length}개 단계): ${keys.join(" → ")}`; + focusLog(); + // 바로 실행 시작 + document.getElementById("run").click(); + }); + document.getElementById("run").addEventListener("click", async () => { const selected = withCollisionScenePrereq(selectedQueue); resetResultBadges(); diff --git a/tools/run/dispenser_color_scan_ros.sh b/tools/run/dispenser_color_scan_ros.sh new file mode 100755 index 0000000..f347dc7 --- /dev/null +++ b/tools/run/dispenser_color_scan_ros.sh @@ -0,0 +1,9 @@ +#!/usr/bin/env bash +# 디스펜서 색상 스캔 (ROS 모드). +# 로봇이 color_scan_pose (joints [3,-12.7,44,-9,133,90]°)에 있어야 합니다. +# 카메라, TF, 로봇 드라이버가 실행 중이어야 합니다. +set -euo pipefail +ROOT="$(cd "$(dirname "$0")/../.." && pwd)" +source "$ROOT/install/local_setup.bash" 2>/dev/null || true +python3 "$ROOT/tools/perception/dispenser_color_scan.py" --ros \ + --output "$ROOT/outputs/dispenser_color_map.json" diff --git a/tools/run/listen_stt_recipe.py b/tools/run/listen_stt_recipe.py new file mode 100644 index 0000000..f93f99b --- /dev/null +++ b/tools/run/listen_stt_recipe.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +"""STT 레시피 대기: /azas/voice/recipe_decision 토픽을 수신해 outputs/latest_recipe.json 저장. + +voice_input 스텝이 먼저 실행(STT+LLM 노드 기동)된 상태에서 호출. +최대 --timeout 초 동안 대기하며 "make_cocktail" 인텐트를 수신하면 저장 후 종료. + +출력 포맷: + {"colors": ["red", "blue"], "pumps": {"red": 2, "blue": 1}, "recipe_id": "..."} +""" +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +OUTPUT_PATH = ROOT / "outputs" / "latest_recipe.json" + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--timeout", type=float, default=60.0, help="레시피 대기 최대 초") + args = parser.parse_args() + + try: + import rclpy + from rclpy.qos import qos_profile_sensor_data + except ImportError as e: + print(f"[listen_stt_recipe] rclpy 없음: {e}", file=sys.stderr) + return 1 + + import time + + received: dict | None = None + + def on_msg(msg) -> None: + nonlocal received + if received is not None: + return + try: + data = json.loads(msg.data) + except Exception: + return + intent = str(data.get("intent", "")).strip().lower() + if intent != "make_cocktail": + print(f"[listen_stt_recipe] intent={intent} 무시 (make_cocktail 아님)") + return + + colors = [str(c).strip().lower() for c in data.get("dispenser_ids", []) if c] + recipe_id = str(data.get("recipe_id", "custom")).strip() + + # pump 수: LLM이 pump_counts 필드를 생성하면 사용, 없으면 1 + pumps_raw = data.get("pump_counts") or {} + pumps = {c: int(pumps_raw.get(c, 1)) for c in colors} + + received = {"colors": colors, "pumps": pumps, "recipe_id": recipe_id} + print(f"[listen_stt_recipe] 수신: {received}") + + rclpy.init() + node = rclpy.create_node("listen_stt_recipe_node") + + # azas_voice가 퍼블리시하는 토픽 - 메시지 타입은 std_msgs/String (JSON payload) + from std_msgs.msg import String + node.create_subscription(String, "/azas/voice/recipe_decision", on_msg, qos_profile_sensor_data) + + print(f"[listen_stt_recipe] 레시피 대기 중... (최대 {args.timeout:.0f}초)") + deadline = time.time() + args.timeout + try: + while rclpy.ok() and received is None and time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.2) + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + if received is None: + print(f"[listen_stt_recipe] {args.timeout:.0f}초 내 레시피 없음", file=sys.stderr) + return 1 + + OUTPUT_PATH.parent.mkdir(parents=True, exist_ok=True) + OUTPUT_PATH.write_text(json.dumps(received, ensure_ascii=False, indent=2), encoding="utf-8") + print(f"[listen_stt_recipe] 저장: {OUTPUT_PATH}") + print(json.dumps(received, ensure_ascii=False)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/publish_collision_scene_rviz.py b/tools/run/publish_collision_scene_rviz.py new file mode 100644 index 0000000..befe98d --- /dev/null +++ b/tools/run/publish_collision_scene_rviz.py @@ -0,0 +1,239 @@ +#!/usr/bin/env python3 +"""Standalone collision scene visualizer for RViz. + +Publishes ALL collision boxes (workspace walls + table + dispenser body) +as MarkerArray on /azas/collision_scene/markers. + +Usage: + source /home/ssu/Azas/install/local_setup.bash + python3 tools/run/publish_collision_scene_rviz.py + +RViz: Add > MarkerArray > topic: /azas/collision_scene/markers + Fixed Frame: base_link +""" +from __future__ import annotations + +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(ROOT)) + +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from visualization_msgs.msg import Marker, MarkerArray +from geometry_msgs.msg import Pose +import yaml + +SAFETY_YAML = ROOT / "src" / "azas_bringup" / "config" / "safety.yaml" +DISPENSER_YAML = ROOT / "src" / "azas_bringup" / "config" / "measured_dispenser_collision.yaml" +CALIBRATION_YAML = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +WALL_THICKNESS = 0.04 +FRAME_ID = "base_link" + + +def transient_qos(depth: int = 10) -> QoSProfile: + return QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=depth, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + +def make_box_marker( + marker_id: int, ns: str, + cx: float, cy: float, cz: float, + sx: float, sy: float, sz: float, + r: float, g: float, b: float, a: float, + stamp, label: str = "", +) -> list[Marker]: + markers = [] + m = Marker() + m.header.frame_id = FRAME_ID + m.header.stamp = stamp + m.ns = ns + m.id = marker_id + m.type = Marker.CUBE + m.action = Marker.ADD + m.pose = Pose() + m.pose.position.x = cx + m.pose.position.y = cy + m.pose.position.z = cz + m.pose.orientation.w = 1.0 + m.scale.x = sx + m.scale.y = sy + m.scale.z = sz + m.color.r = r + m.color.g = g + m.color.b = b + m.color.a = a + markers.append(m) + + if label: + t = Marker() + t.header.frame_id = FRAME_ID + t.header.stamp = stamp + t.ns = ns + "_labels" + t.id = marker_id + 10000 + t.type = Marker.TEXT_VIEW_FACING + t.action = Marker.ADD + t.pose = Pose() + t.pose.position.x = cx + t.pose.position.y = cy + t.pose.position.z = cz + sz / 2.0 + 0.05 + t.pose.orientation.w = 1.0 + t.scale.z = 0.04 + t.color.r = 1.0 + t.color.g = 1.0 + t.color.b = 1.0 + t.color.a = 1.0 + t.text = label + markers.append(t) + return markers + + +def build_markers(stamp) -> list[Marker]: + markers: list[Marker] = [] + mid = 0 + + # ── 1. 워크스페이스 경계 벽 (safety.yaml) ────────────────────────────── + safety = yaml.safe_load(SAFETY_YAML.read_text()) + wb = safety["motion"]["workspace_bounds_m"] + x_min, x_max = wb["x_min"], wb["x_max"] + y_min, y_max = wb["y_min"], wb["y_max"] + z_min, z_max = wb["z_min"], wb["z_max"] + t = WALL_THICKNESS + height = z_max - z_min + cx = (x_min + x_max) / 2 + cy = (y_min + y_max) / 2 + cz = z_min + height / 2 + dx = x_max - x_min + dy = y_max - y_min + + walls = [ + # (label, cx, cy, cz, sx, sy, sz) + ("+Y wall", cx, y_max + t/2, cz, dx + 2*t, t, height), + ("-Y wall", cx, y_min - t/2, cz, dx + 2*t, t, height), + ("+X wall", x_max + t/2, cy, cz, t, dy, height), + ("-X wall", x_min - t/2, cy, cz, t, dy, height), + ("floor", cx, cy, z_min - t/2, dx, dy, t), + ("ceiling", cx, cy, z_max + t/2, dx, dy, t), + ] + for label, wcx, wcy, wcz, wsx, wsy, wsz in walls: + markers += make_box_marker(mid, "workspace_walls", + wcx, wcy, wcz, wsx, wsy, wsz, + 0.2, 0.6, 1.0, 0.18, stamp, label) + mid += 1 + + # ── 2. 테이블 (calibration.yaml) ────────────────────────────────────── + calib = yaml.safe_load(CALIBRATION_YAML.read_text()) + tbl = calib.get("table", {}) + if tbl: + tcx = tbl.get("center_xy_m", [0.45, 0.0])[0] + tcy = tbl.get("center_xy_m", [0.45, 0.0])[1] + tsx, tsy = tbl.get("size_xy_m", [1.2, 1.0]) + thick = tbl.get("thickness_m", 0.04) + surf_z = tbl.get("surface_z_m", 0.0) + markers += make_box_marker(mid, "table", + tcx, tcy, surf_z - thick/2, tsx, tsy, thick, + 0.6, 0.4, 0.2, 0.55, stamp, "table") + mid += 1 + + # ── 3. 디스펜서 합산 박스 (measured_dispenser_collision.yaml) ────────── + disp = yaml.safe_load(DISPENSER_YAML.read_text()) + for obj_id, obj in (disp.get("estimated_collision_objects") or {}).items(): + if obj.get("type") != "box": + continue + dcx, dcy, dcz = obj["center_xyz_m"] + dsx, dsy, dsz = obj["size_xyz_m"] + markers += make_box_marker(mid, "dispenser_collision", + dcx, dcy, dcz, dsx, dsy, dsz, + 1.0, 0.35, 0.05, 0.70, stamp, obj_id) + mid += 1 + + # ── 4. 디스펜서 front-hold 위치 (녹색 구) ────────────────────────────── + for hold_name, hold in (disp.get("front_hold_poses") or {}).items(): + xyz = hold.get("position_xyz_m") + if not xyz: + continue + s = Marker() + s.header.frame_id = FRAME_ID + s.header.stamp = stamp + s.ns = "dispenser_front_hold" + s.id = mid + s.type = Marker.SPHERE + s.action = Marker.ADD + s.pose = Pose() + s.pose.position.x, s.pose.position.y, s.pose.position.z = xyz + s.pose.orientation.w = 1.0 + s.scale.x = s.scale.y = s.scale.z = 0.03 + s.color.r = 0.0 + s.color.g = 0.95 + s.color.b = 0.3 + s.color.a = 0.9 + markers.append(s) + mid += 1 + + lbl = Marker() + lbl.header.frame_id = FRAME_ID + lbl.header.stamp = stamp + lbl.ns = "dispenser_front_hold_labels" + lbl.id = mid + lbl.type = Marker.TEXT_VIEW_FACING + lbl.action = Marker.ADD + lbl.pose = Pose() + lbl.pose.position.x, lbl.pose.position.y = xyz[0], xyz[1] + lbl.pose.position.z = xyz[2] + 0.06 + lbl.pose.orientation.w = 1.0 + lbl.scale.z = 0.035 + lbl.color.r = 0.0 + lbl.color.g = 0.95 + lbl.color.b = 0.3 + lbl.color.a = 1.0 + lbl.text = hold_name + markers.append(lbl) + mid += 1 + + return markers + + +class CollisionScenePublisher(Node): + def __init__(self): + super().__init__("collision_scene_rviz_publisher") + self.pub = self.create_publisher( + MarkerArray, "/azas/collision_scene/markers", transient_qos(10) + ) + self.timer = self.create_timer(2.0, self._publish) + self._publish() + self.get_logger().info( + "Publishing collision scene to /azas/collision_scene/markers\n" + "RViz: Add > MarkerArray > /azas/collision_scene/markers (Fixed Frame: base_link)" + ) + + def _publish(self): + stamp = self.get_clock().now().to_msg() + clear = Marker() + clear.header.frame_id = FRAME_ID + clear.header.stamp = stamp + clear.action = Marker.DELETEALL + markers = [clear] + build_markers(stamp) + self.pub.publish(MarkerArray(markers=markers)) + + +def main(): + rclpy.init() + node = CollisionScenePublisher() + try: + rclpy.spin(node) + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 7aa9e5b..c7b37a3 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -140,8 +140,34 @@ class Step: True, "MoveLine IK 대신 실측 관절 자세 사용: joint_2=-5°, joint_3=50°, joint_5=135° 상한으로 테이블 보기", ), - Step("voice_input", "음성 입력", "run", "ros2 launch azas_voice azas_voice.launch.py", True, False, "STT/레시피 노드"), - Step("recipe_generate", "레시피 생성", "blocked", "", False, False, "음성/레시피 토픽 통합 버튼은 별도 연결 필요"), + Step( + "color_scan", + "디스펜서 색상 스캔", + "run", + "tools/run/dispenser_color_scan_ros.sh", + True, + False, + "카메라+TF로 디스펜서 1~4 색상을 판별해 outputs/dispenser_color_map.json 저장. 로봇이 color_scan_pose에 있어야 함", + ), + Step("voice_input", "음성 입력 (STT+LLM 노드 시작)", "background", "ros2 launch azas_voice azas_voice.launch.py", True, False, "STT → /stt_result → llm_recipe_mapper → /azas/voice/recipe_decision"), + Step( + "listen_stt_recipe", + "STT 레시피 수신 대기 (60초)", + "run", + "tools/run/listen_stt_recipe.py --timeout 60", + True, + False, + "사용자가 말하면 /azas/voice/recipe_decision 수신 → outputs/latest_recipe.json 저장", + ), + Step( + "run_color_recipe_sequence", + "색상 레시피 디스펜서 시퀀스 실행", + "run", + "tools/run/run_color_recipe_sequence.py", + True, + True, + "latest_recipe.json + dispenser_color_map.json → 색깔→디스펜서ID 매핑 → 순서대로 move+press 실행", + ), Step( "side_grip", "PR #20 RealSense 컵 인식 후 side grip", @@ -446,6 +472,8 @@ def cleanup_doosan_stack(*, grace_sec: float = 3.0) -> list[str]: if old is not None: events.extend(terminate_process_tree(old, label="stored connect_robot", grace_sec=grace_sec)) + _service_ready_cache.clear() + if psutil is None: return events @@ -547,6 +575,7 @@ def cleanup_rg2_stack(*, grace_sec: float = 2.0) -> list[str]: if old is not None: events.extend(terminate_process_tree(old, label="stored connect_gripper", grace_sec=grace_sec)) events.extend(cleanup_matching_processes(RG2_STACK_PATTERNS, label="rg2 cleanup", grace_sec=grace_sec)) + _service_ready_cache.clear() return events @@ -721,6 +750,25 @@ def ros_service_names(timeout_sec: float = 6.0) -> tuple[set[str], str]: return {line.strip() for line in output.splitlines() if line.strip().startswith("/")}, output +# Per-process service cache: once a service is confirmed ready, skip re-checking +# for SERVICE_CACHE_TTL seconds. Avoids ~2s `ros2 service list` calls per step. +_service_ready_cache: dict[str, float] = {} +SERVICE_CACHE_TTL = 600.0 + + +def _cache_services(confirmed: set[str] | list[str]) -> None: + now = time.monotonic() + for svc in confirmed: + _service_ready_cache[svc] = now + + +def _all_cached(required: list[str]) -> bool: + if not required: + return True + cutoff = time.monotonic() - SERVICE_CACHE_TTL + return all(_service_ready_cache.get(svc, 0.0) > cutoff for svc in required) + + def action_server_count(action_name: str, timeout_sec: float = 4.0) -> tuple[int, str]: rc, output = ros2_call(f"ros2 action info {shlex.quote(action_name)}", timeout_sec=timeout_sec) if rc != 0: @@ -743,7 +791,7 @@ def wait_for_action_server(action_name: str, *, timeout_sec: float = 15.0) -> tu return False, f"action server did not become ready within {timeout_sec:.1f}s: {action_name}\n{last_output}" -def motion_services_ready(service_prefix: str) -> tuple[bool, str]: +def motion_services_ready(service_prefix: str) -> tuple[bool, str, set[str]]: clean = service_prefix.strip("/") or "dsr01" required = { f"/{clean}/motion/move_line", @@ -754,8 +802,8 @@ def motion_services_ready(service_prefix: str) -> tuple[bool, str]: services, output = ros_service_names(timeout_sec=6.0) missing = sorted(required - services) if missing: - return False, "missing motion services: " + ", ".join(missing) + "\n--- services ---\n" + output - return True, "motion services are present" + return False, "missing motion services: " + ", ".join(missing) + "\n--- services ---\n" + output, set() + return True, "motion services are present", services def wait_for_motion_services_ready( @@ -769,9 +817,11 @@ def wait_for_motion_services_ready( attempt = 0 while time.monotonic() < deadline: attempt += 1 - ready, output = motion_services_ready(service_prefix) + ready, output, services = motion_services_ready(service_prefix) last_output = output if ready: + if services: + _cache_services(services) return True, f"motion services became ready after {attempt} check(s)\n{output}" if proc is not None and proc.poll() is not None: return False, f"connect process exited while waiting for motion services\n{output}" @@ -1000,6 +1050,13 @@ def wait_for_required_services( timeout_sec: float = 20.0, proc: subprocess.Popen[str] | None = None, ) -> tuple[bool, str]: + # Fast path: all required services were recently confirmed → skip ros2 service list. + # Skip when proc is given: the caller is waiting for a freshly-spawned process to + # register its services, so a cached entry from the previous run must not mask the + # fact that the new process has not finished initialising yet. + if proc is None and _all_cached(required): + return True, f"required services confirmed via cache (TTL {SERVICE_CACHE_TTL:.0f}s): {', '.join(required)}" + deadline = time.monotonic() + max(timeout_sec, 0.1) last_output = "" attempt = 0 @@ -1009,6 +1066,7 @@ def wait_for_required_services( missing = [service for service in required if service not in services] last_output = output if not missing: + _cache_services(services) return True, f"required services became ready after {attempt} check(s): {', '.join(required)}" if proc is not None and proc.poll() is not None: return ( @@ -2007,7 +2065,7 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: if step.kind == "background": restart_output = "" if step.key == "connect_robot": - ready, ready_output = motion_services_ready(env["SERVICE_PREFIX"]) + ready, ready_output, _svc = motion_services_ready(env["SERVICE_PREFIX"]) if ready: robot_ready, robot_ready_output = doosan_robot_ready(env["SERVICE_PREFIX"]) if not robot_ready: diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py new file mode 100644 index 0000000..45f00de --- /dev/null +++ b/tools/run/run_color_recipe_sequence.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +"""색상 레시피 시퀀스 실행. + +outputs/latest_recipe.json (색깔 목록) + outputs/dispenser_color_map.json (위치→색깔) +를 읽어 색깔→디스펜서 ID를 매핑한 뒤 run_measured_dispenser_recipe_sequence.py 실행. + +사용법: + python3 tools/run/run_color_recipe_sequence.py + python3 tools/run/run_color_recipe_sequence.py --colors red:2,blue:1 # 직접 지정 +""" +from __future__ import annotations + +import argparse +import json +import subprocess +import sys +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +COLOR_MAP_PATH = ROOT / "outputs" / "dispenser_color_map.json" +RECIPE_PATH = ROOT / "outputs" / "latest_recipe.json" +SEQUENCE_SCRIPT = ROOT / "tools" / "run" / "run_measured_dispenser_recipe_sequence.py" +CONFIRM_PHRASE = "ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE" + + +def load_color_map() -> dict[str, str]: + """dispenser_id → color_name 매핑 로드.""" + if not COLOR_MAP_PATH.exists(): + print(f"[run_color_recipe] 색상 맵 없음: {COLOR_MAP_PATH}", file=sys.stderr) + print("[run_color_recipe] color_scan 스텝을 먼저 실행하세요.", file=sys.stderr) + sys.exit(1) + data = json.loads(COLOR_MAP_PATH.read_text(encoding="utf-8")) + return {str(k): str(v).lower().strip() for k, v in data.items()} + + +def color_to_dispenser_id(color: str, color_map: dict[str, str]) -> str | None: + """색깔 이름 → 디스펜서 ID (없으면 None).""" + color = color.lower().strip() + for did, c in color_map.items(): + if c == color: + return did + return None + + +def parse_colors_arg(raw: str) -> list[tuple[str, int]]: + """'red:2,blue:1' → [('red', 2), ('blue', 1)].""" + result = [] + for part in raw.split(","): + part = part.strip() + if not part: + continue + if ":" in part: + c, n = part.split(":", 1) + result.append((c.strip().lower(), int(n.strip()))) + else: + result.append((part.lower(), 1)) + return result + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--colors", default="", + help="직접 색깔 지정: 'red:2,blue:1' (생략 시 latest_recipe.json 사용)") + parser.add_argument("--confirm", action="store_true", + help=f"확인 구문({CONFIRM_PHRASE}) 자동 전달") + args = parser.parse_args() + + color_map = load_color_map() + print(f"[run_color_recipe] 색상 맵: {color_map}") + + # 색깔+펌프 수 결정 + if args.colors: + color_pumps = parse_colors_arg(args.colors) + else: + if not RECIPE_PATH.exists(): + print(f"[run_color_recipe] 레시피 없음: {RECIPE_PATH}", file=sys.stderr) + print("[run_color_recipe] listen_stt_recipe 스텝을 먼저 실행하세요.", file=sys.stderr) + return 1 + recipe = json.loads(RECIPE_PATH.read_text(encoding="utf-8")) + colors = recipe.get("colors", []) + pumps = recipe.get("pumps", {}) + color_pumps = [(c, int(pumps.get(c, 1))) for c in colors] + + print(f"[run_color_recipe] 레시피 색깔+펌프: {color_pumps}") + + # 색깔 → 디스펜서 ID 매핑 + sequence: list[str] = [] + for color, pumps in color_pumps: + did = color_to_dispenser_id(color, color_map) + if did is None: + print(f"[run_color_recipe] '{color}' 색깔이 색상 맵에 없음 → 건너뜀", file=sys.stderr) + continue + for _ in range(pumps): + sequence.append(did) + + if not sequence: + print("[run_color_recipe] 실행할 디스펜서 없음 (색상 맵과 레시피 색깔이 불일치)", file=sys.stderr) + return 1 + + dispenser_ids_str = ",".join(sequence) + print(f"[run_color_recipe] 실행 순서: {dispenser_ids_str}") + + cmd = [ + sys.executable, str(SEQUENCE_SCRIPT), + "--dispenser-ids", dispenser_ids_str, + ] + if args.confirm: + cmd += ["--confirm", CONFIRM_PHRASE] + + print(f"[run_color_recipe] 실행: {' '.join(cmd)}") + result = subprocess.run(cmd, check=False) + return result.returncode + + +if __name__ == "__main__": + raise SystemExit(main()) From 75484951a4f060545c32fb491a489955ea2f6fbb Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 15:37:52 +0900 Subject: [PATCH 05/88] Adopt yolo cup uprighting perception assets Integrate the upstream cup detector model and image-only orientation utilities while keeping Azas motion pose generation behind the existing detection-to-TF pipeline. Constraint: Cup poses and robot motion must continue through /azas/cup_detection and /jarvis/tumbler_dispenser/tumbler_pose; upstream mock/base-coordinate motion code was not imported. Rejected: Vendoring yolo_pick_demo as an executable motion package | it directly computes base coordinates and includes mock pose values that conflict with Azas safety rules. Confidence: high Scope-risk: moderate Directive: Do not reintroduce upstream motion or mock coordinate paths without replacing them with Azas topic/TF contracts and real calibration evidence. Tested: PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest -q src/azas_perception/test/test_depth_and_detection_logic.py; python3 -m py_compile perception and launch files; colcon build --packages-select azas_perception azas_bringup --symlink-install --allow-overriding azas_bringup Not-tested: Live RealSense/YOLO inference and real robot motion. --- .gitattributes | 1 + docs/yolo_cup_uprighting_merge.md | 32 +++++++ .../launch/cocktail_dryrun.launch.py | 7 +- .../launch/robot_connection_control.launch.py | 7 +- .../launch/yolo_perception.launch.py | 10 ++- .../launch/yolo_to_floor_place.launch.py | 7 +- .../azas_perception/cup_uprighting_vision.py | 86 +++++++++++++++++++ .../yolo_tumbler_detector_node.py | 22 ++++- .../config/yolo_cup_uprighting_best.pt | 3 + src/azas_perception/setup.py | 5 +- .../test/test_depth_and_detection_logic.py | 27 ++++++ 11 files changed, 200 insertions(+), 7 deletions(-) create mode 100644 docs/yolo_cup_uprighting_merge.md create mode 100644 src/azas_perception/azas_perception/cup_uprighting_vision.py create mode 100644 src/azas_perception/config/yolo_cup_uprighting_best.pt diff --git a/.gitattributes b/.gitattributes index 5f27808..912a0a0 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1 +1,2 @@ cup_classifier_best.pth filter=lfs diff=lfs merge=lfs -text +src/azas_perception/config/yolo_cup_uprighting_best.pt filter=lfs diff=lfs merge=lfs -text diff --git a/docs/yolo_cup_uprighting_merge.md b/docs/yolo_cup_uprighting_merge.md new file mode 100644 index 0000000..842c6ea --- /dev/null +++ b/docs/yolo_cup_uprighting_merge.md @@ -0,0 +1,32 @@ +# yolo_cup_uprighting merge note + +This branch integrates the safe perception parts of +`https://github.com/ssarahstar/yolo_cup_uprighting` into Azas. + +## Integrated + +- `best.pt` is copied into `src/azas_perception/config/yolo_cup_uprighting_best.pt`. + It is packaged by `azas_perception/setup.py` and tracked through Git LFS. +- YOLO launch defaults now resolve the packaged model through + `FindPackageShare("azas_perception")`, removing the old local-only + `/home/ssu/Downloads/best.pt` default. +- Image-only cup axis / red-marker helpers are adapted into + `azas_perception.cup_uprighting_vision` with unit tests. + +## Deliberately not integrated + +The upstream `yolo_pick_demo` motion node directly computes base-frame points +and contains mock coordinates for local testing. That does not match the Azas +project rule that cup poses must flow through: + +`/azas/cup_detection` -> `/jarvis/tumbler_dispenser/tumbler_pose` + +No upstream motion sequence, mock coordinate, or generated robot trajectory was +merged. Real robot motion should continue to consume the validated +`PoseStamped` from the Azas TF bridge. + +## Validation target + +- Perception unit tests pass. +- Launch files keep the same override surface: operators can still pass + `model_path:=...` when testing another model. diff --git a/src/azas_bringup/launch/cocktail_dryrun.launch.py b/src/azas_bringup/launch/cocktail_dryrun.launch.py index 0fc4a81..7fb9289 100644 --- a/src/azas_bringup/launch/cocktail_dryrun.launch.py +++ b/src/azas_bringup/launch/cocktail_dryrun.launch.py @@ -54,7 +54,12 @@ def generate_launch_description(): DeclareLaunchArgument("llm_api_key_env", default_value="OPENAI_API_KEY"), DeclareLaunchArgument("stt_topic", default_value="/stt_result"), DeclareLaunchArgument("run_yolo", default_value="true"), - DeclareLaunchArgument("model_path", default_value="/home/ssu/Downloads/best.pt"), + DeclareLaunchArgument( + "model_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_perception"), "config", "yolo_cup_uprighting_best.pt"] + ), + ), DeclareLaunchArgument("color_topic", default_value="/camera/camera/color/image_raw"), DeclareLaunchArgument("depth_topic", default_value="/camera/camera/aligned_depth_to_color/image_raw"), DeclareLaunchArgument("camera_info_topic", default_value="/camera/camera/color/camera_info"), diff --git a/src/azas_bringup/launch/robot_connection_control.launch.py b/src/azas_bringup/launch/robot_connection_control.launch.py index 5c3283e..9e6483c 100644 --- a/src/azas_bringup/launch/robot_connection_control.launch.py +++ b/src/azas_bringup/launch/robot_connection_control.launch.py @@ -87,7 +87,12 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("selected_dispenser_id", default_value="1"), - DeclareLaunchArgument("model_path", default_value="/home/ssu/Downloads/best.pt"), + DeclareLaunchArgument( + "model_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_perception"), "config", "yolo_cup_uprighting_best.pt"] + ), + ), DeclareLaunchArgument("color_topic", default_value="/camera/camera/color/image_raw"), DeclareLaunchArgument("depth_topic", default_value="/camera/camera/aligned_depth_to_color/image_raw"), DeclareLaunchArgument("camera_info_topic", default_value="/camera/camera/color/camera_info"), diff --git a/src/azas_bringup/launch/yolo_perception.launch.py b/src/azas_bringup/launch/yolo_perception.launch.py index 9115b6e..67cd5ac 100644 --- a/src/azas_bringup/launch/yolo_perception.launch.py +++ b/src/azas_bringup/launch/yolo_perception.launch.py @@ -1,8 +1,9 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare def generate_launch_description(): @@ -10,7 +11,12 @@ def generate_launch_description(): # docs. Override these launch args if the camera driver is launched with a # different namespace; do not patch coordinates or frames in code. return LaunchDescription([ - DeclareLaunchArgument("model_path", default_value="/home/ssu/Downloads/best.pt"), + DeclareLaunchArgument( + "model_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_perception"), "config", "yolo_cup_uprighting_best.pt"] + ), + ), DeclareLaunchArgument("color_topic", default_value="/camera/camera/color/image_raw"), DeclareLaunchArgument("depth_topic", default_value="/camera/camera/aligned_depth_to_color/image_raw"), DeclareLaunchArgument("camera_info_topic", default_value="/camera/camera/color/camera_info"), diff --git a/src/azas_bringup/launch/yolo_to_floor_place.launch.py b/src/azas_bringup/launch/yolo_to_floor_place.launch.py index 3ddab18..b2987a8 100644 --- a/src/azas_bringup/launch/yolo_to_floor_place.launch.py +++ b/src/azas_bringup/launch/yolo_to_floor_place.launch.py @@ -96,7 +96,12 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("selected_dispenser_id", default_value="1"), - DeclareLaunchArgument("model_path", default_value="/home/ssu/Downloads/best.pt"), + DeclareLaunchArgument( + "model_path", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_perception"), "config", "yolo_cup_uprighting_best.pt"] + ), + ), DeclareLaunchArgument("color_topic", default_value="/camera/camera/color/image_raw"), DeclareLaunchArgument("depth_topic", default_value="/camera/camera/aligned_depth_to_color/image_raw"), DeclareLaunchArgument("camera_info_topic", default_value="/camera/camera/color/camera_info"), diff --git a/src/azas_perception/azas_perception/cup_uprighting_vision.py b/src/azas_perception/azas_perception/cup_uprighting_vision.py new file mode 100644 index 0000000..75eacd3 --- /dev/null +++ b/src/azas_perception/azas_perception/cup_uprighting_vision.py @@ -0,0 +1,86 @@ +from __future__ import annotations + +import math + +import cv2 +import numpy as np + + +def calculate_cup_major_axis_angle_rad(image_bgr: np.ndarray, bbox: tuple[int, int, int, int]) -> float: + """Estimate the long-axis angle of a cup crop in image coordinates. + + This is the non-motion perception portion adapted from the + yolo_cup_uprighting demo. It intentionally returns only an image-plane + diagnostic angle; robot poses and trajectories must still come from the + Azas depth/TF pipeline and motion stack. + """ + + if image_bgr is None or image_bgr.size == 0: + return 0.0 + x1, y1, x2, y2 = _clamp_bbox(image_bgr, bbox) + roi = image_bgr[y1:y2, x1:x2] + if roi.size == 0: + return 0.0 + + gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) + _, threshold = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU) + contours, _ = cv2.findContours(threshold, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if not contours: + return 0.0 + + rect = cv2.minAreaRect(max(contours, key=cv2.contourArea)) + (_, _), (width, height), angle_deg = rect + if width < height: + angle_deg += 90.0 + return float(math.radians(angle_deg)) + + +def is_red_marker_aligned_with_angle( + image_bgr: np.ndarray, + bbox: tuple[int, int, int, int], + theta_rad: float, +) -> bool: + """Return whether a red cup marker lies in the positive theta direction. + + If the marker is absent or the crop is invalid, the function returns True + so callers do not invent a robot-side correction from missing evidence. + """ + + if image_bgr is None or image_bgr.size == 0: + return True + x1, y1, x2, y2 = _clamp_bbox(image_bgr, bbox) + roi = image_bgr[y1:y2, x1:x2] + if roi.size == 0: + return True + + hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) + lower_red1 = np.array([0, 100, 100]) + upper_red1 = np.array([10, 255, 255]) + lower_red2 = np.array([160, 100, 100]) + upper_red2 = np.array([180, 255, 255]) + mask = cv2.inRange(hsv, lower_red1, upper_red1) + cv2.inRange(hsv, lower_red2, upper_red2) + + moments = cv2.moments(mask) + if moments["m00"] == 0: + return True + + marker = np.array( + [ + moments["m10"] / moments["m00"] - (roi.shape[1] / 2.0), + moments["m01"] / moments["m00"] - (roi.shape[0] / 2.0), + ], + dtype=float, + ) + axis = np.array([math.cos(theta_rad), math.sin(theta_rad)], dtype=float) + return bool(np.dot(marker, axis) > 0.0) + + +def _clamp_bbox(image_bgr: np.ndarray, bbox: tuple[int, int, int, int]) -> tuple[int, int, int, int]: + height, width = image_bgr.shape[:2] + x1, y1, x2, y2 = map(int, bbox) + return ( + max(0, min(x1, width)), + max(0, min(y1, height)), + max(0, min(x2, width)), + max(0, min(y2, height)), + ) diff --git a/src/azas_perception/azas_perception/yolo_tumbler_detector_node.py b/src/azas_perception/azas_perception/yolo_tumbler_detector_node.py index 9709732..0e8e86d 100644 --- a/src/azas_perception/azas_perception/yolo_tumbler_detector_node.py +++ b/src/azas_perception/azas_perception/yolo_tumbler_detector_node.py @@ -8,6 +8,7 @@ import cv2 import numpy as np import rclpy +from ament_index_python.packages import get_package_share_directory from azas_interfaces.msg import CupDetection from geometry_msgs.msg import Pose from rclpy.node import Node @@ -27,6 +28,25 @@ YOLO = None +def default_yolo_model_path() -> str: + """Return the packaged yolo_cup_uprighting model when installed.""" + + try: + packaged = ( + Path(get_package_share_directory("azas_perception")) + / "config" + / "yolo_cup_uprighting_best.pt" + ) + except Exception: + packaged = Path() + if packaged.exists(): + return str(packaged) + source_tree = Path(__file__).resolve().parents[1] / "config" / "yolo_cup_uprighting_best.pt" + if source_tree.exists(): + return str(source_tree) + return "/home/ssu/Downloads/best.pt" + + @dataclass(frozen=True) class Detection2D: x_min: int @@ -76,7 +96,7 @@ class YoloTumblerDetectorNode(Node): def __init__(self): super().__init__("yolo_tumbler_detector_node") - self.declare_parameter("model_path", "/home/ssu/Downloads/best.pt") + self.declare_parameter("model_path", default_yolo_model_path()) self.declare_parameter("color_topic", "/camera/camera/color/image_raw") self.declare_parameter("depth_topic", "/camera/camera/aligned_depth_to_color/image_raw") self.declare_parameter("camera_info_topic", "/camera/camera/color/camera_info") diff --git a/src/azas_perception/config/yolo_cup_uprighting_best.pt b/src/azas_perception/config/yolo_cup_uprighting_best.pt new file mode 100644 index 0000000..ff4f02e --- /dev/null +++ b/src/azas_perception/config/yolo_cup_uprighting_best.pt @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ead799aa50f916e9942f46da9ec1875bbcec792d3ed01b535f61fa63f1bc9e9a +size 23860084 diff --git a/src/azas_perception/setup.py b/src/azas_perception/setup.py index 524d99f..830271e 100644 --- a/src/azas_perception/setup.py +++ b/src/azas_perception/setup.py @@ -10,7 +10,10 @@ data_files=[ ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), (f"share/{package_name}", ["package.xml"]), - (f"share/{package_name}/config", glob("config/*.npy") + glob("config/*.yaml")), + ( + f"share/{package_name}/config", + glob("config/*.npy") + glob("config/*.yaml") + glob("config/*.pt"), + ), ], install_requires=["setuptools"], zip_safe=True, diff --git a/src/azas_perception/test/test_depth_and_detection_logic.py b/src/azas_perception/test/test_depth_and_detection_logic.py index fe0cdb6..aee558e 100644 --- a/src/azas_perception/test/test_depth_and_detection_logic.py +++ b/src/azas_perception/test/test_depth_and_detection_logic.py @@ -1,11 +1,17 @@ import pytest import numpy as np +import cv2 +from azas_perception.cup_uprighting_vision import ( + calculate_cup_major_axis_angle_rad, + is_red_marker_aligned_with_angle, +) from azas_perception.depth_projection import CameraIntrinsics, pixel_depth_to_camera_point from azas_perception.yolo_tumbler_detector_node import ( BboxHeightStats, Detection2D, YoloTumblerDetectorNode, + default_yolo_model_path, ) @@ -223,3 +229,24 @@ def test_height_orientation_can_classify_inverted_when_thresholds_are_configured stat_name="p90", min_valid_ratio=0.1, ) == "inverted" + + +def test_packaged_yolo_cup_uprighting_model_is_default_when_available(): + assert default_yolo_model_path().endswith("yolo_cup_uprighting_best.pt") + + +def test_cup_uprighting_major_axis_angle_uses_image_only(): + image = np.zeros((120, 120, 3), dtype=np.uint8) + cv2.rectangle(image, (20, 50), (100, 70), (255, 255, 255), thickness=-1) + + theta = calculate_cup_major_axis_angle_rad(image, (0, 0, 120, 120)) + + assert abs(theta) < 0.1 or abs(abs(theta) - np.pi) < 0.1 + + +def test_red_marker_alignment_reports_direction_without_robot_pose(): + image = np.zeros((100, 100, 3), dtype=np.uint8) + cv2.circle(image, (75, 50), 8, (0, 0, 255), thickness=-1) + + assert is_red_marker_aligned_with_angle(image, (0, 0, 100, 100), 0.0) + assert not is_red_marker_aligned_with_angle(image, (0, 0, 100, 100), np.pi) From 5e552a7df27316a3251a11f0a24e23caafb91a34 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 16:59:20 +0900 Subject: [PATCH 06/88] Update calibration and launch files for improved color scanning pose and collision handling --- docs/robot_pipeline_control.html | 13 +-- src/azas_bringup/config/calibration.yaml | 10 +- .../launch/doosan_moveit_rviz_only.launch.py | 16 ++- .../checks/check_dispenser_recipe_sequence.py | 106 ------------------ tools/run/dispenser_color_scan_ros.sh | 2 +- tools/run/robot_pipeline_control_server.py | 82 +++----------- 6 files changed, 44 insertions(+), 185 deletions(-) delete mode 100755 tools/checks/check_dispenser_recipe_sequence.py diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index ea114eb..6bbe380 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -668,7 +668,7 @@

Azas Robot Pipeline Control

- + @@ -686,9 +686,10 @@

Azas Robot Pipeline Control

🍹 칵테일 사이클 — 실행할 단계 선택
- +
+ @@ -821,7 +822,7 @@

RealSense 카메라 화면

선택 실행 순서

-
큐 순서대로 실행됩니다. 레시피 디스펜서 단계는 컵 놓기/프레스/다시 잡기를 통합 루프로 실행합니다.
+
큐 순서대로 실행됩니다. 필요한 충돌 장면 단계는 실행 전에 자동으로 보강됩니다.
대기
@@ -906,7 +907,6 @@

파이프라인 단계

if (key === "start_camera" || key === "detect_cup_lid") return "camera"; if (key.includes("gripper")) return "grip"; if (key.startsWith("teach_front_hold_")) return "teach"; - if (key === "run_dispenser_recipe_sequence") return "press"; if (key === "start_collision_scene") return "core"; if (key.startsWith("move_to_dispenser") || key.includes("cup") || key.includes("holder") || key.includes("side_grip") || key.includes("home") || key.includes("lift")) return "move"; if (key.startsWith("press_dispenser")) return "press"; @@ -962,8 +962,7 @@

파이프라인 단계

}); function needsCollisionScene(key) { - return key === "run_dispenser_recipe_sequence" - || key === "shake_closed_cup" + return key === "shake_closed_cup" || key.startsWith("move_to_dispenser_") || key.startsWith("pick_from_dispenser_"); } @@ -1414,7 +1413,7 @@

파이프라인 단계

} const includeConnect = document.getElementById("cycleIncludeConnect").checked; const connectKeys = includeConnect - ? ["connect_robot", "status_check", "lift_robot", "start_camera"] + ? ["connect_robot", "status_check", "lift_robot", "start_camera", "start_collision_scene"] : []; const keys = [...connectKeys, ...cycleKeys]; selectedQueue = []; diff --git a/src/azas_bringup/config/calibration.yaml b/src/azas_bringup/config/calibration.yaml index f66ad26..fea0a72 100644 --- a/src/azas_bringup/config/calibration.yaml +++ b/src/azas_bringup/config/calibration.yaml @@ -18,14 +18,14 @@ hand_eye: rpy_rad: null # 확인 필요: fill from npy after deciding canonical file # Saved robot pose for dispenser color scanning. -# Joint values from yolo_cup_pick_node.launch.py camera_home_joint_*_deg defaults (measured). +# 직접 측정한 디스펜서 색상 스캔 포즈 (2026-06-05). color_scan_pose: - source: yolo_cup_pick_node.launch.py camera_home defaults + source: operator_measured ee_link: link_6 - joints_deg: [3.0, -12.7, 44.0, -9.0, 133.0, 90.0] - joints_rad: [0.0524, -0.2217, 0.7679, -0.1571, 2.3213, 1.5708] + joints_deg: [0.0, 10.0, 20.0, 0.0, 90.0, 0.0] + joints_rad: [0.0, 0.1745, 0.3491, 0.0, 1.5708, 0.0] joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] - cartesian_xyz_m: [0.45, 0.0, 0.64] + cartesian_xyz_m: null cartesian_frame: base_link cup_offsets: default: diff --git a/src/azas_bringup/launch/doosan_moveit_rviz_only.launch.py b/src/azas_bringup/launch/doosan_moveit_rviz_only.launch.py index 1901852..dac992c 100644 --- a/src/azas_bringup/launch/doosan_moveit_rviz_only.launch.py +++ b/src/azas_bringup/launch/doosan_moveit_rviz_only.launch.py @@ -2,8 +2,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from moveit_configs_utils import MoveItConfigsBuilder @@ -49,9 +50,20 @@ def rviz_node_function(context): def generate_launch_description(): + collision_scene = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare("azas_bringup"), + "launch", + "workspace_collision_scene.launch.py", + ]) + ]) + ) + return LaunchDescription( [ DeclareLaunchArgument("model", default_value="m0609"), + collision_scene, OpaqueFunction(function=rviz_node_function), ] ) diff --git a/tools/checks/check_dispenser_recipe_sequence.py b/tools/checks/check_dispenser_recipe_sequence.py deleted file mode 100755 index 87c53f7..0000000 --- a/tools/checks/check_dispenser_recipe_sequence.py +++ /dev/null @@ -1,106 +0,0 @@ -#!/usr/bin/env python3 -"""Static regression check for the measured dispenser recipe sequence panel step.""" - -from __future__ import annotations - -import importlib.util -import subprocess -import sys -from pathlib import Path - -ROOT = Path(__file__).resolve().parents[2] -PANEL_PATH = ROOT / "tools" / "run" / "robot_pipeline_control_server.py" -RECIPE_SCRIPT = ROOT / "tools" / "run" / "run_measured_dispenser_recipe_sequence.py" - - -def load_panel_module(): - spec = importlib.util.spec_from_file_location("robot_pipeline_control_server", PANEL_PATH) - if spec is None or spec.loader is None: - raise RuntimeError(f"could not load {PANEL_PATH}") - module = importlib.util.module_from_spec(spec) - sys.modules[spec.name] = module - spec.loader.exec_module(module) - return module - - -def main() -> int: - if not RECIPE_SCRIPT.is_file(): - print(f"[FAIL] missing recipe script: {RECIPE_SCRIPT}") - return 1 - - panel = load_panel_module() - steps = {step.key: step for step in panel.STEPS} - step = steps.get("run_dispenser_recipe_sequence") - if step is None or not step.implemented or not step.real_motion: - print("[FAIL] panel recipe sequence step is not real/implemented") - return 1 - - services = panel.required_services_for_step(step, "dsr01") - required = { - "/jarvis/rg2/set_width", - "/dsr01/motion/move_joint", - "/dsr01/motion/move_line", - "/dsr01/motion/move_wait", - "/dsr01/motion/ikin", - "/dsr01/motion/check_motion", - "/dsr01/system/get_robot_state", - "/dsr01/tcp/get_current_tcp", - "/dsr01/tcp/set_current_tcp", - "/dsr01/aux_control/get_current_posx", - } - missing = sorted(required.difference(services)) - if missing: - print(f"[FAIL] recipe sequence missing required service gates: {missing}") - return 1 - - command = panel.command_for( - step, - { - "service_prefix": "dsr01", - "recipe_dispenser_ids": "1,3,2", - "dispenser_tcp_name": "GripperDA_v1_jarvis", - "armed": True, - }, - ) - checks = [ - "run_measured_dispenser_recipe_sequence.py", - "--dispenser-ids 1,3,2", - "--dispenser-tcp-name GripperDA_v1_jarvis", - "ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE", - ] - for expected in checks: - if expected not in command: - print(f"[FAIL] recipe command missing: {expected}") - print(command) - return 1 - - dry = subprocess.run( - [sys.executable, str(RECIPE_SCRIPT), "--dispenser-ids", "1,3,2", "--service-prefix", "dsr01"], - cwd=str(ROOT), - check=False, - text=True, - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT, - timeout=10, - ) - if dry.returncode != 0: - print("[FAIL] recipe sequence dry-run failed") - print(dry.stdout) - return 1 - for expected in [ - "[DRY-RUN]", - "dispenser_ids=1,3,2", - "source=existing measured front_hold poses and taught dispenser press poses", - "move/release -> press -> re-grasp/lift", - ]: - if expected not in dry.stdout: - print(f"[FAIL] recipe dry-run missing: {expected}") - print(dry.stdout) - return 1 - - print("[PASS] measured dispenser recipe sequence panel step is wired") - return 0 - - -if __name__ == "__main__": - raise SystemExit(main()) diff --git a/tools/run/dispenser_color_scan_ros.sh b/tools/run/dispenser_color_scan_ros.sh index f347dc7..df60033 100755 --- a/tools/run/dispenser_color_scan_ros.sh +++ b/tools/run/dispenser_color_scan_ros.sh @@ -1,6 +1,6 @@ #!/usr/bin/env bash # 디스펜서 색상 스캔 (ROS 모드). -# 로봇이 color_scan_pose (joints [3,-12.7,44,-9,133,90]°)에 있어야 합니다. +# 로봇이 color_scan_pose (joints [0,10,20,0,90,0]°)에 있어야 합니다. # 카메라, TF, 로봇 드라이버가 실행 중이어야 합니다. set -euo pipefail ROOT="$(cd "$(dirname "$0")/../.." && pwd)" diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index c7b37a3..9464a4a 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -140,6 +140,15 @@ class Step: True, "MoveLine IK 대신 실측 관절 자세 사용: joint_2=-5°, joint_3=50°, joint_5=135° 상한으로 테이블 보기", ), + Step( + "move_to_color_scan_pose", + "색상 스캔 포즈 이동 [0,10,20,0,90,0]°", + "run", + "tools/run/direct_movej_joints.py --j1 0 --j2 10 --j3 20 --j4 0 --j5 90 --j6 0 --velocity 30 --acceleration 30 --execute --confirm ENABLE_DIRECT_MOVEJ", + True, + True, + "색상 스캔 전 카메라가 디스펜서를 향하는 포즈로 이동. color_scan_pose: [0,10,20,0,90,0]°", + ), Step( "color_scan", "디스펜서 색상 스캔", @@ -147,7 +156,7 @@ class Step: "tools/run/dispenser_color_scan_ros.sh", True, False, - "카메라+TF로 디스펜서 1~4 색상을 판별해 outputs/dispenser_color_map.json 저장. 로봇이 color_scan_pose에 있어야 함", + "카메라+TF로 디스펜서 1~4 색상을 판별해 outputs/dispenser_color_map.json 저장. 로봇이 color_scan_pose [0,10,20,0,90,0]°에 있어야 함", ), Step("voice_input", "음성 입력 (STT+LLM 노드 시작)", "background", "ros2 launch azas_voice azas_voice.launch.py", True, False, "STT → /stt_result → llm_recipe_mapper → /azas/voice/recipe_decision"), Step( @@ -172,13 +181,12 @@ class Step: "side_grip", "PR #20 RealSense 컵 인식 후 side grip", "background", - "ros2 launch dsr_practice yolo_cup_pick_node.launch.py auto_pick:=false grasp_mode:=side", + "ros2 launch dsr_practice yolo_cup_pick_node.launch.py auto_pick:=true grasp_mode:=side moveit_controller_name:=/dsr01/dsr_moveit_controller", True, True, - "PR #20 merged manual side-grip flow: 카메라 창에서 cup 탐지 후 p 키로 side-grip 실행", + "auto_pick=true: 컵 감지 즉시 자동 side-grip. 서버 command_for()가 실제 파라미터를 오버라이드함. 패널에서 OpenCV 창에서 확인 후 ESC 종료", ), Step("gripper_soft_grasp", "그리퍼 살짝 잡기", "run", "ros2 service call /jarvis/rg2/set_width azas_interfaces/srv/SetGripper", True, True, "큰 컵용: 완전 close 대신 폭 75mm/약한 힘으로 살짝 오므림"), - Step("gripper_open", "그리퍼 full open / 컵 놓기 검증", "run", "tools/run/rg2_full_open_verify.sh", True, True, "컵을 배출구 아래에 둔 뒤 RG2 full-open 명령 success=True 검증"), Step( "move_to_dispenser_1", "고정 디스펜서 1 배출구 아래로 컵 이동", @@ -287,17 +295,6 @@ class Step: True, "front_hold_poses.dispenser_4 재사용: RG2 open→측정 front-hold 접근→soft side-grip→수직 lift", ), - Step( - "run_dispenser_recipe_sequence", - "레시피 디스펜서 통합 실행 / move→press→pick", - "run", - "tools/run/run_measured_dispenser_recipe_sequence.py --dispenser-ids 1,2,3,4", - True, - True, - "설정의 RECIPE_DISPENSER_IDS 순서대로 실행. 컵 이동/놓기와 다시 side-grip 집기는 통합 ROS 클라이언트로 처리해 반복 명령 실행 시간을 줄임", - ), - Step("repeat_dispense", "5,6 반복", "blocked", "", False, True, "레시피별 디스펜서 ID 반복 로직 필요"), - Step("pick_lid", "뚜껑을 집기", "blocked", "", False, True, "뚜껑 좌표/그리퍼 폭 필요"), Step( "place_cup_holder", "컵을 컵홀더에 놓기 / side grip", @@ -307,7 +304,6 @@ class Step: True, "실제모션 후보: 측정된 side_grip_place pre_place→place_final→RG2 full-open→retreat", ), - Step("attach_lid", "뚜껑을 컵에 끼우기", "blocked", "", False, True, "뚜껑 체결 동작 미구현"), Step( "shake_rviz_preview", "쉐이킹 RViz 미리보기 / 무모션", @@ -318,8 +314,6 @@ class Step: "실제 로봇 미사용: 별도 ROS_DOMAIN_ID에서 쉐이킹 궤적/마커를 RViz로 표시", ), Step("shake_closed_cup", "컵홀더 컵 다시 잡기 후 쉐이킹", "run", "tools/run/pick_from_cup_holder_side_grip.py && tools/run/run_rule_based_shake_real.sh", True, True, "시작 시 컵홀더에 놓인 닫힌 컵을 측정된 cup_holder.side_grip_place pose로 다시 side-grip 픽업한 뒤, J3 양수 고정 및 J4/J5/J6 트위스트 쉐이킹을 실행"), - Step("remove_lid", "뚜껑을 열기/제거하기", "blocked", "", False, True, "뚜껑 제거 동작 미구현"), - Step("pour_cocktail", "칵테일을 다른 컵에 붓기", "blocked", "", False, True, "따르기 경로 미구현"), ] processes: dict[str, subprocess.Popen[str]] = {} @@ -897,8 +891,6 @@ def text_output(output: str | bytes | None) -> str: def required_services_for_step(step: Step, service_prefix: str) -> list[str]: clean = service_prefix.strip("/") or "dsr01" - if step.key == "gripper_open": - return ["/jarvis/rg2/set_width"] if step.key == "gripper_soft_grasp": return ["/jarvis/rg2/set_width"] if step.key.startswith("teach_front_hold_"): @@ -952,19 +944,6 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: f"/{clean}/aux_control/get_current_posj", f"/{clean}/aux_control/get_current_posx", ] - if step.key == "run_dispenser_recipe_sequence": - return [ - "/jarvis/rg2/set_width", - f"/{clean}/motion/move_joint", - f"/{clean}/motion/move_line", - f"/{clean}/motion/move_wait", - f"/{clean}/motion/ikin", - f"/{clean}/motion/check_motion", - f"/{clean}/system/get_robot_state", - f"/{clean}/tcp/get_current_tcp", - f"/{clean}/tcp/set_current_tcp", - f"/{clean}/aux_control/get_current_posx", - ] if step.key == "place_cup_holder": return [ "/jarvis/rg2/set_width", @@ -1003,13 +982,12 @@ def required_service_wait_timeout(step: Step) -> float: step.key.startswith("move_to_dispenser_") or step.key.startswith("press_dispenser_") or step.key.startswith("pick_from_dispenser_") - or step.key == "run_dispenser_recipe_sequence" or step.key == "place_cup_holder" ): return 35.0 if step.key in {"home_robot", "lift_robot", "side_grip", "shake_closed_cup"}: return 30.0 - if step.key in {"gripper_open", "gripper_soft_grasp"}: + if step.key == "gripper_soft_grasp": return 12.0 return 8.0 @@ -1370,7 +1348,6 @@ def requires_doosan_motion(step: Step) -> bool: or step.key.startswith("move_to_dispenser_") or step.key.startswith("press_dispenser_") or step.key.startswith("pick_from_dispenser_") - or step.key == "run_dispenser_recipe_sequence" or step.key == "place_cup_holder" ) @@ -1525,8 +1502,7 @@ def target_xyz_for_step(step_key: str) -> list[float] | None: def requires_collision_scene_step(key: str) -> bool: return ( - key == "run_dispenser_recipe_sequence" - or key == "shake_closed_cup" + key == "shake_closed_cup" or key.startswith("move_to_dispenser_") or key.startswith("pick_from_dispenser_") ) @@ -1548,8 +1524,6 @@ def with_collision_scene_prereq(selected: list[str]) -> list[str]: return ordered def run_timeout_for_step(step: Step) -> float: - if step.key == "run_dispenser_recipe_sequence": - return 900.0 if step.key == "side_grip": return 900.0 if step.key == "place_cup_holder": @@ -1668,9 +1642,12 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe ) if step.key == "connect_gripper": rg2_ip = str(payload.get("rg2_ip") or os.environ.get("RG2_IP") or "192.168.1.1") + gripper_pkg_bash = ROOT / "install" / "azas_gripper" / "share" / "azas_gripper" / "package.bash" return ( f"cd {ROOT} && {ROS_SETUP} && " - f"ros2 launch azas_gripper rg2_trigger.launch.py ip:={shlex.quote(rg2_ip)} " + f"source {shlex.quote(str(gripper_pkg_bash))} && " + f"ros2 launch {shlex.quote(str(ROOT / 'install' / 'azas_gripper' / 'share' / 'azas_gripper' / 'launch' / 'rg2_trigger.launch.py'))} " + f"ip:={shlex.quote(rg2_ip)} " "port:=502 connect:=true open_width:=1100 close_width:=0 force:=300 settle_seconds:=0.6" ) if step.key == "start_collision_scene": @@ -1762,11 +1739,6 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "moveit_controller_name:=/dsr01/dsr_moveit_controller " "start_joint_state_relay:=true" ) - if step.key == "gripper_open": - return ( - f"cd {ROOT} && {ROS_SETUP} && " - "tools/run/rg2_full_open_verify.sh" - ) if step.key == "gripper_soft_grasp": return ( f"cd {ROOT} && {ROS_SETUP} && " @@ -1880,24 +1852,6 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f" && {tumbler_scene_once('remove_world', object_id=f'tumbler_at_dispenser_{dispenser_id}', dispenser_id=dispenser_id)}" f" && {tumbler_scene_once('attach', object_id='carried_tumbler', dispenser_id=dispenser_id)}" ) - if step.key == "run_dispenser_recipe_sequence": - recipe_ids = str( - payload.get("recipe_dispenser_ids") - or os.environ.get("RECIPE_DISPENSER_IDS") - or "1,2,3,4" - ).strip() - tcp_name = str( - payload.get("dispenser_tcp_name") - or os.environ.get("DISPENSER_TCP_NAME") - or DEFAULT_DISPENSER_TCP_NAME - ).strip() - return ( - f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/run_measured_dispenser_recipe_sequence.py " - f"--service-prefix {service_prefix} " - f"--dispenser-ids {shlex.quote(recipe_ids)} " - f"--dispenser-tcp-name {shlex.quote(tcp_name)} " - "--execute --confirm ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE" - ) if step.key == "place_cup_holder": place_final_z_offset_m = str( payload.get("cup_holder_place_final_z_offset_m") From e585e5cc790f10161b917ee014ffe91e0057ad12 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 17:07:48 +0900 Subject: [PATCH 07/88] Keep side-grip panel cleanup from tearing down prerequisites The side-grip cleanup path was terminating the measured dispenser collision scene, and panel logs hid server-inserted prerequisites. Keep collision-scene nodes out of side-grip cleanup and report the actual server execution order. Constraint: Real-motion panel steps must fail closed, but cleanup must not remove required collision-scene prerequisites for the same run. Rejected: Disabling side-grip collision handling | the fix is to preserve prerequisite scenes, not to run without them. Confidence: high Scope-risk: narrow Directive: Keep cleanup patterns scoped to the step they clean; do not include shared prerequisite nodes in one-shot step cleanup lists. Tested: python3 -m py_compile tools/run/robot_pipeline_control_server.py; imported panel module and verified side_grip command keeps start_joint_state_relay:=true when installed; grep confirmed removed panel card keys remain absent. Not-tested: Live side_grip retry on hardware. --- docs/robot_pipeline_control.html | 4 ++-- tools/run/robot_pipeline_control_server.py | 15 ++++++++++++--- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index 6bbe380..1be9061 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -1220,7 +1220,7 @@

파이프라인 단계

for (const returned of results) { setStepStatus(returned.key || key, returned.status, returned.key === key ? itemId : ""); } - log.textContent = JSON.stringify({execution_order: body.selected, results}, null, 2); + log.textContent = JSON.stringify({execution_order: data.execution_order || body.selected, results}, null, 2); } catch (err) { setStepStatus(key, "failed", itemId); log.textContent = String(err); @@ -1474,7 +1474,7 @@

파이프라인 단계

} if (shouldStop) break; } - log.textContent = JSON.stringify({execution_order: selected.map((item) => item.key), results}, null, 2); + log.textContent = JSON.stringify({execution_order: results.map((item) => item.key), results}, null, 2); } catch (err) { log.textContent = String(err); } finally { diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 9464a4a..751f209 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -372,7 +372,6 @@ class Step: "joint_state_relay_legacy", "dsr_practice/joint_state_relay", "joint_state_relay --ros-args", - "measured_dispenser_collision_scene_node", ) RUN_STEP_STACK_PATTERNS = ( @@ -404,6 +403,16 @@ def command_line(proc: Any) -> str: return " ".join(str(part) for part in cmdline) +def installed_executable(package_name: str, executable_name: str) -> bool: + """Best-effort check for an installed ROS package console script.""" + + candidates = [ + ROOT / "install" / package_name / "lib" / package_name / executable_name, + Path("/home/ssu/ros2_ws/install") / package_name / "lib" / package_name / executable_name, + ] + return any(path.exists() and os.access(path, os.X_OK) for path in candidates) + + def tail_file(path: Path | None, *, max_chars: int = 8000) -> str: if path is None or not path.exists(): return "" @@ -1737,7 +1746,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "dispenser_collision_enabled:=true " f"dispenser_collision_config_path:={shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'config' / 'measured_dispenser_collision.yaml'))} " "moveit_controller_name:=/dsr01/dsr_moveit_controller " - "start_joint_state_relay:=true" + f"start_joint_state_relay:={'true' if installed_executable('dsr_practice', 'joint_state_relay') else 'false'}" ) if step.key == "gripper_soft_grasp": return ( @@ -2404,7 +2413,7 @@ def do_POST(self) -> None: for key in selected if key in steps_by_key ] - self.send_json({"results": results}) + self.send_json({"execution_order": selected, "results": results}) return if path == "/api/dispenser_color_map": new_map = payload.get("map") From 4fb68a34fc2409715da8457310d6dc8ccea353fd Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 17:09:33 +0900 Subject: [PATCH 08/88] Run measured dispenser moves in link6 TCP frame Measured front-hold poses are taught as link_6 targets. The panel was compensating the active GripperDA TCP into a current-TCP MoveLine target, which pushed dispenser_1 to x=0.838 and tripped the direct-move bounds. Switch panel dispenser moves to select the zero-offset link_6 TCP before moving and disable current-TCP compensation. Constraint: Do not alter measured dispenser coordinates; preserve fail-closed bounds and use the script's existing TCP selection path. Rejected: Raising direct MoveLine x_max to accept x=0.838 | that would widen the safety envelope instead of using the measured link_6 frame correctly. Confidence: high Scope-risk: narrow Directive: Keep front_hold_poses interpreted as link_6 poses unless they are re-taught with explicit measured_target_frame changes. Tested: python3 -m py_compile tools/run/robot_pipeline_control_server.py; imported panel module and verified collision-scene dedupe plus move_to_dispenser_1 command contains --set-current-tcp-before-move and --no-compensate-current-tcp. Not-tested: Live dispenser move retry on hardware. --- tools/run/robot_pipeline_control_server.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 751f209..7e9e138 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -1518,7 +1518,7 @@ def requires_collision_scene_step(key: str) -> bool: def with_collision_scene_prereq(selected: list[str]) -> list[str]: - ordered = list(selected) + ordered = list(dict.fromkeys(selected)) if "side_grip" in ordered: # PR #20 node also moves to camera-home internally, but the supervised # panel must make the operator-visible sequence explicit and safe: @@ -1782,7 +1782,8 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"--service-prefix {service_prefix} --dispenser-id {shlex.quote(dispenser_id)} " "--timeout-sec 180 --verify-target --verify-timeout-sec 70 " "--ikin-timeout-sec 20 --ikin-retries 2 " - "--target-tolerance-mm 15 --compensate-current-tcp --verify-link6-target --no-moveit-planning-guard " + "--target-tolerance-mm 15 --set-current-tcp-before-move --no-compensate-current-tcp " + "--verify-link6-target --no-moveit-planning-guard " ) # Newly taught side-grip front-hold poses are the verified reachable poses. # Do not synthesize an above/retreat pose here: for the current side-grip From 0b51ae96e8abc321c9035cbfe1721be7f957cede Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 17:16:58 +0900 Subject: [PATCH 09/88] Route dispenser press through measured calibration Use dispenser ID press poses from calibration.yaml for the panel path so HTML execution no longer selects legacy color/taught posx targets. Treat the measured press pose as the final pressed target instead of deriving a lower Z from legacy press depth.\n\nConstraint: Robot coordinates must come only from measured calibration data, not LLM-generated or legacy color aliases.\nRejected: Keep target_dispenser red/green/yellow/blue mapping | It reproduces the observed legacy coordinate motion.\nConfidence: high\nScope-risk: narrow\nDirective: Do not reintroduce target_dispenser/use_taught_posx for panel press_dispenser_N paths.\nTested: python3 -m py_compile tools/run/robot_pipeline_control_server.py src/azas_dispenser/azas_dispenser/dispenser_press_node.py; measured press command generation for dispensers 1-4; colcon build --symlink-install --packages-select azas_dispenser\nNot-tested: Live robot press after restarting the running panel server --- .../azas_dispenser/dispenser_press_node.py | 9 +- tools/run/robot_pipeline_control_server.py | 91 +++++++++++++++---- 2 files changed, 81 insertions(+), 19 deletions(-) diff --git a/src/azas_dispenser/azas_dispenser/dispenser_press_node.py b/src/azas_dispenser/azas_dispenser/dispenser_press_node.py index ee002f3..ee2deed 100644 --- a/src/azas_dispenser/azas_dispenser/dispenser_press_node.py +++ b/src/azas_dispenser/azas_dispenser/dispenser_press_node.py @@ -849,9 +849,14 @@ def build_press_steps(self): return steps def run(self): - if self.press_depth_mm <= 0.0: - self.logger.error("press_depth must be greater than 0.0 m.") + if self.press_depth_mm < 0.0: + self.logger.error("press_depth must be greater than or equal to 0.0 m.") return False + if self.press_depth_mm == 0.0: + self.logger.warning( + "press_depth=0.0: measured press pose is treated as the final pressed target; " + "the press step will hold at that measured pose instead of deriving a lower legacy Z." + ) if not self.wait_for_services(): return False diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 7e9e138..d275f1a 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -21,6 +21,11 @@ except ImportError: # pragma: no cover - local panel can still run without tree cleanup. psutil = None +try: + import yaml +except ImportError: # pragma: no cover - panel can still report a fail-closed blocker. + yaml = None + ROOT = Path(__file__).resolve().parents[2] HTML_PATH = ROOT / "docs" / "robot_pipeline_control.html" @@ -45,6 +50,8 @@ DEFAULT_YOLO_MODEL_PATH = ROOT / "local_models" / "best.pt" PR20_YOLO_MODEL_PATH = DEFAULT_YOLO_MODEL_PATH DEFAULT_DISPENSER_TCP_NAME = "GripperDA_v1_jarvis" +DEFAULT_LINK6_TCP_NAME = "azas_link6_tcp" +CALIBRATION_CONFIG_PATH = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" FAST_MOVE_VELOCITY = "30" FAST_MOVE_ACCELERATION = "30" RVIZ_PREVIEW_ROS_DOMAIN_ID = "79" @@ -95,6 +102,41 @@ def _load_dispenser_press_targets() -> dict[str, str]: DISPENSER_PRESS_TARGETS: dict[str, str] = _load_dispenser_press_targets() +def _number_list(value: Any, *, length: int, label: str) -> list[float]: + if not isinstance(value, list) or len(value) < length: + raise ValueError(f"{label} must be a list with at least {length} numeric values") + try: + return [float(item) for item in value[:length]] + except (TypeError, ValueError) as exc: + raise ValueError(f"{label} contains a non-numeric value") from exc + + +def measured_dispenser_press_pose(dispenser_id: str) -> tuple[list[float], list[float]]: + """Return measured base_link press pose for dispenser_N from calibration.yaml.""" + if yaml is None: + raise RuntimeError("PyYAML is not available, cannot read calibration.yaml") + data = yaml.safe_load(CALIBRATION_CONFIG_PATH.read_text(encoding="utf-8")) or {} + outlets = data.get("dispenser_outlets") or {} + block = outlets.get(str(dispenser_id)) + if not isinstance(block, dict): + raise ValueError(f"dispenser_outlets.{dispenser_id} is missing in {CALIBRATION_CONFIG_PATH}") + xyz_m = _number_list( + block.get("press_pose_xyz_m"), + length=3, + label=f"dispenser_outlets.{dispenser_id}.press_pose_xyz_m", + ) + rpy_deg = _number_list( + block.get("press_pose_rpy_deg"), + length=3, + label=f"dispenser_outlets.{dispenser_id}.press_pose_rpy_deg", + ) + return xyz_m, rpy_deg + + +def fail_closed_shell(message: str) -> str: + return f"echo {shlex.quote('[BLOCKED] ' + message)} >&2; exit 2" + + @dataclass(frozen=True) class Step: key: str @@ -225,39 +267,39 @@ class Step: ), Step( "press_dispenser_1", - "디스펜서 1 누르기 / red", + "디스펜서 1 누르기 / measured", "run", - "ros2 run azas_dispenser dispenser_press_node --ros-args -p target_dispenser:=red", + "ros2 run azas_dispenser dispenser_press_node --ros-args -p use_taught_posx:=false", True, True, - "feature/dispenser 원본 taught posx red 경로 사용: 컵 놓기 후 뒤로 후퇴→HOME 이동→RG2 full-close→transit→press→retreat→HOME 복귀", + "calibration.yaml dispenser_outlets.1 press_pose 측정값 사용: 컵 놓기 후 후퇴→HOME→RG2 full-close→measured press pose→HOME", ), Step( "press_dispenser_2", - "디스펜서 2 누르기 / green", + "디스펜서 2 누르기 / measured", "run", - "ros2 run azas_dispenser dispenser_press_node --ros-args -p target_dispenser:=green", + "ros2 run azas_dispenser dispenser_press_node --ros-args -p use_taught_posx:=false", True, True, - "feature/dispenser 원본 taught posx green 경로 사용: 컵 놓기 후 뒤로 후퇴→HOME 이동→RG2 full-close→transit→press→retreat→HOME 복귀", + "calibration.yaml dispenser_outlets.2 press_pose 측정값 사용: 컵 놓기 후 후퇴→HOME→RG2 full-close→measured press pose→HOME", ), Step( "press_dispenser_3", - "디스펜서 3 누르기 / yellow", + "디스펜서 3 누르기 / measured", "run", - "ros2 run azas_dispenser dispenser_press_node --ros-args -p target_dispenser:=yellow", + "ros2 run azas_dispenser dispenser_press_node --ros-args -p use_taught_posx:=false", True, True, - "feature/dispenser 원본 taught posx yellow 경로 사용: 컵 놓기 후 뒤로 후퇴→HOME 이동→RG2 full-close→transit→press→retreat→HOME 복귀", + "calibration.yaml dispenser_outlets.3 press_pose 측정값 사용: 컵 놓기 후 후퇴→HOME→RG2 full-close→measured press pose→HOME", ), Step( "press_dispenser_4", - "디스펜서 4 누르기 / blue", + "디스펜서 4 누르기 / measured", "run", - "ros2 run azas_dispenser dispenser_press_node --ros-args -p target_dispenser:=blue", + "ros2 run azas_dispenser dispenser_press_node --ros-args -p use_taught_posx:=false", True, True, - "feature/dispenser 원본 taught posx blue 경로 사용: 컵 놓기 후 뒤로 후퇴→HOME 이동→RG2 full-close→transit→press→retreat→HOME 복귀", + "calibration.yaml dispenser_outlets.4 press_pose 측정값 사용: 컵 놓기 후 후퇴→HOME→RG2 full-close→measured press pose→HOME", ), Step( "pick_from_dispenser_1", @@ -1802,21 +1844,36 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe ) if step.key.startswith("press_dispenser_"): dispenser_id = step.key.rsplit("_", 1)[-1] - target = DISPENSER_PRESS_TARGETS.get(dispenser_id, "red") + try: + press_xyz_m, press_rpy_deg = measured_dispenser_press_pose(dispenser_id) + except Exception as exc: + return fail_closed_shell( + f"measured press pose for dispenser_{dispenser_id} is unavailable: {exc}" + ) tcp_name = str( payload.get("dispenser_tcp_name") or os.environ.get("DISPENSER_TCP_NAME") - or DEFAULT_DISPENSER_TCP_NAME + or DEFAULT_LINK6_TCP_NAME ).strip() return ( f"cd {ROOT} && {ROS_SETUP} && " + f"echo {shlex.quote('[Azas] measured press pose dispenser_' + dispenser_id + ': xyz_m=' + str(press_xyz_m) + ' rpy_deg=' + str(press_rpy_deg) + ' source=calibration.yaml dispenser_outlets.' + dispenser_id + '; legacy taught/color posx disabled')} && " "ros2 run azas_dispenser dispenser_press_node --ros-args " f"-p service_prefix:={shlex.quote(service_prefix)} " - "-p use_taught_posx:=true " + "-p use_taught_posx:=false " + "-p use_home_as_reference:=false " + "-p keep_home_orientation:=false " + f"-p dispenser_x:={press_xyz_m[0]:.6f} " + f"-p dispenser_y:={press_xyz_m[1]:.6f} " + "-p dispenser_y_offset:=0.0 " + f"-p dispenser_top_z:={press_xyz_m[2]:.6f} " + f"-p rx:={press_rpy_deg[0]:.6f} " + f"-p ry:={press_rpy_deg[1]:.6f} " + f"-p rz:={press_rpy_deg[2]:.6f} " + "-p press_depth:=0.0 " f"-p tcp_name:={shlex.quote(tcp_name)} " "-p require_tcp_for_taught_posx:=false " - "-p allow_tcp_set_failure:=true " - f"-p target_dispenser:={shlex.quote(target)} " + "-p allow_tcp_set_failure:=false " "-p move_home_first:=true " "-p pre_home_retreat_before_home:=true " "-p pre_home_retreat_dx_mm:=-180.0 " From ba5a00b3a8e5a43dc26bfeb6c65e8caa2ca280f8 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 17:21:16 +0900 Subject: [PATCH 10/88] Make azas-panel refresh stale panel servers Let the one-word launcher restart only the panel HTTP server when its Python source changed, and provide an explicit --restart escape hatch, so operators do not need to remember the long cd/source/pkill sequence.\n\nConstraint: Keep robot/hardware stack cleanup separate; this launcher restarts only robot_pipeline_control_server.py.\nRejected: Tell users to keep typing pkill/source/run commands | It is error-prone and was the direct usability complaint.\nConfidence: high\nScope-risk: narrow\nDirective: Preserve azas-panel as the canonical operator entrypoint.\nTested: bash -n tools/run/open_robot_pipeline_control_panel.sh; azas-panel --restart; curl -fsS http://127.0.0.1:8765/\nNot-tested: Non-ssu workstation with missing /home/ssu/ros2_ws setup --- .../run/open_robot_pipeline_control_panel.sh | 82 +++++++++++++++++++ 1 file changed, 82 insertions(+) diff --git a/tools/run/open_robot_pipeline_control_panel.sh b/tools/run/open_robot_pipeline_control_panel.sh index f05a359..81c509d 100755 --- a/tools/run/open_robot_pipeline_control_panel.sh +++ b/tools/run/open_robot_pipeline_control_panel.sh @@ -11,6 +11,34 @@ PID_FILE="/tmp/azas-panel-8765.pid" COMMAND_DIR="${AZAS_PANEL_COMMAND_DIR:-$HOME/.local/bin}" COMMAND_PATH="$COMMAND_DIR/azas-panel" PANEL_ROS_DOMAIN_ID="${AZAS_PANEL_ROS_DOMAIN_ID:-9}" +SERVER_SCRIPT="$ROOT/tools/run/robot_pipeline_control_server.py" +FORCE_RESTART=0 + +case "${1:-}" in + --restart|restart) + FORCE_RESTART=1 + ;; + -h|--help) + cat <&2 + echo "Usage: azas-panel [--restart]" >&2 + exit 2 + ;; +esac mkdir -p "$LOG_DIR" @@ -36,6 +64,50 @@ panel_ready() { curl -fsS --max-time 1 "$URL" >/dev/null 2>&1 } +server_pid() { + if [[ -f "$PID_FILE" ]]; then + local pid + pid="$(cat "$PID_FILE" 2>/dev/null || true)" + if [[ "$pid" =~ ^[0-9]+$ ]] && ps -p "$pid" -o args= 2>/dev/null | grep -q "robot_pipeline_control_server.py"; then + echo "$pid" + return 0 + fi + fi + pgrep -f "python3 .*tools/run/robot_pipeline_control_server.py|python3 tools/run/robot_pipeline_control_server.py" | head -n 1 +} + +server_needs_restart() { + local pid="$1" + [[ "$FORCE_RESTART" == "1" ]] && return 0 + [[ -z "$pid" ]] && return 1 + [[ ! -f "$SERVER_SCRIPT" ]] && return 1 + local etimes now started script_mtime + etimes="$(ps -p "$pid" -o etimes= 2>/dev/null | tr -d ' ' || true)" + [[ ! "$etimes" =~ ^[0-9]+$ ]] && return 1 + now="$(date +%s)" + started=$((now - etimes)) + script_mtime="$(stat -c %Y "$SERVER_SCRIPT" 2>/dev/null || echo 0)" + [[ "$script_mtime" -gt "$started" ]] +} + +stop_panel_server() { + local pid="${1:-}" + if [[ -n "$pid" ]]; then + echo "[Azas] 기존 패널 서버 종료: pid=$pid" + kill "$pid" 2>/dev/null || true + for _ in $(seq 1 20); do + if ! ps -p "$pid" >/dev/null 2>&1; then + break + fi + sleep 0.1 + done + if ps -p "$pid" >/dev/null 2>&1; then + kill -TERM "$pid" 2>/dev/null || true + fi + fi + rm -f "$PID_FILE" +} + ensure_workspace_built() { if [[ ! -f "/opt/ros/humble/setup.bash" ]]; then cat >&2 <<'MSG' @@ -93,6 +165,16 @@ open_browser() { install_command_symlink ensure_workspace_built +PID="$(server_pid || true)" +if [[ -n "$PID" ]] && server_needs_restart "$PID"; then + if [[ "$FORCE_RESTART" == "1" ]]; then + echo "[Azas] 요청에 따라 패널 서버를 재시작합니다." + else + echo "[Azas] 패널 서버 코드 변경 감지: 새 코드로 자동 재시작합니다." + fi + stop_panel_server "$PID" +fi + if ! panel_ready; then echo "[Azas] 패널 서버 시작 중..." start_panel_server From b93840aea76aa41ff706bf12b42bc01b7f155322 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 17:24:46 +0900 Subject: [PATCH 11/88] Avoid controller TCP dependency for front-hold moves Move measured dispenser front-hold through current-TCP compensation with a final link6 verification instead of requiring the Doosan controller to accept azas_link6_tcp. This keeps the measured link6 target while avoiding false blocks when the controller rejects TCP registration/selection.\n\nConstraint: Cup/dispenser poses remain measured front_hold_poses; no generated coordinates.\nRejected: Require operators to register azas_link6_tcp manually | It blocks the panel path and is avoidable with current-TCP compensation plus link6 verification.\nConfidence: high\nScope-risk: narrow\nDirective: Keep --verify-link6-target enabled when using current-TCP compensation.\nTested: python3 -m py_compile tools/run/move_to_measured_dispenser_front_hold.py tools/run/robot_pipeline_control_server.py; generated move_to_dispenser_1 command; azas-panel --restart; live /api/steps resolved_command check\nNot-tested: Live robot execution after compensation change --- .../move_to_measured_dispenser_front_hold.py | 18 ++++++++++++++++++ tools/run/robot_pipeline_control_server.py | 3 ++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/tools/run/move_to_measured_dispenser_front_hold.py b/tools/run/move_to_measured_dispenser_front_hold.py index cc8ebe8..522e791 100755 --- a/tools/run/move_to_measured_dispenser_front_hold.py +++ b/tools/run/move_to_measured_dispenser_front_hold.py @@ -260,6 +260,12 @@ def parse_args() -> argparse.Namespace: ) parser.add_argument("--tcp-wait-service-sec", type=float, default=5.0) parser.add_argument("--tcp-timeout-sec", type=float, default=8.0) + parser.add_argument("--direct-x-min", type=float, default=0.10) + parser.add_argument("--direct-x-max", type=float, default=0.70) + parser.add_argument("--direct-y-min", type=float, default=-0.45) + parser.add_argument("--direct-y-max", type=float, default=0.45) + parser.add_argument("--direct-z-min", type=float, default=0.05) + parser.add_argument("--direct-z-max", type=float, default=0.80) parser.add_argument("--execute", action="store_true") parser.add_argument( "--confirm", @@ -709,6 +715,18 @@ def main() -> int: f"{args.target_tolerance_mm:.6f}", "--verify-timeout-sec", f"{args.verify_timeout_sec:.6f}", + "--x-min", + f"{args.direct_x_min:.6f}", + "--x-max", + f"{args.direct_x_max:.6f}", + "--y-min", + f"{args.direct_y_min:.6f}", + "--y-max", + f"{args.direct_y_max:.6f}", + "--z-min", + f"{args.direct_z_min:.6f}", + "--z-max", + f"{args.direct_z_max:.6f}", ] if args.precheck_ikin: cmd.append("--precheck-ikin") diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index d275f1a..bdefcaf 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -1824,7 +1824,8 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"--service-prefix {service_prefix} --dispenser-id {shlex.quote(dispenser_id)} " "--timeout-sec 180 --verify-target --verify-timeout-sec 70 " "--ikin-timeout-sec 20 --ikin-retries 2 " - "--target-tolerance-mm 15 --set-current-tcp-before-move --no-compensate-current-tcp " + "--target-tolerance-mm 15 --no-set-current-tcp-before-move --compensate-current-tcp " + "--direct-x-max 0.95 " "--verify-link6-target --no-moveit-planning-guard " ) # Newly taught side-grip front-hold poses are the verified reachable poses. From cde2651ba09a6d6e5db737740d38c06ead1da8cb Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 5 Jun 2026 17:39:15 +0900 Subject: [PATCH 12/88] Align dispenser regrasp bounds with measured TCP compensation Use the same widened direct-move X guard for pick_from_dispenser_N that front-hold placement uses, and pass those bounds through the internal measured front-hold approach. This prevents the current-TCP compensation path from being accepted during placement but blocked during regrasp/lift.\n\nConstraint: Regrasp still reuses measured front_hold_poses only; no operator/LLM cup coordinates.\nRejected: Keep x_max=0.72 for regrasp | Compensated current TCP can be around x=0.84 while link6 verifies against the measured pose.\nConfidence: high\nScope-risk: narrow\nDirective: Keep pick_from_dispenser_N bounds consistent with move_to_dispenser_N compensation assumptions.\nTested: python3 -m py_compile tools/run/pick_from_measured_dispenser_front_hold.py tools/run/move_to_measured_dispenser_front_hold.py tools/run/robot_pipeline_control_server.py; generated command assertions; azas-panel --restart; live /api/steps assertions for move/press/pick\nNot-tested: Live robot execution of the full move-press-pick sequence --- .../run/pick_from_measured_dispenser_front_hold.py | 14 +++++++++++++- tools/run/robot_pipeline_control_server.py | 13 +++++++++++-- 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/tools/run/pick_from_measured_dispenser_front_hold.py b/tools/run/pick_from_measured_dispenser_front_hold.py index c68fcbc..f4cdbf8 100755 --- a/tools/run/pick_from_measured_dispenser_front_hold.py +++ b/tools/run/pick_from_measured_dispenser_front_hold.py @@ -240,6 +240,18 @@ def run_front_hold_move( f"{args.wait_service_sec:.6f}", "--verify-timeout-sec", f"{args.verify_timeout_sec:.6f}", + "--direct-x-min", + f"{args.x_min:.6f}", + "--direct-x-max", + f"{args.x_max:.6f}", + "--direct-y-min", + f"{args.y_min:.6f}", + "--direct-y-max", + f"{args.y_max:.6f}", + "--direct-z-min", + f"{args.z_min:.6f}", + "--direct-z-max", + f"{args.z_max:.6f}", "--target-tolerance-mm", f"{args.target_tolerance_mm:.6f}", "--target-offset-x-m", @@ -398,7 +410,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--joint1-clearance-velocity", type=float, default=20.0) parser.add_argument("--joint1-clearance-acceleration", type=float, default=25.0) parser.add_argument("--x-min", type=float, default=0.10) - parser.add_argument("--x-max", type=float, default=0.72) + parser.add_argument("--x-max", type=float, default=0.95) parser.add_argument("--y-min", type=float, default=-0.35) parser.add_argument("--y-max", type=float, default=0.15) parser.add_argument("--z-min", type=float, default=0.05) diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index bdefcaf..4604e19 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -1854,7 +1854,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe tcp_name = str( payload.get("dispenser_tcp_name") or os.environ.get("DISPENSER_TCP_NAME") - or DEFAULT_LINK6_TCP_NAME + or DEFAULT_DISPENSER_TCP_NAME ).strip() return ( f"cd {ROOT} && {ROS_SETUP} && " @@ -1915,7 +1915,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "--lift-m 0.100 --lift-velocity 18.0 --lift-acceleration 24.0 " "--timeout-sec 120 --wait-service-sec 8 --verify-timeout-sec 45 " "--target-tolerance-mm 15 --gripper-grasp-width-m 0.075 --gripper-force-n 25.0 " - "--x-min 0.10 " + "--x-min 0.10 --x-max 0.95 " "--execute --confirm ENABLE_PICK_FROM_MEASURED_DISPENSER_FRONT_HOLD" f" && {tumbler_scene_once('remove_world', object_id=f'tumbler_at_dispenser_{dispenser_id}', dispenser_id=dispenser_id)}" f" && {tumbler_scene_once('attach', object_id='carried_tumbler', dispenser_id=dispenser_id)}" @@ -2349,6 +2349,15 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: "returncode": 1, "output": output, } + if step.key == "color_scan": + try: + color_map = json.loads(DISPENSER_COLOR_MAP_PATH.read_text(encoding="utf-8")) + lines = ["--- 색상 스캔 결과 ---"] + for did in sorted(color_map.keys(), key=lambda x: int(x) if x.isdigit() else x): + lines.append(f" 디스펜서 {did}: {color_map[did]}") + output = f"{output}\n" + "\n".join(lines) + "\n" + except Exception as exc: + output = f"{output}\n[color_scan] 결과 파일 읽기 실패: {exc}\n" target_xyz = target_xyz_for_step(step.key) if target_xyz is not None: reached, verify_output = wait_for_xyz_target(env["SERVICE_PREFIX"], target_xyz) From 9834c1015e61707a2f0c56b07bfdd49df49ab9dc Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sun, 7 Jun 2026 19:30:35 +0900 Subject: [PATCH 13/88] Preserve measured dispenser operation baseline Constraint: commit requested before further geometry changes. Confidence: medium Scope-risk: broad Directive: Keep cup coordinates sourced from perception topics or measured calibration only. Tested: not run; checkpoint commit of existing workspace state. Not-tested: runtime ROS motion and hardware validation. --- COMMANDS.md | 8 +- docs/robot_pipeline_control.html | 169 ++++- src/azas_bringup/config/calibration.yaml | 72 +- .../config/measured_dispenser_collision.yaml | 221 ++++-- .../dispenser_press_cycle_moveit.launch.py | 79 +++ .../launch/doosan_moveit_rviz_only.launch.py | 10 + .../launch/rg2_link6_tcp.launch.py | 42 ++ .../launch/tumbler_floor_place.launch.py | 32 +- .../rviz/azas_real_mirror_dispenser.rviz | 125 ++++ .../rviz/link6_gripper_tcp_debug.rviz | 126 ++++ src/azas_bringup/rviz/m0609_robot_only.rviz | 64 ++ src/azas_bringup/setup.py | 1 + .../urdf/rg2_link6_tcp.urdf.xacro | 125 ++++ .../azas_dispenser/dispenser_press_node.py | 191 ++++- .../dispenser_press_cycle_moveit_node.py | 377 ++++++++++ .../dispenser_sequence_preview_node.py | 77 +- ...oveit_grasped_tumbler_to_dispenser_node.py | 24 +- .../m0609_shake_joint_state_node.py | 58 +- ...measured_dispenser_collision_scene_node.py | 8 + .../azas_motion/tumbler_floor_place_node.py | 47 +- .../tumbler_shake_sequence_node.py | 8 +- src/azas_motion/setup.py | 1 + .../config/measured_dispenser_collision.yaml | 221 ++++-- src/azas_voice/azas_voice/command_parser.py | 16 +- .../azas_voice/llm_recipe_mapper_node.py | 32 +- src/azas_voice/azas_voice/recipe_catalog.py | 7 + .../check_measured_dispenser_geometry.py | 16 +- .../check_panel_service_discovery_race.py | 97 ++- tools/perception/dispenser_color_scan.py | 16 + tools/run/dispenser_color_scan_ros.sh | 19 +- tools/run/listen_stt_recipe.py | 13 +- .../run/open_robot_pipeline_control_panel.sh | 25 +- ...pick_from_measured_dispenser_front_hold.py | 16 +- tools/run/robot_pipeline_control_server.py | 231 +++++- tools/run/run_color_recipe_sequence.py | 71 +- .../run_course_dispenser_press_cycle_rviz.sh | 186 +++++ tools/run/run_course_moveit_mp_basic_rviz.sh | 82 +++ ...n_dispenser_then_shake_real_mirror_rviz.sh | 233 ++++++ tools/run/run_doosan_real_m0609.sh | 70 ++ .../run_measured_dispenser_recipe_sequence.py | 669 ++++++++++++++++-- tools/run/run_smooth_orange_robot_rviz.sh | 68 ++ 41 files changed, 3517 insertions(+), 436 deletions(-) create mode 100644 src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py create mode 100644 src/azas_bringup/launch/rg2_link6_tcp.launch.py create mode 100644 src/azas_bringup/rviz/azas_real_mirror_dispenser.rviz create mode 100644 src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz create mode 100644 src/azas_bringup/rviz/m0609_robot_only.rviz create mode 100644 src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro create mode 100644 src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py create mode 100755 tools/run/run_course_dispenser_press_cycle_rviz.sh create mode 100755 tools/run/run_course_moveit_mp_basic_rviz.sh create mode 100755 tools/run/run_dispenser_then_shake_real_mirror_rviz.sh create mode 100755 tools/run/run_doosan_real_m0609.sh create mode 100755 tools/run/run_smooth_orange_robot_rviz.sh diff --git a/COMMANDS.md b/COMMANDS.md index 6e8f490..fe6eca2 100644 --- a/COMMANDS.md +++ b/COMMANDS.md @@ -79,7 +79,7 @@ colcon test --packages-select azas_voice ```bash cd /home/ssu/Azas -# 패널 서버가 없으면 시작하고, 브라우저에서 http://127.0.0.1:8765/ 를 엽니다. +# 패널 서버를 새로 초기화하고, 브라우저에서 http://127.0.0.1:8765/ 를 엽니다. # 첫 실행 때 ~/.local/bin/azas-panel symlink도 자동으로 준비합니다. bash tools/run/open_robot_pipeline_control_panel.sh ``` @@ -90,6 +90,12 @@ bash tools/run/open_robot_pipeline_control_panel.sh azas-panel ``` +이미 떠 있는 서버를 그대로 재사용해서 열 때: + +```bash +azas-panel --reuse +``` + 패널 서버만 종료할 때: ```bash diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index 1be9061..38d2a22 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -170,6 +170,30 @@ letter-spacing: 0.02em; } .quick-start-btn:hover { background: #1d3faa !important; } + .direct-dispenser-field { + display: inline-flex; + align-items: center; + gap: 7px; + min-height: 32px; + padding: 0 9px; + border: 1px solid #99f6e4; + border-radius: 8px; + background: #f0fdfa; + color: #115e59; + font-size: 12px; + font-weight: 800; + } + .direct-dispenser-field input { + width: 180px; + height: 24px; + border: 1px solid #5eead4; + border-radius: 6px; + padding: 0 8px; + font-size: 13px; + font-weight: 700; + color: #0f172a; + background: #fff; + } .danger-light { color: var(--red) !important; background: #fff1f2 !important; border-color: #fecdd3 !important; } #cocktailCyclePanel { display: none; @@ -539,6 +563,43 @@ details.command summary::-webkit-details-marker { display: none; } details.command summary::before { content: "▸ "; } details.command[open] summary::before { content: "▾ "; } + .command-editor { + width: 100%; + min-height: 150px; + margin-top: 8px; + padding: 10px; + border: 1px solid var(--line); + border-radius: 10px; + background: #f8fafc; + color: #0f172a; + font-family: ui-monospace, SFMono-Regular, Menlo, Consolas, monospace; + font-size: 12px; + line-height: 1.45; + resize: vertical; + } + .command-tools { + display: flex; + flex-wrap: wrap; + gap: 8px; + margin-top: 8px; + } + .command-tools button { + min-height: 30px; + padding: 0 10px; + border-radius: 8px; + font-size: 12px; + font-weight: 850; + } + .command-save { + border: 1px solid #bfdbfe; + background: #dbeafe; + color: #1d4ed8; + } + .command-reset { + border: 1px solid #fed7aa; + background: #ffedd5; + color: #9a3412; + } .step code { display: block; margin-top: 7px; @@ -645,7 +706,7 @@

Azas Robot Pipeline Control

-
넓은 단계판 · 통합 디스펜서 루프 · 오른쪽 상시 로그
+
넓은 단계판 · 프레스 반복 시퀀스 · 오른쪽 상시 로그
단계 로드 전 @@ -668,7 +729,11 @@

Azas Robot Pipeline Control

- + + + @@ -685,16 +750,22 @@

Azas Robot Pipeline Control

🍹 칵테일 사이클 — 실행할 단계 선택
+
+ 확인된 레시피 JSON의 dispenser_amounts가 있으면 같은 디스펜서를 연속 그룹으로 묶고, + 각 그룹마다 컵을 디스펜서 앞에 놓기 → 해당 횟수만큼 프레스 → 다시 컵 잡고 다음 디스펜서로 이동을 한 단계에서 실행합니다. + 컵 이동/프레스/다시잡기는 별도 프레스 노드를 새로 띄우지 않고 같은 통합 루프 안에서 빠른 속도 설정으로 바로 실행합니다. + DIRECT DISPENSER INPUT을 비우면 색상 스캔 결과 파일이 필요하고, 값을 넣으면 색상 스캔 없이 해당 물리 번호와 횟수로 실행합니다. +
- + - +
@@ -722,9 +793,6 @@

Azas Robot Pipeline Control

- @@ -749,6 +817,7 @@

Azas Robot Pipeline Control

source /home/ssu/ros2_ws/install/setup.bash # 코드 변경 후에만 필요: colcon build --symlink-install --packages-select dsr_practice source /home/ssu/Azas/install/setup.bash +source /home/ssu/Azas/install/dsr_practice/share/dsr_practice/package.bash export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} ros2 launch /home/ssu/Azas/install/dsr_practice/share/dsr_practice/launch/yolo_cup_pick_node.launch.py \ @@ -861,7 +930,7 @@

파이프라인 단계

core: "연결/준비", grip: "그리퍼", move: "컵 이동/배치", - press: "디스펜서 프레스", + press: "통합 디스펜서 (컵놓기→프레스→다시잡기)", shake: "쉐이킹", blocked: "미구현/보류", etc: "기타" @@ -909,7 +978,7 @@

파이프라인 단계

if (key.startsWith("teach_front_hold_")) return "teach"; if (key === "start_collision_scene") return "core"; if (key.startsWith("move_to_dispenser") || key.includes("cup") || key.includes("holder") || key.includes("side_grip") || key.includes("home") || key.includes("lift")) return "move"; - if (key.startsWith("press_dispenser")) return "press"; + if (key === "run_color_recipe_sequence" || key.startsWith("press_dispenser")) return "press"; if (key.includes("shake")) return "shake"; if (!step.implemented || step.kind === "blocked") return "blocked"; return "etc"; @@ -1093,6 +1162,7 @@

파이프라인 단계

parts.push(`
${escapeHtml(groupLabels[group] || group)}${steps.length}개
`); for (const step of steps) { const cmd = step.resolved_command || step.command || "실행 명령 없음"; + const savedBadge = step.command_saved ? `저장 명령` : ""; const status = resultStatuses.get(step.key); const statusClass = status ? `status-${status}` : ""; const isQueued = queuedKeysSet().has(step.key); @@ -1109,8 +1179,9 @@

파이프라인 단계

${escapeHtml(step.note)}
- ${step.implemented ? "명령 후보" : "미구현"} + ${step.implemented ? "명령 후보" : "미구현"} ${step.real_motion ? `실제모션` : ""} + ${savedBadge} ${result}
@@ -1118,8 +1189,12 @@

파이프라인 단계

${step.key === "side_grip" ? `` : ""}
- 명령 보기 - ${escapeHtml(cmd)} + 명령 보기 / 편집 + +
+ + +
@@ -1203,7 +1278,7 @@

파이프라인 단계

focusLog(); try { const body = payload(); - body.selected = key === "side_grip" ? ["lift_robot", "start_camera", key] : [key]; + body.selected = [key]; const res = await fetch("/api/run", { method: "POST", headers: {"Content-Type": "application/json"}, @@ -1291,6 +1366,18 @@

파이프라인 단계

log.textContent = "단계 목록 로드 완료. 상단 검색/필터 또는 빠른 선택을 사용하세요."; } + async function saveCommandOverride(key, command) { + const res = await fetch("/api/command_override", { + method: "POST", + headers: {"Content-Type": "application/json"}, + body: JSON.stringify({key, command}) + }); + const data = await res.json(); + if (!res.ok) throw new Error(data.error || "명령 저장 실패"); + await loadSteps(); + return data; + } + document.addEventListener("change", (event) => { if (!event.target.classList?.contains("step-check")) return; if (event.target.checked) { @@ -1301,14 +1388,48 @@

파이프라인 단계

updateSelectedCount(); }); document.addEventListener("click", (event) => { - const runKey = event.target?.dataset?.runStep; + const targetElement = event.target instanceof Element ? event.target : null; + const commandElement = targetElement?.closest(".command-editor, .command-tools, details.command summary"); + if (commandElement) event.stopPropagation(); + const saveKey = targetElement?.dataset?.saveCommand; + if (saveKey) { + event.preventDefault(); + event.stopPropagation(); + const editor = document.querySelector(`.command-editor[data-command-editor="${CSS.escape(saveKey)}"]`); + saveCommandOverride(saveKey, editor?.value || "") + .then(() => { + log.textContent = `저장됨: ${saveKey} 편집 명령`; + focusLog(); + }) + .catch((err) => { + log.textContent = String(err); + focusLog(); + }); + return; + } + const resetKey = targetElement?.dataset?.resetCommand; + if (resetKey) { + event.preventDefault(); + event.stopPropagation(); + saveCommandOverride(resetKey, "") + .then(() => { + log.textContent = `복원됨: ${resetKey} 기본 명령`; + focusLog(); + }) + .catch((err) => { + log.textContent = String(err); + focusLog(); + }); + return; + } + const runKey = targetElement?.dataset?.runStep; if (runKey) { event.preventDefault(); event.stopPropagation(); runSingleStepNow(runKey); return; } - const addKey = event.target?.dataset?.addStep; + const addKey = targetElement?.dataset?.addStep; if (addKey) { event.preventDefault(); event.stopPropagation(); @@ -1316,7 +1437,7 @@

파이프라인 단계

updateSelectedCount(); return; } - const removeId = event.target?.dataset?.removeFlow; + const removeId = targetElement?.dataset?.removeFlow; if (removeId) { removeQueueItem(removeId); updateSelectedCount(); @@ -1404,6 +1525,22 @@

파이프라인 단계

panel.style.display = panel.style.display === "none" ? "block" : "none"; }); + document.getElementById("integratedDispenserBtn").addEventListener("click", () => { + const keys = ["connect_gripper", "run_color_recipe_sequence"]; + const directOrder = document.getElementById("recipeDispenserIds").value.trim(); + selectedQueue = []; + addQueueItems(keys); + itemStatuses = new Map(); + updateSelectedCount(); + renderSteps(); + flowEl.closest(".flow-panel")?.scrollIntoView({behavior: "smooth", block: "start"}); + log.textContent = directOrder + ? `빠른 통합 디스펜서: 직접 입력 ${directOrder} 기준으로 컵놓기→프레스→다시잡기를 바로 실행합니다.` + : "통합 디스펜서: latest_recipe.json 기준입니다. 색상 스캔 결과가 없으면 먼저 색상 스캔을 실행하거나 DIRECT DISPENSER INPUT에 1x1,2x2,3x1처럼 물리 번호와 횟수를 입력하세요."; + focusLog(); + if (directOrder) document.getElementById("run").click(); + }); + document.getElementById("cocktailCycleApplyBtn").addEventListener("click", () => { const cycleKeys = [...document.querySelectorAll(".cycle-step:checked")].map(cb => cb.value); if (!cycleKeys.length) { diff --git a/src/azas_bringup/config/calibration.yaml b/src/azas_bringup/config/calibration.yaml index fea0a72..511209c 100644 --- a/src/azas_bringup/config/calibration.yaml +++ b/src/azas_bringup/config/calibration.yaml @@ -22,8 +22,8 @@ hand_eye: color_scan_pose: source: operator_measured ee_link: link_6 - joints_deg: [0.0, 10.0, 20.0, 0.0, 90.0, 0.0] - joints_rad: [0.0, 0.1745, 0.3491, 0.0, 1.5708, 0.0] + joints_deg: [0.0, 10.0, 32.0, 0.0, 100.0, 90.0] + joints_rad: [0.0, 0.1745, 0.5585, 0.0, 1.7453, 1.5708] joint_order: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] cartesian_xyz_m: null cartesian_frame: base_link @@ -58,56 +58,60 @@ table: # measured teaching/calibration data in base_link; never from STT, LLM, or VLA. dispenser_outlets: "1": - outlet_pose_xyz_m: [0.609, 0.070, 0.087] # measured cup placement pose - outlet_pose_quaternion_xyzw: [0.489, 0.517, 0.517, 0.475] - outlet_pose_rpy_rad: [1.585, -0.015, 1.640] - outlet_pose_rpy_deg: [90.798, -0.845, 93.984] - press_pose_xyz_m: [0.712, 0.071, 0.530] - press_pose_quaternion_xyzw: [0.644, 0.630, 0.339, 0.271] - press_pose_rpy_rad: [2.248, -0.095, 1.594] - press_pose_rpy_deg: [128.791, -5.434, 91.357] + outlet_pose_xyz_m: [0.555, -0.100, 0.093] # measured cup placement pose + outlet_pose_quaternion_xyzw: [0.019717, 0.648068, 0.033158, 0.760605] + outlet_pose_rpy_rad: [0.429694, 1.394728, 0.448776] + outlet_pose_rpy_deg: [24.620, 79.912, 25.713] + press_pose_xyz_m: [0.705, 0.084, 0.520] + press_pose_quaternion_xyzw: [-0.060, 0.878, -0.024, 0.474] + press_pose_rpy_rad: [-2.962871, 0.977873, -2.910616] + press_pose_rpy_deg: [-169.760, 56.028, -166.766] press_joint_state: name: [joint_1, joint_2, joint_4, joint_5, joint_3, joint_6] position_rad: [0.08982130140066147, 0.9058733582496643, 0.008466404862701893, 1.3044253587722778, 0.033748552203178406, 1.6839158535003662] + press_contact_joints_deg: [15.12, 40.50, 32.87, -33.75, 51.99, 28.07] clearance_m: null "2": - outlet_pose_xyz_m: [0.617, 0.028, 0.082] - outlet_pose_quaternion_xyzw: [0.504, 0.504, 0.500, 0.491] - outlet_pose_rpy_rad: [1.587, -0.009, 1.580] - outlet_pose_rpy_deg: [90.946, -0.512, 90.501] - press_pose_xyz_m: [0.718, 0.015, 0.525] - press_pose_quaternion_xyzw: [0.649, 0.626, 0.338, 0.269] - press_pose_rpy_rad: [2.251, -0.103, 1.583] - press_pose_rpy_deg: [128.995, -5.878, 90.707] + outlet_pose_xyz_m: [0.549, -0.150, 0.097] + outlet_pose_quaternion_xyzw: [-0.021772, 0.650220, 0.060986, 0.756981] + outlet_pose_rpy_rad: [0.293265, 1.409774, 0.410750] + outlet_pose_rpy_deg: [16.803, 80.774, 23.534] + press_pose_xyz_m: [0.706, 0.043, 0.510] + press_pose_quaternion_xyzw: [-0.043, 0.878, -0.058, 0.473] + press_pose_rpy_rad: [-2.886216, 0.971433, -2.908521] + press_pose_rpy_deg: [-165.368, 55.659, -166.646] press_joint_state: name: [joint_1, joint_2, joint_4, joint_5, joint_3, joint_6] position_rad: [-0.000876683508977294, 0.9131112098693848, 0.0780220702290535, 1.3043646812438965, 0.032502077519893646, 1.621778964996338] + press_contact_joints_deg: [6.36, 39.76, 30.07, -14.08, 55.67, 27.99] clearance_m: null "3": - outlet_pose_xyz_m: [0.616, -0.026, 0.079] - outlet_pose_quaternion_xyzw: [0.504, 0.504, 0.498, 0.494] - outlet_pose_rpy_rad: [1.587, -0.005, 1.575] - outlet_pose_rpy_deg: [90.926, -0.278, 90.234] - press_pose_xyz_m: [0.716, -0.040, 0.525] - press_pose_quaternion_xyzw: [0.652, 0.623, 0.336, 0.272] - press_pose_rpy_rad: [2.252, -0.099, 1.574] - press_pose_rpy_deg: [129.006, -5.691, 90.174] + outlet_pose_xyz_m: [0.527, -0.204, 0.107] + outlet_pose_quaternion_xyzw: [-0.018792, 0.647687, 0.057548, 0.759498] + outlet_pose_rpy_rad: [0.279465, 1.403250, 0.387859] + outlet_pose_rpy_deg: [16.012, 80.400, 22.223] + press_pose_xyz_m: [0.705, -0.002, 0.513] + press_pose_quaternion_xyzw: [-0.015, 0.881, -0.075, 0.468] + press_pose_rpy_rad: [-2.884226, 0.964050, -2.972819] + press_pose_rpy_deg: [-165.254, 55.236, -170.330] press_joint_state: name: [joint_1, joint_2, joint_4, joint_5, joint_3, joint_6] position_rad: [-0.08701040595769882, 0.9135012626647949, 0.13757061958312988, 1.3107753992080688, 0.032613810151815414, 1.5555728673934937] + press_contact_joints_deg: [-0.29, 40.29, 28.38, -5.98, 55.25, 14.33] clearance_m: null "4": - outlet_pose_xyz_m: [0.607, -0.083, 0.075] - outlet_pose_quaternion_xyzw: [0.511, 0.498, 0.492, 0.499] - outlet_pose_rpy_rad: [1.589, -0.005, 1.551] - outlet_pose_rpy_deg: [91.042, -0.280, 88.871] - press_pose_xyz_m: [0.709, -0.084, 0.529] - press_pose_quaternion_xyzw: [0.650, 0.626, 0.316, 0.293] - press_pose_rpy_rad: [2.251, -0.044, 1.555] - press_pose_rpy_deg: [128.978, -2.505, 89.087] + outlet_pose_xyz_m: [0.517, -0.235, 0.109] + outlet_pose_quaternion_xyzw: [-0.022035, 0.648114, 0.059471, 0.758898] + outlet_pose_rpy_rad: [0.268008, 1.405233, 0.383736] + outlet_pose_rpy_deg: [15.356, 80.514, 21.986] + press_pose_xyz_m: [0.705, -0.050, 0.513] + press_pose_quaternion_xyzw: [0.020, 0.883, -0.078, 0.462] + press_pose_rpy_rad: [-2.931106, 0.959652, -3.076684] + press_pose_rpy_deg: [-167.940, 54.984, -176.281] press_joint_state: name: [joint_1, joint_2, joint_4, joint_5, joint_3, joint_6] position_rad: [-0.1493324339389801, 0.9293974041938782, 0.1349058598279953, 1.3465725183486938, -0.016838623210787773, 1.4902210235595703] + press_contact_joints_deg: [-7.04, 40.77, 28.37, 4.55, 54.02, 0.22] clearance_m: null cup_holder: diff --git a/src/azas_bringup/config/measured_dispenser_collision.yaml b/src/azas_bringup/config/measured_dispenser_collision.yaml index 850fd95..43a24ec 100644 --- a/src/azas_bringup/config/measured_dispenser_collision.yaml +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml @@ -1,66 +1,199 @@ -# Measured dispenser collision draft from real robot teaching. -# -# IMPORTANT: -# - These values were measured as base_link -> link_6 probe poses while the -# gripper/link_6 assembly was placed against the dispenser. They are not -# direct surface coordinates and must be reviewed with TCP/gripper offset -# before enabling hard real-motion collision enforcement. -# - The boxes below are conservative primitive estimates for MoveIt Planning -# Scene. Keep them disabled until verified in RViz against the real workcell. - metadata: frame_id: base_link measured_target_frame: link_6 source: operator_teaching_tf2_echo status: measured_draft_single_box_not_enabled - body_bottom_z_m: 0.000 + body_bottom_z_m: 0.0 body_bottom_reason: dispenser bottles start on same floor plane as robot base margin_m: - x: 0.020 - y: 0.020 - z: 0.020 - + x: 0.02 + y: 0.02 + z: 0.02 front_hold_poses: - # Cup hold/front limit poses. Cup pose itself still comes from vision. dispenser_1: - position_xyz_m: [0.609000, 0.070000, 0.087000] - quaternion_xyzw: [0.489000, 0.517000, 0.517000, 0.475000] - rpy_deg: [90.798, -0.845, 93.984] + position_xyz_m: + - 0.609 + - 0.07 + - 0.087 + quaternion_xyzw: + - 0.489 + - 0.517 + - 0.517 + - 0.475 + rpy_deg: + - 90.798 + - -0.845 + - 93.984 dispenser_2: - position_xyz_m: [0.617000, 0.028000, 0.082000] - quaternion_xyzw: [0.504000, 0.504000, 0.500000, 0.491000] - rpy_deg: [90.946, -0.512, 90.501] + position_xyz_m: + - 0.617 + - 0.028 + - 0.082 + quaternion_xyzw: + - 0.504 + - 0.504 + - 0.5 + - 0.491 + rpy_deg: + - 90.946 + - -0.512 + - 90.501 dispenser_3: - position_xyz_m: [0.616000, -0.026000, 0.079000] - quaternion_xyzw: [0.504000, 0.504000, 0.498000, 0.494000] - rpy_deg: [90.926, -0.278, 90.234] + position_xyz_m: + - 0.616 + - -0.026 + - 0.079 + quaternion_xyzw: + - 0.504 + - 0.504 + - 0.498 + - 0.494 + rpy_deg: + - 90.926 + - -0.278 + - 90.234 dispenser_4: - position_xyz_m: [0.607000, -0.083000, 0.075000] - quaternion_xyzw: [0.511000, 0.498000, 0.492000, 0.499000] - rpy_deg: [91.042, -0.280, 88.871] - + position_xyz_m: + - 0.607 + - -0.083 + - 0.075 + quaternion_xyzw: + - 0.511 + - 0.498 + - 0.492 + - 0.499 + rpy_deg: + - 91.042 + - -0.28 + - 88.871 raw_probe_poses: left_front_bottom_probe: - position_xyz_m: [0.767, 0.072, 0.219] - quaternion_xyzw: [0.642, 0.732, 0.215, 0.079] - rpy_deg: [155.003, -9.233, 99.561] + position_xyz_m: + - 0.767 + - 0.072 + - 0.219 + quaternion_xyzw: + - 0.642 + - 0.732 + - 0.215 + - 0.079 + rpy_deg: + - 155.003 + - -9.233 + - 99.561 right_back_top_probe: - position_xyz_m: [0.763, -0.103, 0.412] - quaternion_xyzw: [0.658, 0.654, 0.200, 0.313] - rpy_deg: [137.024, 8.372, 86.350] - + position_xyz_m: + - 0.763 + - -0.103 + - 0.412 + quaternion_xyzw: + - 0.658 + - 0.654 + - 0.2 + - 0.313 + rpy_deg: + - 137.024 + - 8.372 + - 86.35 estimated_collision_objects: dispenser_combined_body_box: type: box frame_id: base_link - # Single measured draft box from left-front-bottom and right-back-top - # link_6 probe poses. X/Y are expanded by metadata.margin_m on each side; - # Z uses body_bottom_z_m as the dispenser rests on the base/table plane. - center_xyz_m: [0.7650, -0.0155, 0.2160] - size_xyz_m: [0.0440, 0.2150, 0.4320] + center_xyz_m: + - 0.765 + - -0.0155 + - 0.216 + size_xyz_m: + - 0.044 + - 0.215 + - 0.432 bounds_xyz_m: - min: [0.7430, -0.1230, 0.0000] - max: [0.7870, 0.0920, 0.4320] - orientation_xyzw: [0.0, 0.0, 0.0, 1.0] + min: + - 0.743 + - -0.123 + - 0.0 + max: + - 0.787 + - 0.092 + - 0.432 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + dispenser_1_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - 0.084 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - -0.1368 + - 0.9906 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# note': x is aligned to the front face of dispenser_combined_body_box; z/y keep + measured lane/height. + dispenser_2_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - 0.043 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - -0.1368 + - 0.9906 + publish_to_planning_scene: true + enabled_for_real_motion: false + dispenser_3_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - -0.002 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - -0.1368 + - 0.9906 + publish_to_planning_scene: true + enabled_for_real_motion: false + dispenser_4_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - -0.05 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - -0.1368 + - 0.9906 publish_to_planning_scene: true enabled_for_real_motion: false diff --git a/src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py b/src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py new file mode 100644 index 0000000..a2b7928 --- /dev/null +++ b/src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py @@ -0,0 +1,79 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder(robot_name="m0609", package_name="dsr_moveit_config_m0609") + .robot_description() + .robot_description_semantic("config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("dsr_practice"), "config", "moveit_py.yaml"] + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("dispenser_id", default_value="1"), + DeclareLaunchArgument("press_count", default_value="2"), + DeclareLaunchArgument("start_delay_sec", default_value="4.0"), + DeclareLaunchArgument("dispenser_x", default_value="0.50"), + DeclareLaunchArgument("dispenser_y", default_value="0.00"), + DeclareLaunchArgument("cup_place_z", default_value=""), + DeclareLaunchArgument("cup_lift_z", default_value="0.54"), + DeclareLaunchArgument("cup_lift_m", default_value="0.08"), + DeclareLaunchArgument("cup_pre_grasp_backoff_m", default_value="0.08"), + DeclareLaunchArgument("cup_release_retract_m", default_value="0.05"), + DeclareLaunchArgument("press_ready_z", default_value="0.54"), + DeclareLaunchArgument("press_down_m", default_value="0.08"), + DeclareLaunchArgument("press_up_m", default_value="0.02"), + DeclareLaunchArgument("trajectory_time_scale", default_value="5.0"), + DeclareLaunchArgument("planning_time_sec", default_value="5.0"), + Node( + package="azas_motion", + executable="dispenser_press_cycle_moveit_node", + name="dispenser_press_cycle_moveit_node", + output="screen", + additional_env={ + "DISPENSER_ID": LaunchConfiguration("dispenser_id"), + "PRESS_COUNT": LaunchConfiguration("press_count"), + "DISPENSER_X": LaunchConfiguration("dispenser_x"), + "DISPENSER_Y": LaunchConfiguration("dispenser_y"), + "CUP_PLACE_Z": LaunchConfiguration("cup_place_z"), + "CUP_LIFT_Z": LaunchConfiguration("cup_lift_z"), + "CUP_LIFT_M": LaunchConfiguration("cup_lift_m"), + "CUP_PRE_GRASP_BACKOFF_M": LaunchConfiguration("cup_pre_grasp_backoff_m"), + "CUP_RELEASE_RETRACT_M": LaunchConfiguration("cup_release_retract_m"), + "PRESS_READY_Z": LaunchConfiguration("press_ready_z"), + "PRESS_DOWN_M": LaunchConfiguration("press_down_m"), + "PRESS_UP_M": LaunchConfiguration("press_up_m"), + "TRAJECTORY_TIME_SCALE": LaunchConfiguration("trajectory_time_scale"), + "PLANNING_TIME_SEC": LaunchConfiguration("planning_time_sec"), + }, + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + { + "press_count": ParameterValue(LaunchConfiguration("press_count"), value_type=int), + "start_delay_sec": ParameterValue(LaunchConfiguration("start_delay_sec"), value_type=float), + "dispenser_x": ParameterValue(LaunchConfiguration("dispenser_x"), value_type=float), + "dispenser_y": ParameterValue(LaunchConfiguration("dispenser_y"), value_type=float), + "cup_lift_z": ParameterValue(LaunchConfiguration("cup_lift_z"), value_type=float), + "press_down_m": ParameterValue(LaunchConfiguration("press_down_m"), value_type=float), + "press_up_m": ParameterValue(LaunchConfiguration("press_up_m"), value_type=float), + }, + ], + ), + ] + ) diff --git a/src/azas_bringup/launch/doosan_moveit_rviz_only.launch.py b/src/azas_bringup/launch/doosan_moveit_rviz_only.launch.py index dac992c..c03e9c7 100644 --- a/src/azas_bringup/launch/doosan_moveit_rviz_only.launch.py +++ b/src/azas_bringup/launch/doosan_moveit_rviz_only.launch.py @@ -59,11 +59,21 @@ def generate_launch_description(): ]) ]) ) + gripper_tcp_tree = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare("azas_bringup"), + "launch", + "rg2_link6_tcp.launch.py", + ]) + ]) + ) return LaunchDescription( [ DeclareLaunchArgument("model", default_value="m0609"), collision_scene, + gripper_tcp_tree, OpaqueFunction(function=rviz_node_function), ] ) diff --git a/src/azas_bringup/launch/rg2_link6_tcp.launch.py b/src/azas_bringup/launch/rg2_link6_tcp.launch.py new file mode 100644 index 0000000..d75177d --- /dev/null +++ b/src/azas_bringup/launch/rg2_link6_tcp.launch.py @@ -0,0 +1,42 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_description = ParameterValue( + Command( + [ + FindExecutable(name="xacro"), + " ", + PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "urdf", "rg2_link6_tcp.urdf.xacro"] + ), + " open_tcp_offset_m:=", + LaunchConfiguration("open_tcp_offset_m"), + " closed_tcp_offset_m:=", + LaunchConfiguration("closed_tcp_offset_m"), + ] + ), + value_type=str, + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("open_tcp_offset_m", default_value="0.15"), + DeclareLaunchArgument("closed_tcp_offset_m", default_value="0.25"), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="azas_rg2_link6_tcp_state_publisher", + output="screen", + remappings=[ + ("robot_description", "/azas/rg2_link6_tcp/robot_description"), + ], + parameters=[{"robot_description": robot_description}], + ), + ] + ) diff --git a/src/azas_bringup/launch/tumbler_floor_place.launch.py b/src/azas_bringup/launch/tumbler_floor_place.launch.py index 2b1c03a..fa5e137 100644 --- a/src/azas_bringup/launch/tumbler_floor_place.launch.py +++ b/src/azas_bringup/launch/tumbler_floor_place.launch.py @@ -36,6 +36,7 @@ def generate_launch_description(): place_approach_height = LaunchConfiguration("place_approach_height") place_mouth_under_outlet = LaunchConfiguration("place_mouth_under_outlet") outlet_mouth_clearance = LaunchConfiguration("outlet_mouth_clearance") + disable_gripper_commands = LaunchConfiguration("disable_gripper_commands") gripper_open_service = LaunchConfiguration("gripper_open_service") gripper_close_service = LaunchConfiguration("gripper_close_service") gripper_set_service = LaunchConfiguration("gripper_set_service") @@ -44,6 +45,7 @@ def generate_launch_description(): gripper_grasp_force_n = LaunchConfiguration("gripper_grasp_force_n") gripper_preopen_force_n = LaunchConfiguration("gripper_preopen_force_n") gripper_max_width_m = LaunchConfiguration("gripper_max_width_m") + motion_response_timeout_sec = LaunchConfiguration("motion_response_timeout_sec") gripper_min_width_m = LaunchConfiguration("gripper_min_width_m") params = { @@ -102,18 +104,18 @@ def generate_launch_description(): 0.1375, ], "dispenser_outlet_positions": [ - 0.609, - 0.070, - 0.087, - 0.617, - 0.028, - 0.082, - 0.616, - -0.026, - 0.079, - 0.607, - -0.083, - 0.075, + 0.555, + -0.100, + 0.093, + 0.549, + -0.150, + 0.097, + 0.527, + -0.204, + 0.107, + 0.517, + -0.235, + 0.109, ], "home_joints_deg": [0.0, 0.0, 90.0, 0.0, 90.0, 0.0], "move_home_first": False, @@ -125,12 +127,14 @@ def generate_launch_description(): "joint_acceleration": 20.0, "line_velocity": 30.0, "line_acceleration": 50.0, + "motion_response_timeout_sec": ParameterValue(motion_response_timeout_sec, value_type=float), "workspace_x_min": 0.0, "workspace_x_max": 0.80, "workspace_y_min": -0.35, "workspace_y_max": 0.35, "workspace_z_min": 0.0, "workspace_z_max": 0.80, + "disable_gripper_commands": ParameterValue(disable_gripper_commands, value_type=bool), # Optional std_srvs/Trigger services. Leave empty until RG2 wrapper is confirmed. "gripper_open_service": gripper_open_service, "gripper_close_service": gripper_close_service, @@ -169,7 +173,7 @@ def generate_launch_description(): DeclareLaunchArgument("tumbler_position_y", default_value="-0.22"), DeclareLaunchArgument("tumbler_position_z", default_value="0.05"), DeclareLaunchArgument("tumbler_bottom_diameter", default_value="0.065"), - DeclareLaunchArgument("tumbler_top_diameter", default_value="0.075"), + DeclareLaunchArgument("tumbler_top_diameter", default_value="0.109"), DeclareLaunchArgument("grasp_height", default_value="0.085"), DeclareLaunchArgument("side_grasp_approach_offset", default_value="0.10"), DeclareLaunchArgument("side_grasp_candidate_count", default_value="16"), @@ -180,6 +184,7 @@ def generate_launch_description(): DeclareLaunchArgument("place_approach_height", default_value="0.06"), DeclareLaunchArgument("place_mouth_under_outlet", default_value="false"), DeclareLaunchArgument("outlet_mouth_clearance", default_value="0.0"), + DeclareLaunchArgument("disable_gripper_commands", default_value="false"), DeclareLaunchArgument("gripper_open_service", default_value=""), DeclareLaunchArgument("gripper_close_service", default_value=""), DeclareLaunchArgument("gripper_set_service", default_value="/jarvis/rg2/set_width"), @@ -189,6 +194,7 @@ def generate_launch_description(): DeclareLaunchArgument("gripper_preopen_force_n", default_value="8.0"), DeclareLaunchArgument("gripper_max_width_m", default_value="0.110"), DeclareLaunchArgument("gripper_min_width_m", default_value="0.0"), + DeclareLaunchArgument("motion_response_timeout_sec", default_value="45.0"), Node( package="azas_motion", executable="tumbler_floor_place_node", diff --git a/src/azas_bringup/rviz/azas_real_mirror_dispenser.rviz b/src/azas_bringup/rviz/azas_real_mirror_dispenser.rviz new file mode 100644 index 0000000..dc6b8e7 --- /dev/null +++ b/src/azas_bringup/rviz/azas_real_mirror_dispenser.rviz @@ -0,0 +1,125 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Name: Grid + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Class: rviz_default_plugins/RobotModel + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azasvirt/robot_description + Enabled: true + Name: M0609 Robot - real joint_states only + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Path + Buffer Length: 1 + Color: 35; 170; 255 + Enabled: true + Line Style: Lines + Line Width: 0.025 + Name: Real node floor/dispenser transfer plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 35; 170; 255 + Pose Style: Axes + Radius: 0.025 + Shaft Length: 0.08 + Shaft Radius: 0.008 + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /jarvis/tumbler_floor_place/plan + Value: true + - Class: rviz_default_plugins/Path + Buffer Length: 1 + Color: 255; 180; 30 + Enabled: true + Line Style: Lines + Line Width: 0.02 + Name: Real node shake plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 180; 30 + Pose Style: Axes + Radius: 0.025 + Shaft Length: 0.08 + Shaft Radius: 0.008 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /jarvis/tumbler_shake_sequence/plan + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.4 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 24; 24; 28 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.7 + Focal Point: + X: 0.38 + Y: -0.08 + Z: 0.30 + Name: Current View + Pitch: 0.58 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 2.45 + Saved: ~ +Window Geometry: + Height: 900 + Hide Left Dock: false + Hide Right Dock: true + Width: 1280 + X: 40 + Y: 40 diff --git a/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz b/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz new file mode 100644 index 0000000..4e7d086 --- /dev/null +++ b/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz @@ -0,0 +1,126 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Name: Grid + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Class: rviz_default_plugins/RobotModel + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dsr01/robot_description + Enabled: true + Name: M0609 Robot + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/RobotModel + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/rg2_link6_tcp/robot_description + Enabled: true + Name: RG2 Link6 TCP + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.35 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/collision_scene/markers + Name: Collision Scene Markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/measured_dispenser_collision/markers + Name: Measured Dispenser Collision + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/dispenser_press/tcp_axes + Name: Press TCP Axes + Value: true + Enabled: true + Global Options: + Background Color: 24; 24; 28 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.5 + Focal Point: + X: 0.45 + Y: 0.0 + Z: 0.45 + Name: Current View + Pitch: 0.55 + Target Frame: link_6 + Value: Orbit (rviz) + Yaw: 2.4 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 900 + Hide Left Dock: false + Hide Right Dock: true + Width: 1280 + X: 90 + Y: 50 diff --git a/src/azas_bringup/rviz/m0609_robot_only.rviz b/src/azas_bringup/rviz/m0609_robot_only.rviz new file mode 100644 index 0000000..bda007b --- /dev/null +++ b/src/azas_bringup/rviz/m0609_robot_only.rviz @@ -0,0 +1,64 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Name: Grid + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Class: rviz_default_plugins/RobotModel + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Name: M0609 Robot only - controller joint_states + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 24; 24; 28 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.7 + Focal Point: + X: 0.38 + Y: -0.08 + Z: 0.30 + Name: Current View + Pitch: 0.58 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 2.45 + Saved: ~ +Window Geometry: + Height: 900 + Hide Left Dock: false + Hide Right Dock: true + Width: 1280 + X: 40 + Y: 40 diff --git a/src/azas_bringup/setup.py b/src/azas_bringup/setup.py index 3df40a3..2b73273 100644 --- a/src/azas_bringup/setup.py +++ b/src/azas_bringup/setup.py @@ -13,6 +13,7 @@ (f"share/{package_name}/launch", glob("launch/*.launch.py")), (f"share/{package_name}/config", glob("config/*.yaml")), (f"share/{package_name}/rviz", glob("rviz/*.rviz")), + (f"share/{package_name}/urdf", glob("urdf/*.urdf.xacro")), ], install_requires=["setuptools"], zip_safe=True, diff --git a/src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro b/src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro new file mode 100644 index 0000000..7a8ffa3 --- /dev/null +++ b/src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro @@ -0,0 +1,125 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/azas_dispenser/azas_dispenser/dispenser_press_node.py b/src/azas_dispenser/azas_dispenser/dispenser_press_node.py index ee2deed..483b7f1 100644 --- a/src/azas_dispenser/azas_dispenser/dispenser_press_node.py +++ b/src/azas_dispenser/azas_dispenser/dispenser_press_node.py @@ -154,6 +154,19 @@ def __init__(self): self.approach_pause_seconds = float( get_param(self.node, "approach_pause_seconds", 0.5) ) + self.press_count = max(int(get_param(self.node, "press_count", 1)), 1) + self.post_press_retreat_after_sequence = bool( + get_param(self.node, "post_press_retreat_after_sequence", True) + ) + self.post_press_retreat_dx_mm = float( + get_param(self.node, "post_press_retreat_dx_mm", -120.0) + ) + self.post_press_retreat_dy_mm = float( + get_param(self.node, "post_press_retreat_dy_mm", 0.0) + ) + self.post_press_retreat_wait_seconds = float( + get_param(self.node, "post_press_retreat_wait_seconds", 1.0) + ) self.rx = float(get_param(self.node, "rx", 180.0)) self.ry = float(get_param(self.node, "ry", 0.0)) @@ -293,23 +306,42 @@ def wait_for_services(self): return True def call_service(self, client, request, label): - future = client.call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is None: - self.logger.error(f"{label} 호출 실패: {future.exception()}") + result = self.service_result(client, request, label) + if result is None: return False - if not future.result().success: + if not result.success: self.logger.error(f"{label} 가 success=false를 반환했습니다") return False return True + def service_result(self, client, request, label): + future = client.call_async(request) + try: + rclpy.spin_until_future_complete( + self.node, + future, + timeout_sec=self.service_wait_timeout_sec, + ) + except Exception as exc: + self.logger.error(f"{label} 응답 대기 중 예외 발생: {exc}") + return None + if not future.done(): + self.logger.error( + f"{label} 호출이 {self.service_wait_timeout_sec:.1f}초 안에 응답하지 않았습니다. " + "Doosan 컨트롤러 연결/상태를 확인하세요." + ) + future.cancel() + return None + result = future.result() + if result is None: + self.logger.error(f"{label} 호출 실패: {future.exception()}") + return None + return result + def current_tcp_name(self): get_req = GetCurrentTcp.Request() - future = self.get_current_tcp.call_async(get_req) - rclpy.spin_until_future_complete(self.node, future) - result = future.result() + result = self.service_result(self.get_current_tcp, get_req, "tcp/get_current_tcp") if result is None: - self.logger.error(f"tcp/get_current_tcp 호출 실패: {future.exception()}") return None if not result.success: self.logger.error("tcp/get_current_tcp 가 success=false를 반환했습니다") @@ -324,11 +356,12 @@ def set_tcp_name(self, name, label): def current_joints_deg(self, label): req = GetCurrentPosj.Request() - future = self.get_current_posj.call_async(req) - rclpy.spin_until_future_complete(self.node, future) - result = future.result() + result = self.service_result( + self.get_current_posj, + req, + f"{label}: aux_control/get_current_posj", + ) if result is None: - self.logger.error(f"{label}: aux_control/get_current_posj 호출 실패: {future.exception()}") return None if not result.success or len(result.pos) < 6: self.logger.error( @@ -433,11 +466,12 @@ def close_gripper(self): req.width_m = self.gripper_close_width_m req.force_n = self.gripper_close_force_n - future = self.gripper_client.call_async(req) - rclpy.spin_until_future_complete(self.node, future) - result = future.result() + result = self.service_result( + self.gripper_client, + req, + "그리퍼 close", + ) if result is None: - self.logger.error(f"그리퍼 close 호출 실패: {future.exception()}") return False if not result.success: self.logger.error(f"그리퍼 close 실패: {result.message}") @@ -561,12 +595,13 @@ def read_current_posx(self): req = GetCurrentPosx.Request() req.ref = DR_BASE - future = self.get_current_posx.call_async(req) - rclpy.spin_until_future_complete(self.node, future) - result = future.result() + result = self.service_result( + self.get_current_posx, + req, + "aux_control/get_current_posx", + ) if result is None: - self.logger.error(f"get_current_posx 호출 실패: {future.exception()}") return None if not result.success: self.logger.error("get_current_posx가 success=false를 반환했습니다") @@ -806,33 +841,109 @@ def build_press_steps(self): f"top_z={top_z:.1f} mm, pressed_z={pressed_z:.1f} mm" ) else: + current_pose = self.read_current_posx() + if current_pose is None: + return None if self.keep_home_orientation: - current_pose = self.read_current_posx() - if current_pose is None: - return None home_x, home_y, home_z = current_pose[:3] self.rx = current_pose[3] self.ry = current_pose[4] self.rz = current_pose[5] - home_lift_step = ( - home_x, - home_y, - home_z + self.home_lift_height_mm, - "lift above HOME", - ) x_mm = self.dispenser_x_mm y_mm = self.dispenser_y_mm + self.dispenser_y_offset_mm top_z = self.dispenser_top_z_mm approach_z = top_z + self.approach_height_mm pressed_z = top_z - self.press_depth_mm + transit_z = max(current_pose[2], approach_z) + self.transit_height_mm + retreat_z = approach_z + if self.post_press_retreat_after_sequence: + retreat_x = x_mm + self.post_press_retreat_dx_mm + retreat_y = y_mm + self.post_press_retreat_dy_mm self.logger.info( - "고정된 디스펜서 위치를 HOME TCP 방향과 함께 사용합니다. " + "측정된 디스펜서 프레스 위치를 사용합니다. " f"target=({x_mm:.1f}, {y_mm:.1f}), " + f"press_z={pressed_z:.1f}, approach_z={approach_z:.1f}, " + f"transit_z={transit_z:.1f}, " f"rpy=({self.rx:.1f}, {self.ry:.1f}, {self.rz:.1f})" ) + steps = [ + ( + current_pose[0], + current_pose[1], + transit_z, + current_pose[3], + current_pose[4], + current_pose[5], + "lift from previous position", + ), + (x_mm, y_mm, transit_z, self.rx, self.ry, self.rz, "align above measured press pose"), + (x_mm, y_mm, approach_z, self.rx, self.ry, self.rz, "approach above dispenser"), + ] + for press_index in range(1, self.press_count + 1): + suffix = f" {press_index}/{self.press_count}" if self.press_count > 1 else "" + if self.press_depth_mm == 0.0: + steps.append( + ( + x_mm, + y_mm, + top_z, + self.rx, + self.ry, + self.rz, + f"press dispenser pump{suffix}", + ) + ) + else: + steps.extend( + [ + ( + x_mm, + y_mm, + top_z, + self.rx, + self.ry, + self.rz, + f"move to measured press pose{suffix}", + ), + ( + x_mm, + y_mm, + pressed_z, + self.rx, + self.ry, + self.rz, + f"press dispenser pump{suffix}", + ), + ] + ) + steps.append( + ( + x_mm, + y_mm, + approach_z, + self.rx, + self.ry, + self.rz, + f"retreat above dispenser{suffix}", + ) + ) + if self.post_press_retreat_after_sequence: + steps.append( + ( + retreat_x, + retreat_y, + retreat_z, + self.rx, + self.ry, + self.rz, + "retreat away from dispenser and wait", + ) + ) + return steps + steps = [ (x_mm, y_mm, approach_z, self.rx, self.ry, self.rz, "approach above dispenser"), (x_mm, y_mm, top_z, self.rx, self.ry, self.rz, "move to dispenser top"), @@ -840,12 +951,6 @@ def build_press_steps(self): (x_mm, y_mm, approach_z, self.rx, self.ry, self.rz, "retreat above dispenser"), ] - if home_lift_step is not None: - home_x, home_y, home_z, home_label = home_lift_step - return [ - (home_x, home_y, home_z, self.rx, self.ry, self.rz, home_label) - ] + steps - return steps def run(self): @@ -891,7 +996,7 @@ def run(self): rx, ry, rz = self.rx, self.ry, self.rz else: x_mm, y_mm, z_mm, rx, ry, rz, label = step - is_press_down = label == "press dispenser pump" + is_press_down = label.startswith("press dispenser pump") line_velocity = self.line_velocity if is_press_down else self.travel_line_velocity line_acceleration = ( self.line_acceleration if is_press_down else self.travel_line_acceleration @@ -920,9 +1025,17 @@ def run(self): f"접근 위치에서 {self.approach_pause_seconds:.2f}초간 대기합니다" ) time.sleep(self.approach_pause_seconds) - if label == "press dispenser pump" and self.hold_seconds > 0.0: + if label.startswith("press dispenser pump") and self.hold_seconds > 0.0: self.logger.info(f"누르는 동작을 {self.hold_seconds:.2f}초간 유지합니다") time.sleep(self.hold_seconds) + if ( + label == "retreat away from dispenser and wait" + and self.post_press_retreat_wait_seconds > 0.0 + ): + self.logger.info( + f"프레스 후 후퇴 위치에서 {self.post_press_retreat_wait_seconds:.2f}초간 대기합니다" + ) + time.sleep(self.post_press_retreat_wait_seconds) if self.return_home: self.logger.info( diff --git a/src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py b/src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py new file mode 100644 index 0000000..961580b --- /dev/null +++ b/src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py @@ -0,0 +1,377 @@ +#!/usr/bin/env python3 +"""Course-style MoveItPy dispenser press cycle using measured Azas press joints. + +Sequence goal: +- cup to dispenser front pose +- gripper open log +- lift while open +- gripper close log +- move to measured dispenser press contact joints (from calibration.yaml) +- repeat press by lifting from measured contact pose and returning down +- return to cup grasp pose + +No fake /joint_states and no display-path publisher are used. RViz observes the +Doosan controller-backed joint states, same path as real execution. +""" + +from __future__ import annotations + +import math +import time +from dataclasses import dataclass +from pathlib import Path + +import rclpy +import yaml +from geometry_msgs.msg import Pose, PoseStamped, Quaternion +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters +from rclpy.logging import get_logger + +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" +JOINT_NAMES = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] +ROOT = Path(__file__).resolve().parents[3] +DEFAULT_CALIBRATION = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" + + +@dataclass(frozen=True) +class Config: + dispenser_id: str + press_count: int + waypoint_hold_sec: float + planning_group: str + base_frame: str + ee_link: str + calibration_path: Path + cup_lift_m: float + press_up_m: float + cup_pre_grasp_backoff_m: float + cup_release_retract_m: float + cup_place_z: float | None + planning_time_sec: float + + +@dataclass(frozen=True) +class OutletCalibration: + outlet_xyz_m: list[float] + outlet_quat_xyzw: list[float] + press_xyz_m: list[float] + press_quat_xyzw: list[float] + press_contact_joints_deg: list[float] + + +def _env(name: str, default: str) -> str: + import os + + return os.environ.get(name, default) + + +def _env_float(name: str, default: float) -> float: + return float(_env(name, str(default))) + + +def _env_int(name: str, default: int) -> int: + return int(_env(name, str(default))) + + +def _env_optional_float(name: str) -> float | None: + raw = _env(name, "").strip() + if not raw: + return None + return float(raw) + + +def read_config() -> Config: + return Config( + dispenser_id=_env("DISPENSER_ID", "1"), + press_count=max(_env_int("PRESS_COUNT", 2), 1), + waypoint_hold_sec=max(_env_float("WAYPOINT_HOLD_SEC", 1.0), 0.0), + planning_group=_env("PLANNING_GROUP", GROUP_NAME), + base_frame=_env("BASE_FRAME", BASE_FRAME), + ee_link=_env("EE_LINK", EE_LINK), + calibration_path=Path(_env("CALIBRATION_PATH", str(DEFAULT_CALIBRATION))), + cup_lift_m=_env_float("CUP_LIFT_M", 0.08), + press_up_m=_env_float("PRESS_UP_M", 0.02), + cup_pre_grasp_backoff_m=max(_env_float("CUP_PRE_GRASP_BACKOFF_M", 0.08), 0.0), + cup_release_retract_m=max(_env_float("CUP_RELEASE_RETRACT_M", 0.05), 0.0), + cup_place_z=_env_optional_float("CUP_PLACE_Z"), + planning_time_sec=_env_float("PLANNING_TIME_SEC", 5.0), + ) + + +def _numeric_list(value, label: str, length: int) -> list[float]: + if not isinstance(value, list) or len(value) != length: + raise ValueError(f"{label} must be a list of {length} numbers") + return [float(v) for v in value] + + +def load_outlet(cfg: Config) -> OutletCalibration: + data = yaml.safe_load(cfg.calibration_path.read_text(encoding="utf-8")) or {} + outlets = data.get("dispenser_outlets") or {} + block = outlets.get(str(cfg.dispenser_id)) + if not isinstance(block, dict): + raise ValueError(f"dispenser_outlets.{cfg.dispenser_id} missing in {cfg.calibration_path}") + return OutletCalibration( + outlet_xyz_m=_numeric_list(block.get("outlet_pose_xyz_m"), f"outlet {cfg.dispenser_id} outlet_pose_xyz_m", 3), + outlet_quat_xyzw=_numeric_list(block.get("outlet_pose_quaternion_xyzw"), f"outlet {cfg.dispenser_id} outlet_pose_quaternion_xyzw", 4), + press_xyz_m=_numeric_list(block.get("press_pose_xyz_m"), f"outlet {cfg.dispenser_id} press_pose_xyz_m", 3), + press_quat_xyzw=_numeric_list(block.get("press_pose_quaternion_xyzw"), f"outlet {cfg.dispenser_id} press_pose_quaternion_xyzw", 4), + press_contact_joints_deg=_numeric_list(block.get("press_contact_joints_deg"), f"outlet {cfg.dispenser_id} press_contact_joints_deg", 6), + ) + + +def quat_xyzw(values: list[float]) -> Quaternion: + q = Quaternion() + q.x, q.y, q.z, q.w = [float(v) for v in values] + return q + + +def pose_goal(cfg: Config, x: float, y: float, z: float, quat_xyzw_values: list[float]) -> PoseStamped: + pose = PoseStamped() + pose.header.frame_id = cfg.base_frame + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = float(z) + pose.pose.orientation = quat_xyzw(quat_xyzw_values) + return pose + + +def pose_stamped_from_pose(cfg: Config, pose_value: Pose) -> PoseStamped: + pose = PoseStamped() + pose.header.frame_id = cfg.base_frame + pose.pose.position.x = float(pose_value.position.x) + pose.pose.position.y = float(pose_value.position.y) + pose.pose.position.z = float(pose_value.position.z) + pose.pose.orientation.x = float(pose_value.orientation.x) + pose.pose.orientation.y = float(pose_value.orientation.y) + pose.pose.orientation.z = float(pose_value.orientation.z) + pose.pose.orientation.w = float(pose_value.orientation.w) + return pose + + +def clone_pose_with_z(pose_value: Pose, z: float) -> Pose: + pose = Pose() + pose.position.x = float(pose_value.position.x) + pose.position.y = float(pose_value.position.y) + pose.position.z = float(z) + pose.orientation.x = float(pose_value.orientation.x) + pose.orientation.y = float(pose_value.orientation.y) + pose.orientation.z = float(pose_value.orientation.z) + pose.orientation.w = float(pose_value.orientation.w) + return pose + + +def plan_and_execute_pose_stamped(robot, arm, params, cfg: Config, label: str, goal: PoseStamped, logger) -> None: + arm.set_start_state_to_current_state() + arm.set_goal_state(pose_stamped_msg=goal, pose_link=cfg.ee_link) + logger.info( + f"Goal {label}: pose xyz=({goal.pose.position.x:.3f}, {goal.pose.position.y:.3f}, {goal.pose.position.z:.3f}) " + f"quat=[{goal.pose.orientation.x:.6f}, {goal.pose.orientation.y:.6f}, {goal.pose.orientation.z:.6f}, {goal.pose.orientation.w:.6f}]" + ) + logger.info(f"Planning trajectory: {label}") + result = arm.plan(parameters=params) + if not result: + raise RuntimeError(f"planning failed at {label}") + logger.info(f"Executing plan: {label}") + robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + logger.info(f"Execution finished: {label}") + time.sleep(cfg.waypoint_hold_sec) + + +def plan_and_execute_pose(robot, arm, params, cfg: Config, label: str, xyz_m: list[float], quat: list[float], logger) -> None: + goal = pose_goal(cfg, xyz_m[0], xyz_m[1], xyz_m[2], quat) + arm.set_start_state_to_current_state() + arm.set_goal_state(pose_stamped_msg=goal, pose_link=cfg.ee_link) + logger.info(f"Goal {label}: pose xyz=({xyz_m[0]:.3f}, {xyz_m[1]:.3f}, {xyz_m[2]:.3f}) quat={quat}") + logger.info(f"Planning trajectory: {label}") + result = arm.plan(parameters=params) + if not result: + raise RuntimeError(f"planning failed at {label}") + logger.info(f"Executing plan: {label}") + robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + logger.info(f"Execution finished: {label}") + time.sleep(cfg.waypoint_hold_sec) + + +def joint_state(model, cfg: Config, joints_deg: list[float], label: str, logger) -> RobotState: + target = {name: math.radians(float(deg)) for name, deg in zip(JOINT_NAMES, joints_deg)} + state = RobotState(model) + # This assignment style is used elsewhere in this repo and is more stable + # on this Humble MoveItPy build than set_joint_group_positions(). + state.joint_positions = target + state.update() + logger.info(f"Goal {label}: measured joints deg={joints_deg}") + return state + + +def fk_pose_from_joints(model, joints_deg: list[float], ee_link: str, logger) -> Pose: + state = RobotState(model) + state.joint_positions = {name: math.radians(float(deg)) for name, deg in zip(JOINT_NAMES, joints_deg)} + state.update() + pose = state.get_pose(ee_link) + logger.info( + f"FK from measured press joints: {ee_link} xyz=({pose.position.x:.3f}, {pose.position.y:.3f}, {pose.position.z:.3f}) " + f"quat=[{pose.orientation.x:.6f}, {pose.orientation.y:.6f}, {pose.orientation.z:.6f}, {pose.orientation.w:.6f}]" + ) + return pose + + +def plan_and_execute_joints(robot, arm, model, params, cfg: Config, label: str, joints_deg: list[float], logger) -> None: + state = joint_state(model, cfg, joints_deg, label, logger) + arm.set_start_state_to_current_state() + arm.set_goal_state(robot_state=state) + logger.info(f"Planning trajectory: {label}") + result = arm.plan(parameters=params) + if not result: + raise RuntimeError(f"planning failed at {label}") + logger.info(f"Executing plan: {label}") + robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + logger.info(f"Execution finished: {label}") + time.sleep(cfg.waypoint_hold_sec) + + +def gripper_event(logger, state: str, detail: str) -> None: + logger.info(f"GRIPPER_{state}: {detail}") + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + logger = get_logger("dispenser_press_cycle_moveit") + try: + cfg = read_config() + outlet = load_outlet(cfg) + logger.info( + f"Ready: measured-joint dispenser press cycle. dispenser={cfg.dispenser_id} " + f"press_contact_joints_deg={outlet.press_contact_joints_deg}" + ) + robot = MoveItPy(node_name="dispenser_press_cycle_moveit_py") + arm = robot.get_planning_component(cfg.planning_group) + model = robot.get_robot_model() + logger.info("MoveItPy instance created") + plan_params = PlanRequestParameters(robot) + plan_params.planning_pipeline = "ompl" + plan_params.planner_id = "RRTConnectkConfigDefault" + plan_params.max_velocity_scaling_factor = 0.12 + plan_params.max_acceleration_scaling_factor = 0.08 + plan_params.planning_time = cfg.planning_time_sec + logger.info(f"Planner params: pipeline=ompl planner=RRTConnectkConfigDefault planning_time={cfg.planning_time_sec:.1f}s") + ptp_params = PlanRequestParameters(robot) + ptp_params.planning_pipeline = "pilz_industrial_motion_planner" + ptp_params.planner_id = "PTP" + ptp_params.max_velocity_scaling_factor = 0.10 + ptp_params.max_acceleration_scaling_factor = 0.08 + ptp_params.planning_time = cfg.planning_time_sec + logger.info("Joint approach params: pipeline=pilz_industrial_motion_planner planner=PTP measured press joints") + lin_params = PlanRequestParameters(robot) + lin_params.planning_pipeline = "pilz_industrial_motion_planner" + lin_params.planner_id = "LIN" + lin_params.max_velocity_scaling_factor = 0.04 + lin_params.max_acceleration_scaling_factor = 0.04 + lin_params.planning_time = cfg.planning_time_sec + logger.info("Press params: pipeline=pilz_industrial_motion_planner planner=LIN z-only Cartesian stroke") + + # 1. 컵을 디스펜서 앞에 갖다 놓기: measured outlet pose. + cup_place = list(outlet.outlet_xyz_m) + if cfg.cup_place_z is not None: + cup_place[2] = cfg.cup_place_z + plan_and_execute_pose(robot, arm, plan_params, cfg, "cup_to_measured_dispenser_front", cup_place, outlet.outlet_quat_xyzw, logger) + gripper_event(logger, "OPEN", "컵을 디스펜서 앞에 놓기 위해 그리퍼 펴기") + + # 2. 수출구/nozzle 회피: 컵을 놓은 자리에서 바로 Z 상승하지 않는다. + # 먼저 base_link X 방향으로 뒤로 빠진 뒤, 그 뒤쪽 위치에서만 Z를 올린다. + cup_lift = [ + cup_place[0] - cfg.cup_release_retract_m, + cup_place[1], + cup_place[2] + max(cfg.cup_lift_m, 0.0), + ] + logger.info( + "CUP_RELEASE_NOZZLE_AVOIDANCE: after opening gripper, move to a behind-and-up pre-lift pose " + f"retract_x={cfg.cup_release_retract_m:.3f}m lift_z={cfg.cup_lift_m:.3f}m " + f"from=({cup_place[0]:.3f}, {cup_place[1]:.3f}, {cup_place[2]:.3f}) " + f"to=({cup_lift[0]:.3f}, {cup_lift[1]:.3f}, {cup_lift[2]:.3f})" + ) + plan_and_execute_pose(robot, arm, plan_params, cfg, "move_to_behind_up_pre_lift_nozzle_avoid", cup_lift, outlet.outlet_quat_xyzw, logger) + + # 3. 프레스 자세를 위해 그리퍼 오므리기 + 측정한 접촉 조인트 자세로 이동. + gripper_event(logger, "CLOSE", "프레스 자세 준비를 위해 그리퍼 오므리기") + plan_and_execute_joints( + robot, + arm, + model, + ptp_params, + cfg, + f"move_to_measured_press_contact_joints_{cfg.dispenser_id}", + outlet.press_contact_joints_deg, + logger, + ) + + # 4. 프레스는 measured contact joints의 FK pose를 기준으로 한다. + # 여기서부터는 다른 IK 해석으로 팔을 흔들지 말고, 같은 X/Y/orientation에서 Z만 LIN 이동한다. + press_contact_pose = fk_pose_from_joints(model, outlet.press_contact_joints_deg, cfg.ee_link, logger) + press_ready_pose = clone_pose_with_z( + press_contact_pose, + press_contact_pose.position.z + max(cfg.press_up_m, 0.0), + ) + logger.info( + "PRESS_Z_ONLY: after measured press joint, repeating LIN strokes with fixed " + f"x={press_contact_pose.position.x:.3f}, y={press_contact_pose.position.y:.3f}, " + f"contact_z={press_contact_pose.position.z:.3f}, ready_z={press_ready_pose.position.z:.3f}" + ) + plan_and_execute_pose_stamped( + robot, + arm, + lin_params, + cfg, + "linear_lift_from_contact_to_press_ready_z_only", + pose_stamped_from_pose(cfg, press_ready_pose), + logger, + ) + for index in range(1, cfg.press_count + 1): + plan_and_execute_pose_stamped( + robot, + arm, + lin_params, + cfg, + f"press_{index}_down_z_only", + pose_stamped_from_pose(cfg, press_contact_pose), + logger, + ) + plan_and_execute_pose_stamped( + robot, + arm, + lin_params, + cfg, + f"press_{index}_up_z_only", + pose_stamped_from_pose(cfg, press_ready_pose), + logger, + ) + + # 5. 다시 컵 잡기: 목표점으로 바로 꽂지 않는다. + # 컵 grasp pose보다 base_link X 방향으로 뒤(backoff)인 pre-grasp에 먼저 가고, + # 같은 orientation으로 직선 접근 후 그리퍼를 닫는다. + cup_pre_grasp = [ + cup_place[0] - cfg.cup_pre_grasp_backoff_m, + cup_place[1], + cup_place[2], + ] + logger.info( + "CUP_PRE_GRASP: move behind cup before final grasp " + f"backoff_x={cfg.cup_pre_grasp_backoff_m:.3f}m pre=({cup_pre_grasp[0]:.3f}, {cup_pre_grasp[1]:.3f}, {cup_pre_grasp[2]:.3f}) " + f"grasp=({cup_place[0]:.3f}, {cup_place[1]:.3f}, {cup_place[2]:.3f})" + ) + plan_and_execute_pose(robot, arm, plan_params, cfg, "return_to_cup_pre_grasp_backoff", cup_pre_grasp, outlet.outlet_quat_xyzw, logger) + plan_and_execute_pose(robot, arm, lin_params, cfg, "linear_approach_to_cup_grasp", cup_place, outlet.outlet_quat_xyzw, logger) + gripper_event(logger, "CLOSE", "pre-grasp에서 직선 접근 후 다시 컵 잡기") + logger.info("DONE: measured dispenser press cycle completed by MoveItPy robot.execute().") + except Exception as exc: + logger.error(f"FAILED: measured dispenser press cycle failed: {exc}") + raise + finally: + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/azas_motion/dispenser_sequence_preview_node.py b/src/azas_motion/azas_motion/dispenser_sequence_preview_node.py index b66e543..51197e5 100644 --- a/src/azas_motion/azas_motion/dispenser_sequence_preview_node.py +++ b/src/azas_motion/azas_motion/dispenser_sequence_preview_node.py @@ -29,6 +29,7 @@ class SequenceStep: label: str xyz: XYZ + cup_base_xyz: XYZ | None = None def point(xyz: XYZ) -> Point: @@ -66,7 +67,7 @@ def __init__(self) -> None: self.declare_parameter("side_pre_grasp_offset_m", 0.10) self.declare_parameter("lift_height_m", 0.04) self.declare_parameter("shake_clearance_m", 0.13) - self.declare_parameter("shake_swing_m", 0.075) + self.declare_parameter("shake_swing_m", 0.109) self.declare_parameter("shake_lift_m", 0.055) self.declare_parameter("outlet_mouth_clearance_m", 0.0) self.declare_parameter("publish_rate_hz", 4.0) @@ -95,18 +96,18 @@ def __init__(self) -> None: self.declare_parameter( "dispenser_outlet_positions", [ - 0.609, - 0.070, - 0.087, - 0.617, - 0.028, - 0.082, - 0.616, - -0.026, - 0.079, - 0.607, - -0.083, - 0.075, + 0.555, + -0.100, + 0.093, + 0.549, + -0.150, + 0.097, + 0.527, + -0.204, + 0.107, + 0.517, + -0.235, + 0.109, ], ) @@ -176,6 +177,17 @@ def build_steps(self, cup_pose: PoseStamped) -> List[SequenceStep]: lift = (grasp[0], grasp[1], low_transfer_z) front_lane = (outlet[0] - 0.12, grasp[1], low_transfer_z) outlet_front_hold = (outlet[0], outlet[1], low_transfer_z) + cup_front_base = ( + outlet_front_hold[0], + outlet_front_hold[1], + outlet_front_hold[2] - grasp_height, + ) + empty_lift = (outlet_front_hold[0] - 0.10, outlet_front_hold[1], low_transfer_z + 0.20) + press_ready = (outlet[0] - 0.03, outlet[1], low_transfer_z + 0.28) + press_down = (outlet[0] - 0.03, outlet[1], max(outlet[2] + 0.035, low_transfer_z + 0.12)) + regrasp_pre = (outlet_front_hold[0] - 0.10, outlet_front_hold[1], outlet_front_hold[2]) + regrasp = outlet_front_hold + regrasp_lift = (outlet_front_hold[0], outlet_front_hold[1], low_transfer_z + 0.12) shake_z = max(low_transfer_z + shake_clearance, 0.55) shake_center = (outlet_front_hold[0] - 0.15, outlet_front_hold[1] - 0.38, shake_z + shake_lift) shake_left = ( @@ -204,13 +216,20 @@ def build_steps(self, cup_pose: PoseStamped) -> List[SequenceStep]: SequenceStep("2 side_grasp", grasp), SequenceStep("3 lift_cup", lift), SequenceStep("4 carry_to_front_lane", front_lane), - SequenceStep("5 outlet_front_hold", outlet_front_hold), - SequenceStep("9 shake_center", shake_center), - SequenceStep("10 shake_left", shake_left), - SequenceStep("11 shake_right", shake_right), - SequenceStep("12 shake_forward", shake_forward), - SequenceStep("13 shake_back", shake_back), - SequenceStep("14 shake_recenter", shake_center), + SequenceStep("5 place_cup_front", outlet_front_hold), + SequenceStep("6 release_cup_front", outlet_front_hold, cup_front_base), + SequenceStep("7 lift_empty_gripper", empty_lift, cup_front_base), + SequenceStep("8 press_ready", press_ready, cup_front_base), + SequenceStep("9 press_dispenser", press_down, cup_front_base), + SequenceStep("10 return_to_cup", regrasp_pre, cup_front_base), + SequenceStep("11 regrasp_cup", regrasp, cup_front_base), + SequenceStep("12 regrasp_lift", regrasp_lift), + SequenceStep("13 shake_center", shake_center), + SequenceStep("14 shake_left", shake_left), + SequenceStep("15 shake_right", shake_right), + SequenceStep("16 shake_forward", shake_forward), + SequenceStep("17 shake_back", shake_back), + SequenceStep("18 shake_recenter", shake_center), ] def publish_preview(self) -> None: @@ -341,7 +360,7 @@ def make_markers( Marker.TEXT_VIEW_FACING, "sequence_labels", (step.xyz[0], step.xyz[1], step.xyz[2] + 0.04), - Vector3(x=0.0, y=0.0, z=0.028), + Vector3(x=0.0, y=0.0, z=-0.150), (1.0, 1.0, 1.0, 1.0), ) label.text = step.label @@ -355,7 +374,7 @@ def make_markers( Vector3(x=0.0, y=0.0, z=0.033), (0.2, 1.0, 0.35, 1.0), ) - status.text = "Azas RViz preview: pick cup -> carry to dispenser -> shake" + status.text = "Azas RViz preview: pick -> place front -> press -> re-grasp -> shake" markers.append(status) for marker in markers: @@ -374,7 +393,7 @@ def active_step(self, steps: Sequence[SequenceStep]) -> SequenceStep | None: def make_demo_arm_markers(self, active_step: SequenceStep) -> List[Marker]: x, y, z = active_step.xyz - wrist = (x - 0.075, y, max(z, 0.10)) + wrist = (x - 0.109, y, max(z, 0.10)) shoulder = (0.0, 0.0, 0.225) elbow = ( max(0.10, wrist[0] * 0.48), @@ -421,7 +440,7 @@ def make_demo_arm_markers(self, active_step: SequenceStep) -> List[Marker]: Marker.CUBE, "low_side_grasp_rg2", (x - 0.035, y, z - 0.012), - Vector3(x=0.070, y=0.045, z=0.025), + Vector3(x=-0.100, y=0.045, z=0.025), (0.16, 0.17, 0.18, 0.92), ) finger_a = self.marker( @@ -429,7 +448,7 @@ def make_demo_arm_markers(self, active_step: SequenceStep) -> List[Marker]: Marker.CUBE, "low_side_grasp_rg2", (x + 0.010, y + 0.038, z - 0.030), - Vector3(x=0.075, y=0.010, z=0.052), + Vector3(x=0.109, y=0.010, z=0.052), (0.06, 0.06, 0.07, 0.92), ) finger_b = self.marker( @@ -437,7 +456,7 @@ def make_demo_arm_markers(self, active_step: SequenceStep) -> List[Marker]: Marker.CUBE, "low_side_grasp_rg2", (x + 0.010, y - 0.038, z - 0.030), - Vector3(x=0.075, y=0.010, z=0.052), + Vector3(x=0.109, y=0.010, z=0.052), (0.06, 0.06, 0.07, 0.92), ) markers.extend([palm, finger_a, finger_b]) @@ -453,7 +472,9 @@ def make_cup_markers(self, steps: Sequence[SequenceStep]) -> List[Marker]: active_index = min(self.preview_step_index, len(steps) - 1) active_step = steps[active_index] - if active_index == 0: + if active_step.cup_base_xyz is not None: + base = active_step.cup_base_xyz + elif active_index == 0: base = original_base else: base = ( @@ -469,7 +490,7 @@ def make_cup_markers(self, steps: Sequence[SequenceStep]) -> List[Marker]: Marker.CYLINDER, "animated_cup_body", body_center, - Vector3(x=0.075, y=0.075, z=cup_height), + Vector3(x=0.109, y=0.109, z=cup_height), (0.1, 0.85, 1.0, 0.72), ) mouth = self.marker( diff --git a/src/azas_motion/azas_motion/doosan_moveit_grasped_tumbler_to_dispenser_node.py b/src/azas_motion/azas_motion/doosan_moveit_grasped_tumbler_to_dispenser_node.py index d524657..92f0858 100644 --- a/src/azas_motion/azas_motion/doosan_moveit_grasped_tumbler_to_dispenser_node.py +++ b/src/azas_motion/azas_motion/doosan_moveit_grasped_tumbler_to_dispenser_node.py @@ -94,18 +94,18 @@ def __init__(self) -> None: self.declare_parameter( "dispenser_outlet_positions", [ - 0.609, - 0.070, - 0.087, - 0.617, - 0.028, - 0.082, - 0.616, - -0.026, - 0.079, - 0.607, - -0.083, - 0.075, + 0.555, + -0.100, + 0.093, + 0.549, + -0.150, + 0.097, + 0.527, + -0.204, + 0.107, + 0.517, + -0.235, + 0.109, ], ) diff --git a/src/azas_motion/azas_motion/m0609_shake_joint_state_node.py b/src/azas_motion/azas_motion/m0609_shake_joint_state_node.py index f5ef332..02fc17d 100644 --- a/src/azas_motion/azas_motion/m0609_shake_joint_state_node.py +++ b/src/azas_motion/azas_motion/m0609_shake_joint_state_node.py @@ -13,9 +13,10 @@ class M0609ShakeJointStateNode(Node): def __init__(self) -> None: super().__init__("m0609_shake_joint_state_node") - self.declare_parameter("publish_rate", 30.0) - self.declare_parameter("shake_cycles_per_second", 3.2) - self.declare_parameter("preview_mode", "shake") + self.declare_parameter("publish_rate", 60.0) + self.declare_parameter("shake_cycles_per_second", 0.55) + self.declare_parameter("preview_mode", "side_grasp_move_then_shake") + self.declare_parameter("loop_motion", True) self.declare_parameter( "home_joints_rad", [0.0, math.radians(-35.0), math.radians(50.0), 0.0, math.radians(70.0), 0.0], @@ -26,7 +27,7 @@ def __init__(self) -> None: rate = max(float(self.get_parameter("publish_rate").value), 1.0) self.timer = self.create_timer(1.0 / rate, self.publish_joint_state) self.get_logger().info( - "Publishing RViz-only M0609 joint states for side-grasp / shake visualization." + "Publishing smooth RViz M0609 robot motion from /joint_states; no path display." ) def publish_joint_state(self) -> None: @@ -55,32 +56,42 @@ def publish_joint_state(self) -> None: def high_shake_joints(self, elapsed: float, home: list[float]) -> list[float]: freq = max(float(self.get_parameter("shake_cycles_per_second").value), 0.1) phase = elapsed * math.tau * freq - j5_swing = math.sin(phase) - wrist_counter = math.sin(phase + math.pi * 0.5) - elbow_pulse = math.sin(phase * 0.5) - wrist_snap = math.sin(phase * 1.7 + math.pi * 0.25) + # Deliberately slow/small: this is for readable RViz robot motion, not + # a high-frequency fake shake. The cup stays generally upright while + # the wrist shows a gentle mixing motion. + wrist_roll = math.sin(phase) + wrist_pitch = math.sin(phase + math.pi * 0.5) + wrist_yaw = math.sin(phase * 0.5) return [ home[0], home[1], home[2], - home[3] + math.radians(18.0) * wrist_counter, - home[4] + math.radians(30.0) * j5_swing, - home[5] + math.radians(36.0) * wrist_counter + math.radians(8.0) * wrist_snap, + home[3] + math.radians(7.0) * wrist_roll, + home[4] + math.radians(10.0) * wrist_pitch, + home[5] + math.radians(12.0) * wrist_yaw, ] def side_grasp_move_then_shake_joints(self, elapsed: float, home: list[float]) -> list[float]: + # Joint-space storyboard for RViz visibility only. It follows the + # dispenser task shape without publishing Path/markers as the primary + # visual: approach -> side grasp -> lift -> dispenser hold -> retreat + # -> gentle wrist shake. Every segment uses minimum-jerk interpolation + # so the robot model moves smoothly instead of snapping. keyframes = [ (0.0, home), - (2.0, [0.18, -0.76, 1.48, -0.22, 1.18, 0.20]), # side pre-grasp - (4.0, [0.25, -0.82, 1.56, -0.10, 1.12, 0.12]), # side grasp - (7.5, [0.30, -0.55, 1.22, 0.05, 1.05, 0.10]), # slower lift with cup - (9.5, [-0.12, -0.48, 1.10, 0.18, 1.05, -0.10]), # carry to dispenser - (11.0, [-0.20, -0.60, 1.30, 0.10, 1.18, 0.00]), # outlet front - (13.0, [-0.05, -0.42, 1.05, 0.00, 1.02, 0.00]), # retreat before shake + (3.0, [0.16, -0.70, 1.36, -0.16, 1.17, 0.12]), # side pre-grasp + (5.8, [0.24, -0.76, 1.46, -0.08, 1.13, 0.08]), # side grasp + (8.8, [0.28, -0.58, 1.25, 0.02, 1.08, 0.07]), # lift with cup + (12.8, [-0.08, -0.52, 1.18, 0.10, 1.07, -0.05]), # carry to dispenser + (15.8, [-0.18, -0.62, 1.32, 0.06, 1.18, 0.00]), # outlet front hold + (18.8, [-0.06, -0.48, 1.12, 0.00, 1.08, 0.00]), # retreat before shake ] - cycle_seconds = 19.0 - t = elapsed % cycle_seconds + cycle_seconds = 30.0 + if bool(self.get_parameter("loop_motion").value): + t = elapsed % cycle_seconds + else: + t = min(elapsed, cycle_seconds) if t >= keyframes[-1][0]: shake_elapsed = t - keyframes[-1][0] shake_home = keyframes[-1][1] @@ -91,7 +102,7 @@ def side_grasp_move_then_shake_joints(self, elapsed: float, home: list[float]) - end_t, end_joints = keyframes[index + 1] if start_t <= t < end_t: ratio = (t - start_t) / max(end_t - start_t, 1e-6) - smooth = 0.5 - 0.5 * math.cos(math.pi * ratio) + smooth = self.minimum_jerk(ratio) return [ start + (end - start) * smooth for start, end in zip(start_joints, end_joints) @@ -118,13 +129,18 @@ def cup_target_move_joints(self, elapsed: float, home: list[float]) -> list[floa end_t, end_joints = keyframes[index + 1] if start_t <= t < end_t: ratio = (t - start_t) / max(end_t - start_t, 1e-6) - smooth = 0.5 - 0.5 * math.cos(math.pi * ratio) + smooth = self.minimum_jerk(ratio) return [ start + (end - start) * smooth for start, end in zip(start_joints, end_joints) ] return home + @staticmethod + def minimum_jerk(ratio: float) -> float: + ratio = max(0.0, min(1.0, ratio)) + return ratio * ratio * ratio * (10.0 - 15.0 * ratio + 6.0 * ratio * ratio) + def main(args: list[str] | None = None) -> None: rclpy.init(args=args) diff --git a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py index 2194b39..75d6c3c 100644 --- a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py +++ b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py @@ -174,6 +174,7 @@ def __init__(self) -> None: self._warn_about_draft_status() self._warn_about_front_hold_overlaps() self._legacy_collision_objects_removed = False + self._published_ids_logged = False self._publish_scene() period = ( @@ -240,12 +241,19 @@ def _publish_scene(self) -> None: for object_id in LEGACY_DISPENSER_COLLISION_OBJECT_IDS: self.collision_pub.publish(self._make_remove_collision_object(object_id)) self._legacy_collision_objects_removed = True + published_ids = [] for object_id, object_config in collision_objects.items(): if not object_config.get("publish_to_planning_scene", True): continue self.collision_pub.publish( self._make_collision_object(object_id, object_config) ) + published_ids.append(object_id) + if published_ids and not self._published_ids_logged: + self.get_logger().info( + "Publishing measured dispenser collision objects: " + ", ".join(published_ids) + ) + self._published_ids_logged = True if self.publish_markers: markers = self._make_markers(collision_objects) diff --git a/src/azas_motion/azas_motion/tumbler_floor_place_node.py b/src/azas_motion/azas_motion/tumbler_floor_place_node.py index 6a1ef14..5180dc4 100644 --- a/src/azas_motion/azas_motion/tumbler_floor_place_node.py +++ b/src/azas_motion/azas_motion/tumbler_floor_place_node.py @@ -217,7 +217,7 @@ def __init__(self) -> None: self.declare_parameter("tumbler_height", 0.17) self.declare_parameter("tumbler_radius", 0.0375) self.declare_parameter("tumbler_bottom_diameter", 0.065) - self.declare_parameter("tumbler_top_diameter", 0.075) + self.declare_parameter("tumbler_top_diameter", 0.109) self.declare_parameter("grasp_height", 0.085) self.declare_parameter("side_grasp_approach_offset", 0.10) self.declare_parameter("side_grasp_candidate_count", 16) @@ -251,18 +251,18 @@ def __init__(self) -> None: self.declare_parameter( "dispenser_outlet_positions", [ - 0.609, - 0.070, - 0.087, - 0.617, - 0.028, - 0.082, - 0.616, - -0.026, - 0.079, - 0.607, - -0.083, - 0.075, + 0.555, + -0.100, + 0.093, + 0.549, + -0.150, + 0.097, + 0.527, + -0.204, + 0.107, + 0.517, + -0.235, + 0.109, ], ) @@ -278,6 +278,7 @@ def __init__(self) -> None: self.declare_parameter("line_acceleration", 50.0) self.declare_parameter("hold_seconds_after_grasp", 0.2) self.declare_parameter("hold_seconds_after_place", 0.2) + self.declare_parameter("motion_response_timeout_sec", 45.0) self.declare_parameter("workspace_x_min", 0.0) self.declare_parameter("workspace_x_max", 0.80) @@ -286,6 +287,7 @@ def __init__(self) -> None: self.declare_parameter("workspace_z_min", 0.0) self.declare_parameter("workspace_z_max", 0.80) + self.declare_parameter("disable_gripper_commands", False) self.declare_parameter("gripper_open_service", "") self.declare_parameter("gripper_close_service", "") self.declare_parameter("gripper_set_service", "/jarvis/rg2/set_width") @@ -801,7 +803,9 @@ def call_motion(self, client, request, label: str) -> bool: return False return True - def wait_for_future(self, future, label: str, timeout_sec: float = 10.0): + def wait_for_future(self, future, label: str, timeout_sec: float | None = None): + if timeout_sec is None: + timeout_sec = max(float(self.get_parameter("motion_response_timeout_sec").value), 0.1) deadline = time.monotonic() + timeout_sec while rclpy.ok() and not future.done(): if time.monotonic() > deadline: @@ -826,16 +830,23 @@ def execute_hardware(self, steps: Sequence[MotionStep]) -> bool: return False for step in steps: - if step.gripper == "preopen" and not self.command_gripper(step): - return False + if step.gripper == "preopen": + if bool(self.get_parameter("disable_gripper_commands").value): + self.get_logger().warning("preopen_gripper: disabled for controller/RViz mirror run") + elif not self.command_gripper(step): + return False if not self.call_movel(step): return False if step.gripper == "close": - if not self.command_gripper(step): + if bool(self.get_parameter("disable_gripper_commands").value): + self.get_logger().warning("close_gripper: disabled for controller/RViz mirror run") + elif not self.command_gripper(step): return False time.sleep(float(self.get_parameter("hold_seconds_after_grasp").value)) elif step.gripper == "open": - if not self.command_gripper(step): + if bool(self.get_parameter("disable_gripper_commands").value): + self.get_logger().warning("open_gripper: disabled for controller/RViz mirror run") + elif not self.command_gripper(step): return False time.sleep(float(self.get_parameter("hold_seconds_after_place").value)) diff --git a/src/azas_motion/azas_motion/tumbler_shake_sequence_node.py b/src/azas_motion/azas_motion/tumbler_shake_sequence_node.py index 039d18f..414b1f4 100644 --- a/src/azas_motion/azas_motion/tumbler_shake_sequence_node.py +++ b/src/azas_motion/azas_motion/tumbler_shake_sequence_node.py @@ -381,7 +381,7 @@ def step( ) -> JointSequenceStep: return JointSequenceStep( label=label, - joints_deg=tuple(value + delta for value, delta in zip(base, deltas)), + joints_deg=tuple(float(value + delta) for value, delta in zip(base, deltas)), hold_seconds=hold if phase == "shake" else 0.0, phase=phase, ) @@ -795,7 +795,7 @@ def call_movej(self, step: JointSequenceStep) -> bool: self.get_logger().info( f"{step.label}: calling hardware service " - f"joints_deg={[round(value, 1) for value in req.pos]} " + f"joints_deg={[round(float(value), 1) for value in req.pos]} " f"time={req.time:.2f} vel={req.vel:.1f} acc={req.acc:.1f}" ) future = self.move_joint.call_async(req) @@ -815,7 +815,7 @@ def call_movej(self, step: JointSequenceStep) -> bool: if result is None or not result.success: self.get_logger().error( f"{step.label} returned success=false for " - f"joints_deg={[round(value, 1) for value in req.pos]}. " + f"joints_deg={[round(float(value), 1) for value in req.pos]}. " "Check the Doosan controller log for the exact reject reason." ) return False @@ -1109,7 +1109,7 @@ def run_once(self) -> bool: for step in joint_steps: self.get_logger().info( f"plan {step.label}: joints_deg=" - f"{[round(value, 1) for value in step.joints_deg]} " + f"{[round(float(value), 1) for value in step.joints_deg]} " f"phase={step.phase} time={self._joint_time_for_step(step):.2f} " f"hold={step.hold_seconds:.2f}" ) diff --git a/src/azas_motion/setup.py b/src/azas_motion/setup.py index 5cc8733..17cd7c3 100644 --- a/src/azas_motion/setup.py +++ b/src/azas_motion/setup.py @@ -33,6 +33,7 @@ "doosan_moveit_grasped_tumbler_to_dispenser_node = azas_motion.doosan_moveit_grasped_tumbler_to_dispenser_node:main", "gear_assembly_legacy = azas_motion.gear_assembly_legacy:main", "m0609_shake_joint_state_node = azas_motion.m0609_shake_joint_state_node:main", + "dispenser_press_cycle_moveit_node = azas_motion.dispenser_press_cycle_moveit_node:main", "measured_dispenser_collision_scene_node = azas_motion.measured_dispenser_collision_scene_node:main", "mp_basic_legacy = azas_motion.mp_basic_legacy:main", "mp_waypoint_legacy = azas_motion.mp_waypoint_legacy:main", diff --git a/src/azas_perception/config/measured_dispenser_collision.yaml b/src/azas_perception/config/measured_dispenser_collision.yaml index 850fd95..43a24ec 100644 --- a/src/azas_perception/config/measured_dispenser_collision.yaml +++ b/src/azas_perception/config/measured_dispenser_collision.yaml @@ -1,66 +1,199 @@ -# Measured dispenser collision draft from real robot teaching. -# -# IMPORTANT: -# - These values were measured as base_link -> link_6 probe poses while the -# gripper/link_6 assembly was placed against the dispenser. They are not -# direct surface coordinates and must be reviewed with TCP/gripper offset -# before enabling hard real-motion collision enforcement. -# - The boxes below are conservative primitive estimates for MoveIt Planning -# Scene. Keep them disabled until verified in RViz against the real workcell. - metadata: frame_id: base_link measured_target_frame: link_6 source: operator_teaching_tf2_echo status: measured_draft_single_box_not_enabled - body_bottom_z_m: 0.000 + body_bottom_z_m: 0.0 body_bottom_reason: dispenser bottles start on same floor plane as robot base margin_m: - x: 0.020 - y: 0.020 - z: 0.020 - + x: 0.02 + y: 0.02 + z: 0.02 front_hold_poses: - # Cup hold/front limit poses. Cup pose itself still comes from vision. dispenser_1: - position_xyz_m: [0.609000, 0.070000, 0.087000] - quaternion_xyzw: [0.489000, 0.517000, 0.517000, 0.475000] - rpy_deg: [90.798, -0.845, 93.984] + position_xyz_m: + - 0.609 + - 0.07 + - 0.087 + quaternion_xyzw: + - 0.489 + - 0.517 + - 0.517 + - 0.475 + rpy_deg: + - 90.798 + - -0.845 + - 93.984 dispenser_2: - position_xyz_m: [0.617000, 0.028000, 0.082000] - quaternion_xyzw: [0.504000, 0.504000, 0.500000, 0.491000] - rpy_deg: [90.946, -0.512, 90.501] + position_xyz_m: + - 0.617 + - 0.028 + - 0.082 + quaternion_xyzw: + - 0.504 + - 0.504 + - 0.5 + - 0.491 + rpy_deg: + - 90.946 + - -0.512 + - 90.501 dispenser_3: - position_xyz_m: [0.616000, -0.026000, 0.079000] - quaternion_xyzw: [0.504000, 0.504000, 0.498000, 0.494000] - rpy_deg: [90.926, -0.278, 90.234] + position_xyz_m: + - 0.616 + - -0.026 + - 0.079 + quaternion_xyzw: + - 0.504 + - 0.504 + - 0.498 + - 0.494 + rpy_deg: + - 90.926 + - -0.278 + - 90.234 dispenser_4: - position_xyz_m: [0.607000, -0.083000, 0.075000] - quaternion_xyzw: [0.511000, 0.498000, 0.492000, 0.499000] - rpy_deg: [91.042, -0.280, 88.871] - + position_xyz_m: + - 0.607 + - -0.083 + - 0.075 + quaternion_xyzw: + - 0.511 + - 0.498 + - 0.492 + - 0.499 + rpy_deg: + - 91.042 + - -0.28 + - 88.871 raw_probe_poses: left_front_bottom_probe: - position_xyz_m: [0.767, 0.072, 0.219] - quaternion_xyzw: [0.642, 0.732, 0.215, 0.079] - rpy_deg: [155.003, -9.233, 99.561] + position_xyz_m: + - 0.767 + - 0.072 + - 0.219 + quaternion_xyzw: + - 0.642 + - 0.732 + - 0.215 + - 0.079 + rpy_deg: + - 155.003 + - -9.233 + - 99.561 right_back_top_probe: - position_xyz_m: [0.763, -0.103, 0.412] - quaternion_xyzw: [0.658, 0.654, 0.200, 0.313] - rpy_deg: [137.024, 8.372, 86.350] - + position_xyz_m: + - 0.763 + - -0.103 + - 0.412 + quaternion_xyzw: + - 0.658 + - 0.654 + - 0.2 + - 0.313 + rpy_deg: + - 137.024 + - 8.372 + - 86.35 estimated_collision_objects: dispenser_combined_body_box: type: box frame_id: base_link - # Single measured draft box from left-front-bottom and right-back-top - # link_6 probe poses. X/Y are expanded by metadata.margin_m on each side; - # Z uses body_bottom_z_m as the dispenser rests on the base/table plane. - center_xyz_m: [0.7650, -0.0155, 0.2160] - size_xyz_m: [0.0440, 0.2150, 0.4320] + center_xyz_m: + - 0.765 + - -0.0155 + - 0.216 + size_xyz_m: + - 0.044 + - 0.215 + - 0.432 bounds_xyz_m: - min: [0.7430, -0.1230, 0.0000] - max: [0.7870, 0.0920, 0.4320] - orientation_xyzw: [0.0, 0.0, 0.0, 1.0] + min: + - 0.743 + - -0.123 + - 0.0 + max: + - 0.787 + - 0.092 + - 0.432 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + dispenser_1_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - 0.084 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - -0.1368 + - 0.9906 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# note': x is aligned to the front face of dispenser_combined_body_box; z/y keep + measured lane/height. + dispenser_2_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - 0.043 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - -0.1368 + - 0.9906 + publish_to_planning_scene: true + enabled_for_real_motion: false + dispenser_3_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - -0.002 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - -0.1368 + - 0.9906 + publish_to_planning_scene: true + enabled_for_real_motion: false + dispenser_4_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - -0.05 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - -0.1368 + - 0.9906 publish_to_planning_scene: true enabled_for_real_motion: false diff --git a/src/azas_voice/azas_voice/command_parser.py b/src/azas_voice/azas_voice/command_parser.py index 43511b8..431a35b 100644 --- a/src/azas_voice/azas_voice/command_parser.py +++ b/src/azas_voice/azas_voice/command_parser.py @@ -10,6 +10,7 @@ MOOD_WORDS, RANDOM_RECIPE_WORDS, RECIPE_ALIASES, + RECIPE_DESCRIPTIONS, RECIPE_DISPENSERS, RECIPE_DISPLAY_NAMES, ) @@ -25,9 +26,11 @@ class RecipeDecision: dispenser_ids: tuple[str, ...] confirmation: str error: str | None = None + # extra: LLM이 추가 필드(profile, dispenser_amounts 등)를 리턴할 때 pass-through + extra: dict | None = None def to_dict(self) -> dict[str, object]: - return { + d = { "valid": self.valid, "utterance": self.utterance, "normalized": self.normalized, @@ -37,6 +40,9 @@ def to_dict(self) -> dict[str, object]: "confirmation": self.confirmation, "error": self.error, } + if self.extra: + d.update(self.extra) + return d def normalize_text(text: str) -> str: @@ -74,11 +80,17 @@ def _recipe_name(recipe_id: str) -> str: return RECIPE_DISPLAY_NAMES.get(recipe_id, recipe_id) +def _recipe_description(recipe_id: str) -> str: + return RECIPE_DESCRIPTIONS.get(recipe_id, "") + + def _random_recipe_decision(utterance: str, normalized: str) -> RecipeDecision: recipe_id = random.choice(tuple(RECIPE_DISPENSERS)) dispenser_ids = RECIPE_DISPENSERS[recipe_id] + description = _recipe_description(recipe_id) confirmation = ( - f"오늘 기분에는 {_recipe_name(recipe_id)}를 추천합니다. " + f"{_recipe_name(recipe_id)}를 추천드릴게요. " + f"{description} " f"진행할까요?" ) return RecipeDecision(True, utterance, normalized, "make_cocktail", recipe_id, dispenser_ids, confirmation) diff --git a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py index 6e92dde..913b62a 100644 --- a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py +++ b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py @@ -65,9 +65,11 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: ) recipe_id = payload.get("recipe_id") recipe_id = str(recipe_id).strip() if recipe_id else None - if recipe_id and not recipe_id.startswith("recipe_") and recipe_id != "custom_color_selection": + _ALLOWED_CUSTOM_RECIPE_IDS = {"custom_color_selection", "custom_preference_mix"} + if recipe_id and not recipe_id.startswith("recipe_") and recipe_id not in _ALLOWED_CUSTOM_RECIPE_IDS: recipe_id = None - if recipe_id and recipe_id != "custom_color_selection" and not dispenser_ids: + _non_custom = recipe_id and recipe_id not in _ALLOWED_CUSTOM_RECIPE_IDS + if _non_custom and not dispenser_ids: dispenser_ids = RECIPE_DISPENSERS.get(recipe_id, ()) if intent == "make_cocktail" and recipe_id is None and not dispenser_ids: @@ -87,6 +89,17 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: recipe_name = RECIPE_DISPLAY_NAMES.get(str(recipe_id), str(recipe_id)) confirmation = f"{recipe_name} 요청을 인식했습니다. 진행할까요?" + # pass-through extra fields from LLM (profile, dispenser_amounts) + extra: dict | None = None + if recipe_id == "custom_preference_mix": + extra = {} + if "profile" in payload: + extra["profile"] = payload["profile"] + if "dispenser_amounts" in payload: + extra["dispenser_amounts"] = payload["dispenser_amounts"] + if not extra: + extra = None + fallback = parse_recipe_command(text) return RecipeDecision( valid, @@ -97,6 +110,7 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: dispenser_ids, confirmation, None if valid else "llm returned unknown intent", + extra, ) @@ -184,11 +198,19 @@ def _call_chat_api(self, text: str, api_key: str) -> dict: "role": "system", "content": ( "Return only JSON for Azas cocktail intent parsing. " - "Allowed fields: valid, intent, recipe_id, dispenser_ids, confirmation. " "Allowed intents: make_cocktail, confirm, cancel, unknown. " - "The user does not know dispenser colors; infer them internally. " - "If the user describes mood or asks for a recommendation, choose one recipe_01..recipe_04. " "Allowed dispenser_ids values: red, yellow, green, blue only. " + "BRANCH A — user describes specific taste/strength preference " + "(e.g. '너무 세지 않게', '향이 풍부하게', '달달하게', '가볍게'): " + "set recipe_id='custom_preference_mix', include relevant dispenser_ids, " + "set profile={{rum,syrup,liqueur,juice: '약하게'|'보통'|'많게'}}, " + "set dispenser_amounts={{color: integer pump count}}. " + "BRANCH B — user asks for recommendation or describes mood without ingredient preference " + "(e.g. '추천해줘', '아무거나', '기분에 맞게'): " + "choose one of recipe_01..recipe_04 and set dispenser_ids accordingly. " + "For random recommendations, mention the menu name and a short taste/aroma description; " + "do not answer only with a color name. " + "Always include a natural Korean confirmation sentence in the confirmation field. " "Never output robot coordinates, calibration values, trajectories, or safety approvals." ), }, diff --git a/src/azas_voice/azas_voice/recipe_catalog.py b/src/azas_voice/azas_voice/recipe_catalog.py index e506fed..a52e866 100644 --- a/src/azas_voice/azas_voice/recipe_catalog.py +++ b/src/azas_voice/azas_voice/recipe_catalog.py @@ -25,6 +25,13 @@ "recipe_04": "블루 메뉴", } +RECIPE_DESCRIPTIONS = { + "recipe_01": "달콤하고 선명한 레드 계열 메뉴입니다.", + "recipe_02": "산뜻하고 가벼운 옐로우 계열 메뉴입니다.", + "recipe_03": "리큐르 중심이라 향이 선명하고 깔끔한 느낌입니다.", + "recipe_04": "시원하고 부드러운 블루 계열 메뉴입니다.", +} + RECIPE_DISPENSERS = { "recipe_01": ("red",), "recipe_02": ("yellow",), diff --git a/tools/checks/check_measured_dispenser_geometry.py b/tools/checks/check_measured_dispenser_geometry.py index 16bcc3e..88f8582 100755 --- a/tools/checks/check_measured_dispenser_geometry.py +++ b/tools/checks/check_measured_dispenser_geometry.py @@ -27,18 +27,18 @@ TOLERANCE_M = float(os.environ.get("DISPENSER_GEOMETRY_TOLERANCE_M", "0.003")) EXPECTED_OUTLETS = { - "1": [0.609, 0.070, 0.087], - "2": [0.617, 0.028, 0.082], - "3": [0.616, -0.026, 0.079], - "4": [0.607, -0.083, 0.075], + "1": [0.555, -0.100, 0.093], + "2": [0.549, -0.150, 0.097], + "3": [0.527, -0.204, 0.107], + "4": [0.517, -0.235, 0.109], } # Press stage runs at outlet + press_x_extension and outlet_z - press_depth. EXPECTED_PRESS_DOWN = { - "1": [0.712, 0.071, 0.530], - "2": [0.718, 0.015, 0.525], - "3": [0.716, -0.040, 0.525], - "4": [0.709, -0.084, 0.529], + "1": [0.705, 0.084, 0.520], + "2": [0.706, 0.043, 0.510], + "3": [0.705, -0.002, 0.513], + "4": [0.705, -0.050, 0.513], } diff --git a/tools/checks/check_panel_service_discovery_race.py b/tools/checks/check_panel_service_discovery_race.py index e04db92..1a0d083 100755 --- a/tools/checks/check_panel_service_discovery_race.py +++ b/tools/checks/check_panel_service_discovery_race.py @@ -5,6 +5,7 @@ import importlib.util import sys +import tempfile from pathlib import Path ROOT = Path(__file__).resolve().parents[2] @@ -23,32 +24,76 @@ def load_panel_module(): def main() -> int: panel = load_panel_module() - shake = next(step for step in panel.STEPS if step.key == "shake_closed_cup") - required = panel.required_services_for_step(shake, "dsr01") - calls = {"ros_service_names": 0} - - def wait_ready(required_services, *, timeout_sec=20.0, proc=None): - if required_services != required: - raise AssertionError("unexpected required service set") - return True, "required services became ready after 1 check(s): " + ", ".join(required_services) - - def flaky_service_list(*, timeout_sec=2.0): - calls["ros_service_names"] += 1 - return ["/jarvis/rg2/open", "/jarvis/rg2/close"], "/jarvis/rg2/open\n/jarvis/rg2/close\n" - - panel.wait_for_required_services = wait_ready - panel.ros_service_names = flaky_service_list - - missing, output = panel.missing_required_services(shake, "dsr01") - if missing: - print("[FAIL] ready service wait was converted into a false missing list:", missing) - return 1 - if calls["ros_service_names"] != 0: - print("[FAIL] service list was called after a successful wait sample") - return 1 - if "required services became ready" not in output: - print("[FAIL] ready evidence was not preserved") - return 1 + with tempfile.TemporaryDirectory(prefix="azas_panel_commands_") as temp_dir: + panel.COMMAND_OVERRIDES_PATH = Path(temp_dir) / "panel_command_overrides.json" + + color_scan_pose = next(step for step in panel.STEPS if step.key == "move_to_color_scan_pose") + color_scan_command = panel.command_for(color_scan_pose, {"service_prefix": "dsr01"}) + if "--service-prefix dsr01" not in color_scan_command: + print("[FAIL] color scan pose command does not target namespaced Doosan MoveJoint service") + print(color_scan_command) + return 1 + for expected in ("--j1 0", "--j2 10", "--j3 32", "--j4 0", "--j5 100", "--j6 90"): + if expected not in color_scan_command: + print("[FAIL] color scan pose command does not use the saved camera-view joint target") + print(color_scan_command) + return 1 + color_scan_required = panel.required_services_for_step(color_scan_pose, "dsr01") + if "/dsr01/motion/move_joint" not in color_scan_required: + print("[FAIL] color scan pose preflight does not require namespaced MoveJoint service") + print(color_scan_required) + return 1 + color_scan_order = panel.with_collision_scene_prereq(["color_scan"]) + expected_color_scan_order = ["move_to_color_scan_pose", "start_camera", "color_scan"] + if color_scan_order != expected_color_scan_order: + print("[FAIL] color_scan does not auto-run camera pose and RealSense prerequisites") + print(color_scan_order) + return 1 + + custom_command = "echo custom panel command" + panel.save_command_override("move_to_color_scan_pose", custom_command) + if panel.command_for(color_scan_pose, {"service_prefix": "dsr01"}) != custom_command: + print("[FAIL] saved panel command override was not used by command_for") + return 1 + panel.save_command_override("move_to_color_scan_pose", "") + if panel.command_for(color_scan_pose, {"service_prefix": "dsr01"}) == custom_command: + print("[FAIL] clearing panel command override did not restore generated command") + return 1 + + side_grip = next(step for step in panel.STEPS if step.key == "side_grip") + side_grip_command = panel.command_for(side_grip, {"service_prefix": "dsr01"}) + expected_package_source = "install/dsr_practice/share/dsr_practice/package.bash" + if expected_package_source not in side_grip_command: + print("[FAIL] side_grip command does not force the Azas dsr_practice overlay") + print(side_grip_command) + return 1 + + shake = next(step for step in panel.STEPS if step.key == "shake_closed_cup") + required = panel.required_services_for_step(shake, "dsr01") + calls = {"ros_service_names": 0} + + def wait_ready(required_services, *, timeout_sec=20.0, proc=None): + if required_services != required: + raise AssertionError("unexpected required service set") + return True, "required services became ready after 1 check(s): " + ", ".join(required_services) + + def flaky_service_list(*, timeout_sec=2.0): + calls["ros_service_names"] += 1 + return ["/jarvis/rg2/open", "/jarvis/rg2/close"], "/jarvis/rg2/open\n/jarvis/rg2/close\n" + + panel.wait_for_required_services = wait_ready + panel.ros_service_names = flaky_service_list + + missing, output = panel.missing_required_services(shake, "dsr01") + if missing: + print("[FAIL] ready service wait was converted into a false missing list:", missing) + return 1 + if calls["ros_service_names"] != 0: + print("[FAIL] service list was called after a successful wait sample") + return 1 + if "required services became ready" not in output: + print("[FAIL] ready evidence was not preserved") + return 1 print("[PASS] panel trusts successful required-service wait sample") return 0 diff --git a/tools/perception/dispenser_color_scan.py b/tools/perception/dispenser_color_scan.py index dcbc379..2a39889 100644 --- a/tools/perception/dispenser_color_scan.py +++ b/tools/perception/dispenser_color_scan.py @@ -283,6 +283,22 @@ def main() -> int: else: color_map = scan_from_ros() + unknown_ids = [did for did, color in color_map.items() if str(color).lower() == "unknown"] + if unknown_ids: + out = Path(args.output) + failed_out = out.with_suffix(out.suffix + ".failed") + failed_out.parent.mkdir(parents=True, exist_ok=True) + failed_out.write_text(json.dumps(color_map, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") + if out.exists(): + out.unlink() + print( + "[dispenser_color_scan] ERROR: unknown color result for dispenser(s): " + + ", ".join(sorted(unknown_ids, key=lambda x: int(x) if str(x).isdigit() else str(x))), + file=sys.stderr, + ) + print(f"[dispenser_color_scan] failed result saved: {failed_out}", file=sys.stderr) + print(json.dumps(color_map, ensure_ascii=False)) + return 1 out = Path(args.output) out.parent.mkdir(parents=True, exist_ok=True) out.write_text(json.dumps(color_map, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") diff --git a/tools/run/dispenser_color_scan_ros.sh b/tools/run/dispenser_color_scan_ros.sh index df60033..33a3f8f 100755 --- a/tools/run/dispenser_color_scan_ros.sh +++ b/tools/run/dispenser_color_scan_ros.sh @@ -1,9 +1,24 @@ #!/usr/bin/env bash # 디스펜서 색상 스캔 (ROS 모드). -# 로봇이 color_scan_pose (joints [0,10,20,0,90,0]°)에 있어야 합니다. +# 로봇이 color_scan_pose (joints [0,10,32,0,100,90]°)에 있어야 합니다. # 카메라, TF, 로봇 드라이버가 실행 중이어야 합니다. set -euo pipefail ROOT="$(cd "$(dirname "$0")/../.." && pwd)" -source "$ROOT/install/local_setup.bash" 2>/dev/null || true + +source_setup() { + local setup_file="$1" + if [ ! -f "$setup_file" ]; then + return 0 + fi + # Colcon setup files may read optional environment variables while this + # wrapper runs with nounset enabled. + set +u + source "$setup_file" + set -u +} + +source_setup /opt/ros/humble/setup.bash +source_setup "$ROOT/install/local_setup.bash" + python3 "$ROOT/tools/perception/dispenser_color_scan.py" --ros \ --output "$ROOT/outputs/dispenser_color_map.json" diff --git a/tools/run/listen_stt_recipe.py b/tools/run/listen_stt_recipe.py index f93f99b..628266b 100644 --- a/tools/run/listen_stt_recipe.py +++ b/tools/run/listen_stt_recipe.py @@ -47,11 +47,16 @@ def on_msg(msg) -> None: print(f"[listen_stt_recipe] intent={intent} 무시 (make_cocktail 아님)") return - colors = [str(c).strip().lower() for c in data.get("dispenser_ids", []) if c] recipe_id = str(data.get("recipe_id", "custom")).strip() - # pump 수: LLM이 pump_counts 필드를 생성하면 사용, 없으면 1 - pumps_raw = data.get("pump_counts") or {} + # pump 수: dispenser_amounts(신규) 또는 pump_counts(구형) 중 있는 쪽 사용, 없으면 1 + pumps_raw = data.get("dispenser_amounts") or data.get("pump_counts") or {} + + # 색상 목록: dispenser_ids 우선, dispenser_amounts 키로 보완 + ids_from_field = [str(c).strip().lower() for c in data.get("dispenser_ids", []) if c] + ids_from_amounts = list(pumps_raw.keys()) if pumps_raw else [] + colors = ids_from_field or ids_from_amounts + pumps = {c: int(pumps_raw.get(c, 1)) for c in colors} received = {"colors": colors, "pumps": pumps, "recipe_id": recipe_id} @@ -62,7 +67,7 @@ def on_msg(msg) -> None: # azas_voice가 퍼블리시하는 토픽 - 메시지 타입은 std_msgs/String (JSON payload) from std_msgs.msg import String - node.create_subscription(String, "/azas/voice/recipe_decision", on_msg, qos_profile_sensor_data) + node.create_subscription(String, "/azas/voice/confirmed_recipe_decision", on_msg, qos_profile_sensor_data) print(f"[listen_stt_recipe] 레시피 대기 중... (최대 {args.timeout:.0f}초)") deadline = time.time() + args.timeout diff --git a/tools/run/open_robot_pipeline_control_panel.sh b/tools/run/open_robot_pipeline_control_panel.sh index 81c509d..0e1d426 100755 --- a/tools/run/open_robot_pipeline_control_panel.sh +++ b/tools/run/open_robot_pipeline_control_panel.sh @@ -12,22 +12,25 @@ COMMAND_DIR="${AZAS_PANEL_COMMAND_DIR:-$HOME/.local/bin}" COMMAND_PATH="$COMMAND_DIR/azas-panel" PANEL_ROS_DOMAIN_ID="${AZAS_PANEL_ROS_DOMAIN_ID:-9}" SERVER_SCRIPT="$ROOT/tools/run/robot_pipeline_control_server.py" -FORCE_RESTART=0 +RESTART_SERVER=1 case "${1:-}" in --restart|restart) - FORCE_RESTART=1 + RESTART_SERVER=1 + ;; + --reuse|reuse) + RESTART_SERVER=0 ;; -h|--help) cat <&2 - echo "Usage: azas-panel [--restart]" >&2 + echo "Usage: azas-panel [--restart|--reuse]" >&2 exit 2 ;; esac @@ -78,7 +81,7 @@ server_pid() { server_needs_restart() { local pid="$1" - [[ "$FORCE_RESTART" == "1" ]] && return 0 + [[ "$RESTART_SERVER" == "1" ]] && return 0 [[ -z "$pid" ]] && return 1 [[ ! -f "$SERVER_SCRIPT" ]] && return 1 local etimes now started script_mtime @@ -167,8 +170,8 @@ ensure_workspace_built PID="$(server_pid || true)" if [[ -n "$PID" ]] && server_needs_restart "$PID"; then - if [[ "$FORCE_RESTART" == "1" ]]; then - echo "[Azas] 요청에 따라 패널 서버를 재시작합니다." + if [[ "$RESTART_SERVER" == "1" ]]; then + echo "[Azas] 패널 서버를 새로 초기화합니다." else echo "[Azas] 패널 서버 코드 변경 감지: 새 코드로 자동 재시작합니다." fi diff --git a/tools/run/pick_from_measured_dispenser_front_hold.py b/tools/run/pick_from_measured_dispenser_front_hold.py index f4cdbf8..b5285b9 100755 --- a/tools/run/pick_from_measured_dispenser_front_hold.py +++ b/tools/run/pick_from_measured_dispenser_front_hold.py @@ -461,8 +461,8 @@ def main() -> int: if args.pregrasp_staging: print( "[Azas] Pre-grasp staging is enabled: first move to a measured-front_hold-derived " - "offset pose, then move above the final pose, then descend into final " - "front-hold slowly. This uses no " + "offset pose behind/above the cup, then move into final front-hold " + "slowly. This uses no " "operator/LLM-generated cup coordinates." ) rc = run_front_hold_move( @@ -477,18 +477,6 @@ def main() -> int: if rc != 0: print("[FAIL] pre-grasp staging approach failed; final cup approach skipped.") return rc - rc = run_front_hold_move( - args, - label="Pre-grasp above-cup alignment", - offset_x_m=0.0, - offset_y_m=0.0, - offset_z_m=args.pregrasp_offset_z_m, - velocity=args.pregrasp_staging_velocity, - acceleration=args.pregrasp_staging_acceleration, - ) - if rc != 0: - print("[FAIL] pre-grasp above-cup alignment failed; final cup approach skipped.") - return rc else: print("[Azas] Pre-grasp staging disabled; using direct final front-hold approach.") rc = run_joint1_clearance(args) diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 4604e19..619e33a 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -56,6 +56,7 @@ FAST_MOVE_ACCELERATION = "30" RVIZ_PREVIEW_ROS_DOMAIN_ID = "79" BACKGROUND_LOG_DIR = ROOT / "log" / "panel" +COMMAND_OVERRIDES_PATH = ROOT / "outputs" / "panel_command_overrides.json" ROBOT_STATE_NAMES = { 0: "STATE_INITIALIZING", 1: "STATE_STANDBY", @@ -72,11 +73,11 @@ } CAMERA_TABLE_VIEW_JOINTS = { "j1": "0", - "j2": "-5", - "j3": "50", + "j2": "10", + "j3": "32", "j4": "0", - "j5": "135", - "j6": "0", + "j5": "100", + "j6": "90", } _DISPENSER_PRESS_TARGETS_DEFAULT: dict[str, str] = { "1": "red", @@ -87,6 +88,41 @@ DISPENSER_COLOR_MAP_PATH = ROOT / "outputs" / "dispenser_color_map.json" +def load_command_overrides() -> dict[str, str]: + if not COMMAND_OVERRIDES_PATH.exists(): + return {} + try: + loaded = json.loads(COMMAND_OVERRIDES_PATH.read_text(encoding="utf-8")) + except Exception: + return {} + if not isinstance(loaded, dict): + return {} + step_keys = {step.key for step in STEPS} if "STEPS" in globals() else set() + return { + str(key): str(value) + for key, value in loaded.items() + if isinstance(value, str) and (not step_keys or str(key) in step_keys) + } + + +def save_command_override(step_key: str, command: str) -> dict[str, str]: + step_keys = {step.key for step in STEPS} + if step_key not in step_keys: + raise ValueError(f"unknown step key: {step_key}") + overrides = load_command_overrides() + command = command.strip() + if command: + overrides[step_key] = command + else: + overrides.pop(step_key, None) + COMMAND_OVERRIDES_PATH.parent.mkdir(parents=True, exist_ok=True) + COMMAND_OVERRIDES_PATH.write_text( + json.dumps(overrides, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + return overrides + + def _load_dispenser_press_targets() -> dict[str, str]: base = dict(_DISPENSER_PRESS_TARGETS_DEFAULT) if DISPENSER_COLOR_MAP_PATH.exists(): @@ -153,10 +189,10 @@ class Step: "connect_robot", "로봇 연결 / 스마트 재연결", "background", - "tools/run/run_doosan_real_no_motion_m0609.sh", + "tools/run/run_doosan_real_m0609.sh", True, False, - "준비됨/시작중이면 유지하고, stale 상태일 때만 정리 후 시작", + "실제 로봇 real-mode bringup. 준비됨/시작중이면 유지하고, stale 상태일 때만 정리 후 시작", ), Step("status_check", "연결 확인", "run", "ros2 service list | grep /dsr01/motion", True, False, "명령 후보만 있음: /dsr01/motion 서비스가 보여야 통과"), Step("connect_gripper", "그리퍼 연결", "background", "ros2 launch azas_gripper rg2_trigger.launch.py", True, False, "RG2 Trigger 서비스(/jarvis/rg2/open, close, set_width) 시작"), @@ -175,21 +211,21 @@ class Step: Step("home_robot", "로봇 원위치 / HOME", "run", "tools/run/direct_movej_joints.py --j1 0 --j2 0 --j3 90 --j4 0 --j5 90 --j6 0", True, True, "실제모션 후보: HOME 관절값 [0, 0, 90, 0, 90, 0]"), Step( "lift_robot", - "카메라 테이블 보기 자세 / J5 안전", + "기본 카메라 보기 자세", "run", - "tools/run/direct_movej_joints.py --j1 0 --j2 -5 --j3 50 --j4 0 --j5 135 --j6 0", + "tools/run/direct_movej_joints.py --j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90", True, True, - "MoveLine IK 대신 실측 관절 자세 사용: joint_2=-5°, joint_3=50°, joint_5=135° 상한으로 테이블 보기", + "기본 카메라 보기 관절 자세: [0, 10, 32, 0, 100, 90]°", ), Step( "move_to_color_scan_pose", - "색상 스캔 포즈 이동 [0,10,20,0,90,0]°", + "색상 스캔/카메라 보기 포즈 이동 [0,10,32,0,100,90]°", "run", - "tools/run/direct_movej_joints.py --j1 0 --j2 10 --j3 20 --j4 0 --j5 90 --j6 0 --velocity 30 --acceleration 30 --execute --confirm ENABLE_DIRECT_MOVEJ", + "tools/run/direct_movej_joints.py --j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90 --velocity 30 --acceleration 30 --execute --confirm ENABLE_DIRECT_MOVEJ", True, True, - "색상 스캔 전 카메라가 디스펜서를 향하는 포즈로 이동. color_scan_pose: [0,10,20,0,90,0]°", + "색상 스캔 전 기본 카메라 보기 포즈로 이동. color_scan_pose: [0,10,32,0,100,90]°", ), Step( "color_scan", @@ -198,7 +234,7 @@ class Step: "tools/run/dispenser_color_scan_ros.sh", True, False, - "카메라+TF로 디스펜서 1~4 색상을 판별해 outputs/dispenser_color_map.json 저장. 로봇이 color_scan_pose [0,10,20,0,90,0]°에 있어야 함", + "카메라+TF로 디스펜서 1~4 색상을 판별해 outputs/dispenser_color_map.json 저장. 로봇이 color_scan_pose [0,10,32,0,100,90]°에 있어야 함", ), Step("voice_input", "음성 입력 (STT+LLM 노드 시작)", "background", "ros2 launch azas_voice azas_voice.launch.py", True, False, "STT → /stt_result → llm_recipe_mapper → /azas/voice/recipe_decision"), Step( @@ -212,12 +248,12 @@ class Step: ), Step( "run_color_recipe_sequence", - "색상 레시피 디스펜서 시퀀스 실행", + "통합 디스펜서 레시피 실행", "run", - "tools/run/run_color_recipe_sequence.py", + "tools/run/run_color_recipe_sequence.py --execute --confirm", True, True, - "latest_recipe.json + dispenser_color_map.json → 색깔→디스펜서ID 매핑 → 순서대로 move+press 실행", + "latest_recipe.json + dispenser_color_map.json → 컵 놓기→dispenser_amounts 횟수 프레스→컵 다시 잡기/다음 디스펜서 이동을 한 단계로 실행", ), Step( "side_grip", @@ -272,7 +308,7 @@ class Step: "ros2 run azas_dispenser dispenser_press_node --ros-args -p use_taught_posx:=false", True, True, - "calibration.yaml dispenser_outlets.1 press_pose 측정값 사용: 컵 놓기 후 후퇴→HOME→RG2 full-close→measured press pose→HOME", + "calibration.yaml dispenser_outlets.1 press_pose 측정값 사용: 현재 위치 수직상승→프레스 위치→하강 누름→상승→후퇴 대기", ), Step( "press_dispenser_2", @@ -281,7 +317,7 @@ class Step: "ros2 run azas_dispenser dispenser_press_node --ros-args -p use_taught_posx:=false", True, True, - "calibration.yaml dispenser_outlets.2 press_pose 측정값 사용: 컵 놓기 후 후퇴→HOME→RG2 full-close→measured press pose→HOME", + "calibration.yaml dispenser_outlets.2 press_pose 측정값 사용: 현재 위치 수직상승→프레스 위치→하강 누름→상승→후퇴 대기", ), Step( "press_dispenser_3", @@ -290,7 +326,7 @@ class Step: "ros2 run azas_dispenser dispenser_press_node --ros-args -p use_taught_posx:=false", True, True, - "calibration.yaml dispenser_outlets.3 press_pose 측정값 사용: 컵 놓기 후 후퇴→HOME→RG2 full-close→measured press pose→HOME", + "calibration.yaml dispenser_outlets.3 press_pose 측정값 사용: 현재 위치 수직상승→프레스 위치→하강 누름→상승→후퇴 대기", ), Step( "press_dispenser_4", @@ -299,7 +335,7 @@ class Step: "ros2 run azas_dispenser dispenser_press_node --ros-args -p use_taught_posx:=false", True, True, - "calibration.yaml dispenser_outlets.4 press_pose 측정값 사용: 컵 놓기 후 후퇴→HOME→RG2 full-close→measured press pose→HOME", + "calibration.yaml dispenser_outlets.4 press_pose 측정값 사용: 현재 위치 수직상승→프레스 위치→하강 누름→상승→후퇴 대기", ), Step( "pick_from_dispenser_1", @@ -362,6 +398,7 @@ class Step: process_logs: dict[str, Path] = {} DOOSAN_STACK_PATTERNS = ( + "run_doosan_real_m0609.sh", "run_doosan_real_no_motion_m0609.sh", "run_emulator", "dsr_bringup2/lib/dsr_bringup2", @@ -690,6 +727,7 @@ def find_existing_doosan_launch() -> tuple[int | None, str]: return None, "" current_pid = os.getpid() launch_markers = ( + "run_doosan_real_m0609.sh", "run_doosan_real_no_motion_m0609.sh", "dsr_bringup2_moveit.launch.py", ) @@ -954,7 +992,7 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: f"/{clean}/motion/check_motion", f"/{clean}/system/get_robot_state", ] - if step.key in {"home_robot", "lift_robot"}: + if step.key in {"home_robot", "lift_robot", "move_to_color_scan_pose"}: return [ f"/{clean}/motion/move_joint", f"/{clean}/motion/check_motion", @@ -970,6 +1008,20 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: f"/{clean}/aux_control/get_current_posx", "/jarvis/rg2/set_width", ] + if step.key == "run_color_recipe_sequence": + return [ + "/jarvis/rg2/set_width", + f"/{clean}/motion/move_line", + f"/{clean}/motion/move_joint", + f"/{clean}/motion/move_wait", + f"/{clean}/motion/fkin", + f"/{clean}/motion/ikin", + f"/{clean}/motion/check_motion", + f"/{clean}/system/get_robot_state", + f"/{clean}/tcp/get_current_tcp", + f"/{clean}/aux_control/get_current_posj", + f"/{clean}/aux_control/get_current_posx", + ] if step.key.startswith("press_dispenser_"): return [ "/jarvis/rg2/set_width", @@ -1033,10 +1085,11 @@ def required_service_wait_timeout(step: Step) -> float: step.key.startswith("move_to_dispenser_") or step.key.startswith("press_dispenser_") or step.key.startswith("pick_from_dispenser_") + or step.key == "run_color_recipe_sequence" or step.key == "place_cup_holder" ): return 35.0 - if step.key in {"home_robot", "lift_robot", "side_grip", "shake_closed_cup"}: + if step.key in {"home_robot", "lift_robot", "move_to_color_scan_pose", "side_grip", "shake_closed_cup"}: return 30.0 if step.key == "gripper_soft_grasp": return 12.0 @@ -1326,8 +1379,36 @@ def side_grip_preflight(env: dict[str, str], service_prefix: str) -> tuple[bool, + ", ".join(str(path) for path in calibration_candidates) ) - camera_ready, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=8.0) - checks.append("--- camera topics ---\n" + camera_output) + camera_ready, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=5.0) + if not camera_ready: + # 카메라가 depth 없이 켜져 있을 수 있음 → 자동 재시작 + checks.append("[AUTO] 카메라 토픽 불완전 — depth 포함 자동 재시작 중...") + cleanup_camera_stack() + time.sleep(1.5) + camera_cmd = ( + f"cd {ROOT} && {ROS_SETUP} && " + "ros2 launch realsense2_camera rs_launch.py " + "camera_name:=camera " + "enable_color:=true enable_depth:=true align_depth.enable:=true" + ) + log_path = background_log_path("start_camera") + log_handle = log_path.open("w", encoding="utf-8", buffering=1) + camera_proc = subprocess.Popen( + ["bash", "-lc", camera_cmd], + cwd=str(ROOT), + env=env, + stdout=log_handle, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + log_handle.close() + processes["start_camera"] = camera_proc + process_logs["start_camera"] = log_path + camera_ready, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=20.0, proc=camera_proc) + checks.append("[AUTO] 카메라 재시작 후 토픽 확인:\n" + camera_output) + else: + checks.append("--- camera topics ---\n" + camera_output) if not camera_ready: ok = False @@ -1570,13 +1651,36 @@ def with_collision_scene_prereq(selected: list[str]) -> list[str]: for prereq in reversed(prerequisites): if prereq not in ordered: ordered.insert(side_index, prereq) + + if "color_scan" in ordered: + color_index = ordered.index("color_scan") + prerequisites = ["move_to_color_scan_pose", "start_camera"] + for prereq in reversed(prerequisites): + if prereq not in ordered: + ordered.insert(color_index, prereq) + + if "run_color_recipe_sequence" in ordered and "connect_gripper" not in ordered: + run_index = ordered.index("run_color_recipe_sequence") + ordered.insert(run_index, "connect_gripper") + if any(requires_collision_scene_step(key) for key in ordered): - ordered = ["start_collision_scene"] + [key for key in ordered if key != "start_collision_scene"] - return ordered + ordered = [key for key in ordered if key != "start_collision_scene"] + first_collision_index = next( + ( + index + for index, key in enumerate(ordered) + if requires_collision_scene_step(key) + ), + 0, + ) + ordered.insert(first_collision_index, "start_collision_scene") + return list(dict.fromkeys(ordered)) def run_timeout_for_step(step: Step) -> float: if step.key == "side_grip": return 900.0 + if step.key == "run_color_recipe_sequence": + return 1200.0 if step.key == "place_cup_holder": return 240.0 return 180.0 @@ -1599,7 +1703,7 @@ def shell_env(payload: dict[str, Any]) -> dict[str, str]: payload.get("selected_dispenser_id") or env.get("SELECTED_DISPENSER_ID") or "2" ) env["RECIPE_DISPENSER_IDS"] = str( - payload.get("recipe_dispenser_ids") or env.get("RECIPE_DISPENSER_IDS") or "1,2,3,4" + payload.get("recipe_dispenser_ids") or env.get("RECIPE_DISPENSER_IDS") or "" ) env["CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M"] = str( payload.get("cup_holder_place_final_z_offset_m") @@ -1621,11 +1725,15 @@ def shell_env(payload: dict[str, Any]) -> dict[str, str]: or infer_rt_host(env["ROBOT_HOST"]) or "192.168.137.50" ) - env["DOOSAN_NO_MOTION_CONFIRM"] = "CONNECT_DOOSAN_NO_MOTION" + env["DOOSAN_REAL_MOTION_CONFIRM"] = "ENABLE_DOOSAN_REAL_MOTION_BRINGUP" return env def command_for(step: Step, payload: dict[str, Any]) -> str: + command_override = load_command_overrides().get(step.key) + if command_override: + return command_override + service_prefix = str(payload.get("service_prefix") or "dsr01") def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispenser_id: str = "1") -> str: @@ -1649,7 +1757,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe return ( f"cd {ROOT} && ROBOT_HOST={shlex.quote(robot_host)} " f"ROBOT_NAME={shlex.quote(robot_name)} RT_HOST={shlex.quote(rt_host)} " - "DOOSAN_NO_MOTION_CONFIRM=CONNECT_DOOSAN_NO_MOTION " + "DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP " f"{step.command}" ) if step.key == "status_check": @@ -1669,6 +1777,16 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "echo '--- trajectory action ---' && " f"ros2 action info /{clean}/dsr_moveit_controller/follow_joint_trajectory" ) + if step.key == "run_color_recipe_sequence": + recipe_dispenser_ids = str(payload.get("recipe_dispenser_ids") or "").strip() + direct_ids_arg = "" + if recipe_dispenser_ids: + direct_ids_arg = f" --dispenser-ids {shlex.quote(recipe_dispenser_ids)}" + return ( + f"cd {ROOT} && {ROS_SETUP} && " + "python3 tools/run/run_color_recipe_sequence.py --execute --confirm" + f"{direct_ids_arg}" + ) if step.key == "lift_robot": joints = { name: str(os.environ.get(f"CAMERA_TABLE_VIEW_{name.upper()}", value)) @@ -1691,6 +1809,14 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"--j4 0 --j5 90 --j6 0 --velocity {FAST_MOVE_VELOCITY} --acceleration {FAST_MOVE_ACCELERATION} " "--execute --confirm ENABLE_DIRECT_MOVEJ" ) + if step.key == "move_to_color_scan_pose": + return ( + f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/direct_movej_joints.py " + f"--service-prefix {service_prefix} " + "--j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90 " + "--velocity 30 --acceleration 30 --timeout-sec 60 " + "--execute --confirm ENABLE_DIRECT_MOVEJ" + ) if step.key == "connect_gripper": rg2_ip = str(payload.get("rg2_ip") or os.environ.get("RG2_IP") or "192.168.1.1") gripper_pkg_bash = ROOT / "install" / "azas_gripper" / "share" / "azas_gripper" / "package.bash" @@ -1738,6 +1864,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "colcon build --symlink-install --packages-select dsr_practice; " "fi && " f"source {shlex.quote(str(ROOT / 'install' / 'local_setup.bash'))} && " + f"source {shlex.quote(str(ROOT / 'install' / 'dsr_practice' / 'share' / 'dsr_practice' / 'package.bash'))} && " f"export PYTHONPATH={shlex.quote(str(ROOT / 'tools' / 'run' / 'python_compat'))}:${{PYTHONPATH:-}} && " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " @@ -1871,12 +1998,15 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"-p rx:={press_rpy_deg[0]:.6f} " f"-p ry:={press_rpy_deg[1]:.6f} " f"-p rz:={press_rpy_deg[2]:.6f} " + "-p press_count:=1 " + # calibration.yaml press_pose_xyz_m is the taught final press pose. + # Do not subtract an extra legacy pump depth here. "-p press_depth:=0.0 " f"-p tcp_name:={shlex.quote(tcp_name)} " "-p require_tcp_for_taught_posx:=false " "-p allow_tcp_set_failure:=false " - "-p move_home_first:=true " - "-p pre_home_retreat_before_home:=true " + "-p move_home_first:=false " + "-p pre_home_retreat_before_home:=false " "-p pre_home_retreat_dx_mm:=-180.0 " "-p pre_home_retreat_dy_mm:=0.0 " "-p pre_home_retreat_min_z_mm:=520.0 -p pre_home_retreat_lift_first:=true " @@ -1886,8 +2016,12 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "-p joint1_clearance_before_home:=false " "-p joint1_clearance_return_home:=false " "-p joint1_clearance_offset_deg:=12.0 " - "-p return_home:=true " - "-p close_gripper_at_home:=true " + "-p return_home:=false " + "-p close_gripper_at_home:=false " + "-p post_press_retreat_after_sequence:=true " + "-p post_press_retreat_dx_mm:=-120.0 " + "-p post_press_retreat_dy_mm:=0.0 " + "-p post_press_retreat_wait_seconds:=1.0 " "-p gripper_service:=/jarvis/rg2/set_width " "-p gripper_close_width:=0.0 " "-p gripper_close_force:=30.0 " @@ -1993,6 +2127,8 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true " "tools/run/run_rule_based_shake_real.sh" ) + if step.command.strip(): + return f"cd {ROOT} && {ROS_SETUP} && {step.command}" return "" @@ -2356,8 +2492,24 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: for did in sorted(color_map.keys(), key=lambda x: int(x) if x.isdigit() else x): lines.append(f" 디스펜서 {did}: {color_map[did]}") output = f"{output}\n" + "\n".join(lines) + "\n" + if not color_map: + return { + "key": step.key, + "status": "failed", + "returncode": 1, + "output": output + "[color_scan] 결과가 비어 있습니다.\n", + } + unknown = [str(did) for did, color in color_map.items() if str(color).lower() == "unknown"] + if unknown: + output += "[color_scan] WARNING: unknown result for dispenser(s): " + ", ".join(sorted(unknown)) + "\n" except Exception as exc: output = f"{output}\n[color_scan] 결과 파일 읽기 실패: {exc}\n" + return { + "key": step.key, + "status": "failed", + "returncode": 1, + "output": output, + } target_xyz = target_xyz_for_step(step.key) if target_xyz is not None: reached, verify_output = wait_for_xyz_target(env["SERVICE_PREFIX"], target_xyz) @@ -2430,6 +2582,7 @@ def do_GET(self) -> None: self.wfile.write(body) return if path == "/api/steps": + command_overrides = load_command_overrides() preview_payload = { "robot_host": os.environ.get("ROBOT_HOST", DEFAULT_ROBOT_HOST), "robot_name": os.environ.get("ROBOT_NAME", "dsr01"), @@ -2444,6 +2597,7 @@ def do_GET(self) -> None: for step in STEPS: item = asdict(step) item["resolved_command"] = command_for(step, preview_payload) if step.implemented else "" + item["command_saved"] = step.key in command_overrides data.append(item) self.send_json(data) return @@ -2475,6 +2629,7 @@ def do_POST(self) -> None: path = urlparse(self.path).path if path == "/api/run": selected = with_collision_scene_prereq([str(key) for key in payload.get("selected") or []]) + selected = list(dict.fromkeys(selected)) steps_by_key = {step.key: step for step in STEPS} results = [ run_step(steps_by_key[key], payload) @@ -2498,6 +2653,16 @@ def do_POST(self) -> None: ) self.send_json({"map": DISPENSER_PRESS_TARGETS}) return + if path == "/api/command_override": + step_key = str(payload.get("key") or "") + command = str(payload.get("command") or "") + try: + overrides = save_command_override(step_key, command) + except ValueError as exc: + self.send_json({"error": str(exc)}, 400) + return + self.send_json({"overrides": overrides}) + return if path == "/api/stop": self.send_json(stop_all()) return diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 45f00de..9638bb4 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -7,6 +7,7 @@ 사용법: python3 tools/run/run_color_recipe_sequence.py python3 tools/run/run_color_recipe_sequence.py --colors red:2,blue:1 # 직접 지정 + python3 tools/run/run_color_recipe_sequence.py --dispenser-ids 1x1,2x2,3x1 """ from __future__ import annotations @@ -27,7 +28,11 @@ def load_color_map() -> dict[str, str]: """dispenser_id → color_name 매핑 로드.""" if not COLOR_MAP_PATH.exists(): print(f"[run_color_recipe] 색상 맵 없음: {COLOR_MAP_PATH}", file=sys.stderr) - print("[run_color_recipe] color_scan 스텝을 먼저 실행하세요.", file=sys.stderr) + print( + "[run_color_recipe] color_scan 스텝을 먼저 실행하거나, " + "패널 DIRECT DISPENSER INPUT에 1x1,2x2,3x1처럼 물리 디스펜서 번호와 횟수를 입력하세요.", + file=sys.stderr, + ) sys.exit(1) data = json.loads(COLOR_MAP_PATH.read_text(encoding="utf-8")) return {str(k): str(v).lower().strip() for k, v in data.items()} @@ -57,13 +62,75 @@ def parse_colors_arg(raw: str) -> list[tuple[str, int]]: return result +def parse_direct_dispenser_sequence(raw: str) -> list[str]: + """Parse physical dispenser input. + + Accepted forms: + 1,2,2,3 + 1x1,2x2,3x1 + 1:1,2:2,3:1 + """ + result: list[str] = [] + for part in raw.replace(";", ",").split(","): + item = part.strip().lower() + if not item: + continue + if "x" in item: + dispenser_id, count_raw = item.split("x", 1) + elif ":" in item: + dispenser_id, count_raw = item.split(":", 1) + else: + dispenser_id, count_raw = item, "1" + dispenser_id = dispenser_id.strip() + if dispenser_id not in {"1", "2", "3", "4"}: + raise ValueError(f"unsupported dispenser id: {dispenser_id!r}") + try: + count = int(count_raw.strip()) + except ValueError as exc: + raise ValueError(f"invalid count for dispenser {dispenser_id}: {count_raw!r}") from exc + if count < 1: + raise ValueError(f"count must be >= 1 for dispenser {dispenser_id}") + result.extend([dispenser_id] * count) + if not result: + raise ValueError("direct dispenser input is empty") + return result + + def main() -> int: parser = argparse.ArgumentParser() parser.add_argument("--colors", default="", help="직접 색깔 지정: 'red:2,blue:1' (생략 시 latest_recipe.json 사용)") + parser.add_argument("--dispenser-ids", default="", + help="직접 물리 디스펜서 지정: '1,2,2,3' 또는 '1x1,2x2,3x1'") parser.add_argument("--confirm", action="store_true", help=f"확인 구문({CONFIRM_PHRASE}) 자동 전달") + parser.add_argument("--execute", action="store_true", + help="실제 measured dispenser sequence를 실행") args = parser.parse_args() + if args.execute and not args.confirm: + print(f"[BLOCKED] --execute requires --confirm ({CONFIRM_PHRASE})", file=sys.stderr) + return 2 + + direct_dispenser_ids = args.dispenser_ids.strip() + if direct_dispenser_ids: + try: + sequence = parse_direct_dispenser_sequence(direct_dispenser_ids) + except ValueError as exc: + print(f"[run_color_recipe] 잘못된 직접 입력: {exc}", file=sys.stderr) + return 1 + dispenser_ids_str = ",".join(sequence) + print(f"[run_color_recipe] 직접 디스펜서 실행 순서: {dispenser_ids_str}") + cmd = [ + sys.executable, str(SEQUENCE_SCRIPT), + "--dispenser-ids", dispenser_ids_str, + ] + if args.execute: + cmd += ["--execute"] + if args.confirm: + cmd += ["--confirm", CONFIRM_PHRASE] + print(f"[run_color_recipe] 실행: {' '.join(cmd)}") + result = subprocess.run(cmd, check=False) + return result.returncode color_map = load_color_map() print(f"[run_color_recipe] 색상 맵: {color_map}") @@ -104,6 +171,8 @@ def main() -> int: sys.executable, str(SEQUENCE_SCRIPT), "--dispenser-ids", dispenser_ids_str, ] + if args.execute: + cmd += ["--execute"] if args.confirm: cmd += ["--confirm", CONFIRM_PHRASE] diff --git a/tools/run/run_course_dispenser_press_cycle_rviz.sh b/tools/run/run_course_dispenser_press_cycle_rviz.sh new file mode 100755 index 0000000..91d6044 --- /dev/null +++ b/tools/run/run_course_dispenser_press_cycle_rviz.sh @@ -0,0 +1,186 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Course-material execution path for the requested dispenser cycle: +# 1) Doosan MoveIt bringup as in 25장 (virtual now, real later by MODE/HOST) +# 2) Azas MoveItPy node follows 26~28장: plan() -> robot.execute(blocking=True) +# 3) RViz robot motion is controller-backed /joint_states. No fake joint publisher. +# 4) Default RVIZ_MODE=bringup keeps the course/MoveIt RViz, including the orange planned/goal robot display. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" +MODE="${MODE:-virtual}" +HOST="${HOST:-127.0.0.1}" +PORT="${PORT:-12347}" +MODEL="${MODEL:-m0609}" +COLOR="${COLOR:-white}" +RT_HOST="${RT_HOST:-192.168.137.50}" +START_DELAY_SEC="${START_DELAY_SEC:-22}" +JOINT_WAIT_SEC="${JOINT_WAIT_SEC:-60}" +DISPENSER_ID="${DISPENSER_ID:-1}" +PRESS_COUNT="${PRESS_COUNT:-2}" +RVIZ_MODE="${RVIZ_MODE:-bringup}" # bringup|clean|none +RVIZ_CONFIG="${RVIZ_CONFIG:-${ROOT_DIR}/src/azas_bringup/rviz/m0609_robot_only.rviz}" +DISPENSER_COLLISION_ENABLED="${DISPENSER_COLLISION_ENABLED:-1}" +# The measured combined box is the glass-bottle/body area, not the press button/head. +# Keep markers visible in RViz by default, but do not feed this draft body box into +# MoveIt collision checking for the press stroke unless explicitly requested. +DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS:-1}" +DISPENSER_COLLISION_CONFIG="${DISPENSER_COLLISION_CONFIG:-${ROOT_DIR}/install/azas_bringup/share/azas_bringup/config/measured_dispenser_collision.yaml}" +if [[ ! -f "${DISPENSER_COLLISION_CONFIG}" ]]; then + DISPENSER_COLLISION_CONFIG="${ROOT_DIR}/src/azas_bringup/config/measured_dispenser_collision.yaml" +fi +mkdir -p "${LOG_DIR}" + +cleanup() { + for pid in "${PIDS[@]:-}"; do + if [[ -n "${pid}" ]] && kill -0 "${pid}" 2>/dev/null; then + kill "${pid}" 2>/dev/null || true + wait "${pid}" 2>/dev/null || true + fi + done +} +trap cleanup EXIT +PIDS=() + +set +u +source /opt/ros/humble/setup.bash +source /home/ssu/ws_moveit/install/setup.bash +source /home/ssu/ros2_ws/install/setup.bash +if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then + source "${ROOT_DIR}/install/setup.bash" +fi +set -u + + +STRICT_SINGLE_SESSION="${STRICT_SINGLE_SESSION:-1}" +if [[ "${STRICT_SINGLE_SESSION}" == "1" ]]; then + existing="$(pgrep -af 'dsr_bringup2_moveit|move_group|ros2_control_node|run_emulator' | grep -v "$$" || true)" + if [[ -n "${existing}" ]]; then + echo '[Azas] Refusing: an existing Doosan/MoveIt session is running. Stop it first to avoid RViz state jumping.' >&2 + echo "${existing}" >&2 + exit 2 + fi +fi + +if pgrep -af 'm0609_shake_joint_state_node|side_grasp_ik_preview_node' >/dev/null; then + echo '[Azas] Refusing: fake RViz joint publisher is still running.' >&2 + pgrep -af 'm0609_shake_joint_state_node|side_grasp_ik_preview_node' >&2 || true + exit 1 +fi + +before_rviz="$(pgrep -x rviz2 || true)" + +ros2 launch dsr_bringup2 dsr_bringup2_moveit.launch.py \ + mode:="${MODE}" \ + model:="${MODEL}" \ + host:="${HOST}" \ + port:="${PORT}" \ + color:="${COLOR}" \ + rt_host:="${RT_HOST}" \ + >"${LOG_DIR}/course_dispenser_bringup.log" 2>&1 & +PIDS+=("$!") + +sleep "${START_DELAY_SEC}" + +joint_deadline=$((SECONDS + JOINT_WAIT_SEC)) +while (( SECONDS < joint_deadline )); do + if timeout 3 ros2 topic echo /joint_states --once >"${LOG_DIR}/course_dispenser_joint_state_once.txt" 2>/dev/null; then + if grep -q '^header:' "${LOG_DIR}/course_dispenser_joint_state_once.txt"; then + break + fi + fi + sleep 1 +done +if ! grep -q '^header:' "${LOG_DIR}/course_dispenser_joint_state_once.txt" 2>/dev/null; then + echo '[Azas] No fresh /joint_states. MoveItPy cannot mirror the robot in RViz.' >&2 + tail -100 "${LOG_DIR}/course_dispenser_bringup.log" >&2 || true + exit 1 +fi + +if [[ "${DISPENSER_COLLISION_ENABLED}" == "1" || "${DISPENSER_COLLISION_ENABLED}" == "true" ]]; then + if [[ "${DISPENSER_COLLISION_OBJECTS}" == "1" || "${DISPENSER_COLLISION_OBJECTS}" == "true" ]]; then + DISPENSER_COLLISION_OBJECTS_BOOL=true + else + DISPENSER_COLLISION_OBJECTS_BOOL=false + fi + ros2 run azas_motion measured_dispenser_collision_scene_node \ + --ros-args \ + -p config_path:="${DISPENSER_COLLISION_CONFIG}" \ + -p publish_period_sec:=1.0 \ + -p publish_collision_objects:="${DISPENSER_COLLISION_OBJECTS_BOOL}" \ + -p publish_markers:=true \ + >"${LOG_DIR}/measured_dispenser_collision_scene.log" 2>&1 & + PIDS+=("$!") + echo "[Azas] Dispenser combined box represents bottle/body only; press pre/contact is derived from press_contact_joints_deg FK, not from this box." + echo "[Azas] DISPENSER_COLLISION_OBJECTS=${DISPENSER_COLLISION_OBJECTS} (1=add to MoveIt collision scene, 0=RViz markers only)." + sleep 2 + if [[ "${DISPENSER_COLLISION_OBJECTS}" == "1" || "${DISPENSER_COLLISION_OBJECTS}" == "true" ]]; then + timeout 8 ros2 topic echo /collision_object >"${LOG_DIR}/collision_object_samples.txt" 2>/dev/null || true + if grep -q 'dispenser_combined_body_box' "${LOG_DIR}/collision_object_samples.txt"; then + echo '[Azas] Published measured dispenser collision object: dispenser_combined_body_box' + elif grep -q 'Publishing measured dispenser collision objects: .*dispenser_combined_body_box' "${LOG_DIR}/measured_dispenser_collision_scene.log"; then + echo '[Azas] Collision node is publishing dispenser_combined_body_box; RViz should show PlanningScene/marker display when enabled.' + else + echo '[Azas] Warning: dispenser_combined_body_box was not observed in collision samples/log.' >&2 + tail -80 "${LOG_DIR}/measured_dispenser_collision_scene.log" >&2 || true + tail -120 "${LOG_DIR}/collision_object_samples.txt" >&2 || true + fi + else + timeout 8 ros2 topic echo /azas/measured_dispenser_collision/markers >"${LOG_DIR}/measured_dispenser_collision_markers.txt" 2>/dev/null || true + if grep -q 'dispenser_combined_body_box' "${LOG_DIR}/measured_dispenser_collision_markers.txt"; then + echo '[Azas] Published RViz marker for measured dispenser body box: dispenser_combined_body_box' + else + echo '[Azas] Collision markers enabled; marker sample did not capture label yet.' >&2 + tail -80 "${LOG_DIR}/measured_dispenser_collision_scene.log" >&2 || true + fi + fi +fi + +if [[ "${RVIZ_MODE}" == "clean" ]]; then + # dsr_bringup2_moveit launches its default RViz unconditionally. Replace only + # the RViz processes that appeared after this script started, preserving any + # pre-existing RViz windows. + before_lines="$(printf '%s\n' ${before_rviz:-})" + for pid in $(pgrep -x rviz2 || true); do + if ! grep -qx "${pid}" <<<"${before_lines}"; then + kill "${pid}" 2>/dev/null || true + wait "${pid}" 2>/dev/null || true + fi + done + rviz2 -d "${RVIZ_CONFIG}" >"${LOG_DIR}/course_dispenser_clean_rviz.log" 2>&1 & + PIDS+=("$!") +elif [[ "${RVIZ_MODE}" == "none" ]]; then + before_lines="$(printf '%s\n' ${before_rviz:-})" + for pid in $(pgrep -x rviz2 || true); do + if ! grep -qx "${pid}" <<<"${before_lines}"; then + kill "${pid}" 2>/dev/null || true + wait "${pid}" 2>/dev/null || true + fi + done +fi + +ros2 launch azas_bringup dispenser_press_cycle_moveit.launch.py \ + dispenser_id:="${DISPENSER_ID}" \ + press_count:="${PRESS_COUNT}" \ + trajectory_time_scale:="${TRAJECTORY_TIME_SCALE:-8.0}" \ + press_up_m:="${PRESS_UP_M:-0.02}" \ + cup_pre_grasp_backoff_m:="${CUP_PRE_GRASP_BACKOFF_M:-0.08}" \ + cup_release_retract_m:="${CUP_RELEASE_RETRACT_M:-0.05}" \ + planning_time_sec:="${PLANNING_TIME_SEC:-5.0}" \ + >"${LOG_DIR}/course_dispenser_cycle.log" 2>&1 + +if grep -qE 'process has died|FAILED:|ABORT|GOAL_TOLERANCE_VIOLATED|No motion plan found' "${LOG_DIR}/course_dispenser_cycle.log"; then + echo '[Azas] Dispenser cycle failed. See log:' >&2 + tail -120 "${LOG_DIR}/course_dispenser_cycle.log" >&2 || true + exit 3 +fi +if ! grep -q 'DONE:' "${LOG_DIR}/course_dispenser_cycle.log"; then + echo '[Azas] Dispenser cycle did not report DONE. See log:' >&2 + tail -120 "${LOG_DIR}/course_dispenser_cycle.log" >&2 || true + exit 4 +fi + +echo '[Azas] Dispenser press cycle finished: MoveItPy plan -> robot.execute -> controller /joint_states -> RViz RobotModel.' +echo "[Azas] Logs: ${LOG_DIR}/course_dispenser_bringup.log ${LOG_DIR}/course_dispenser_cycle.log ${LOG_DIR}/measured_dispenser_collision_scene.log ${LOG_DIR}/collision_object_samples.txt ${LOG_DIR}/measured_dispenser_collision_markers.txt" +wait diff --git a/tools/run/run_course_moveit_mp_basic_rviz.sh b/tools/run/run_course_moveit_mp_basic_rviz.sh new file mode 100755 index 0000000..40f3322 --- /dev/null +++ b/tools/run/run_course_moveit_mp_basic_rviz.sh @@ -0,0 +1,82 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Course-material execution path (12~13차시): +# 1) Doosan MoveIt bringup exactly like 25장, no namespace by default +# 2) dsr_practice/mp_basic.launch.py exactly like 26장 +# 3) RViz robot motion comes from MoveItPy robot.execute() -> controller -> /joint_states +# No custom /joint_states publisher. No /display_planned_path usage. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" +MODE="${MODE:-virtual}" +HOST="${HOST:-127.0.0.1}" +PORT="${PORT:-12345}" +MODEL="${MODEL:-m0609}" +COLOR="${COLOR:-white}" +RT_HOST="${RT_HOST:-192.168.137.50}" +START_DELAY_SEC="${START_DELAY_SEC:-18}" +JOINT_WAIT_SEC="${JOINT_WAIT_SEC:-45}" +mkdir -p "${LOG_DIR}" + +cleanup() { + for pid in "${PIDS[@]:-}"; do + if [[ -n "${pid}" ]] && kill -0 "${pid}" 2>/dev/null; then + kill "${pid}" 2>/dev/null || true + wait "${pid}" 2>/dev/null || true + fi + done +} +trap cleanup EXIT +PIDS=() + +set +u +source /opt/ros/humble/setup.bash +source /home/ssu/ws_moveit/install/setup.bash +source /home/ssu/ros2_ws/install/setup.bash +if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then + source "${ROOT_DIR}/install/setup.bash" +fi +set -u + +# Refuse if fake visual joint publishers are still present. The course path must +# be controller-backed /joint_states, not a hand-written animation node. +if pgrep -af 'm0609_shake_joint_state_node|side_grasp_ik_preview_node' >/dev/null; then + echo '[Azas] Refusing: fake RViz joint publisher is still running.' >&2 + pgrep -af 'm0609_shake_joint_state_node|side_grasp_ik_preview_node' >&2 || true + exit 1 +fi + +ros2 launch dsr_bringup2 dsr_bringup2_moveit.launch.py \ + mode:="${MODE}" \ + model:="${MODEL}" \ + host:="${HOST}" \ + port:="${PORT}" \ + color:="${COLOR}" \ + rt_host:="${RT_HOST}" \ + >"${LOG_DIR}/course_moveit_bringup.log" 2>&1 & +PIDS+=("$!") + +sleep "${START_DELAY_SEC}" + +joint_deadline=$((SECONDS + JOINT_WAIT_SEC)) +while (( SECONDS < joint_deadline )); do + if timeout 3 ros2 topic echo /joint_states --once >/tmp/azas_course_joint_state.txt 2>/dev/null; then + if grep -q '^header:' /tmp/azas_course_joint_state.txt; then + break + fi + fi + sleep 1 +done +if ! grep -q '^header:' /tmp/azas_course_joint_state.txt 2>/dev/null; then + echo '[Azas] No fresh /joint_states. Course MoveItPy cannot run.' >&2 + tail -80 "${LOG_DIR}/course_moveit_bringup.log" >&2 || true + exit 1 +fi + +ros2 launch dsr_practice mp_basic.launch.py \ + >"${LOG_DIR}/course_mp_basic.log" 2>&1 + +echo '[Azas] Course mp_basic finished: MoveItPy plan -> robot.execute -> /joint_states -> RViz robot motion.' +echo "[Azas] Logs: ${LOG_DIR}/course_moveit_bringup.log ${LOG_DIR}/course_mp_basic.log" +wait diff --git a/tools/run/run_dispenser_then_shake_real_mirror_rviz.sh b/tools/run/run_dispenser_then_shake_real_mirror_rviz.sh new file mode 100755 index 0000000..9964baf --- /dev/null +++ b/tools/run/run_dispenser_then_shake_real_mirror_rviz.sh @@ -0,0 +1,233 @@ +#!/usr/bin/env bash +set -euo pipefail + +# RViz preview that mirrors the real dispenser-then-shake execution path. +# It intentionally does NOT publish fake /joint_states. If the robot model moves +# in RViz, that movement comes from the connected real/virtual Doosan driver. +# With no robot connected, this shows the exact dry-run Path messages produced +# by the same nodes used by the real script. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +SELECTED_DISPENSER_ID="${SELECTED_DISPENSER_ID:-2}" +USE_DEMO_CUP_POSE="${USE_DEMO_CUP_POSE:-true}" +START_RVIZ="${START_RVIZ:-true}" +START_ROBOT_DESCRIPTION="${START_ROBOT_DESCRIPTION:-false}" +RVIZ_CONFIG="${RVIZ_CONFIG:-${ROOT_DIR}/src/azas_bringup/rviz/azas_real_mirror_dispenser.rviz}" +LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" +GRASP_X="${GRASP_X:-0.42}" +GRASP_Y="${GRASP_Y:--0.24}" +GRASP_Z="${GRASP_Z:-0.05}" +MOUTH_X="${MOUTH_X:-${GRASP_X}}" +MOUTH_Y="${MOUTH_Y:-${GRASP_Y}}" +MOUTH_Z="${MOUTH_Z:-0.22}" +SHAKE_CENTER_X="${SHAKE_CENTER_X:-0.28}" +SHAKE_CENTER_Y="${SHAKE_CENTER_Y:--0.30}" +SHAKE_CENTER_Z="${SHAKE_CENTER_Z:-0.62}" +SHAKE_AMPLITUDE_X="${SHAKE_AMPLITUDE_X:-0.100}" +SHAKE_AMPLITUDE_Y="${SHAKE_AMPLITUDE_Y:-0.040}" +SHAKE_AMPLITUDE_Z="${SHAKE_AMPLITUDE_Z:-0.055}" +SHAKE_CYCLES="${SHAKE_CYCLES:-4}" +SHAKE_TWIST_RX_DEG="${SHAKE_TWIST_RX_DEG:-6.0}" +SHAKE_TWIST_RZ_DEG="${SHAKE_TWIST_RZ_DEG:-22.0}" +APPROACH_LINE_TIME="${APPROACH_LINE_TIME:-3.5}" +SHAKE_LINE_TIME="${SHAKE_LINE_TIME:-0.40}" +MIN_SHAKE_Z="${MIN_SHAKE_Z:-0.55}" +DISPENSER_KEEPOUT_RADIUS="${DISPENSER_KEEPOUT_RADIUS:-0.20}" +# 교안식 검증: 실제/가상 Doosan controller service에 명령을 넣고, +# driver가 내보내는 /joint_states로 RViz가 움직이게 한다. +EXECUTE_CONTROLLER_MOTION="${EXECUTE_CONTROLLER_MOTION:-true}" +# Default to the verified virtual Doosan namespace. Use SERVICE_PREFIX=dsr01 +# explicitly only when the real robot/session is intentionally armed. +SERVICE_PREFIX="${SERVICE_PREFIX:-azasvirt}" +HARDWARE_CONFIRM="${HARDWARE_CONFIRM:-ENABLE_REAL_ROBOT_MOTION}" +DISABLE_GRIPPER_COMMANDS="${DISABLE_GRIPPER_COMMANDS:-true}" +MOTION_RESPONSE_TIMEOUT_SEC="${MOTION_RESPONSE_TIMEOUT_SEC:-60.0}" +SHAKE_CONTROL_MODE="${SHAKE_CONTROL_MODE:-joint}" +VERIFY_JOINT_TARGETS="${VERIFY_JOINT_TARGETS:-false}" +SHAKE_JOINT_VELOCITY="${SHAKE_JOINT_VELOCITY:-90.0}" +SHAKE_JOINT_ACCELERATION="${SHAKE_JOINT_ACCELERATION:-140.0}" + +mkdir -p "${LOG_DIR}" + +cleanup() { + for pid in "${PIDS[@]:-}"; do + if [[ -n "${pid}" ]] && kill -0 "${pid}" 2>/dev/null; then + kill "${pid}" 2>/dev/null || true + wait "${pid}" 2>/dev/null || true + fi + done +} +trap cleanup EXIT +PIDS=() + +set +u +source /opt/ros/humble/setup.bash +source /home/ssu/ros2_ws/install/setup.bash +source "${ROOT_DIR}/install/setup.bash" +set -u + +if [[ "${EXECUTE_CONTROLLER_MOTION}" == "true" ]]; then + missing=0 + service_list="$(ros2 service list --no-daemon || true)" + for service in "/${SERVICE_PREFIX}/motion/move_line" "/${SERVICE_PREFIX}/motion/move_joint"; do + if ! grep -qx "${service}" <<<"${service_list}"; then + echo "[Azas] Missing controller service: ${service}" >&2 + missing=1 + fi + done + if [[ "${missing}" != "0" ]]; then + echo "[Azas] Refusing controller-motion mirror. Start virtual Doosan first:" >&2 + echo " ROBOT_NAME=${SERVICE_PREFIX} bash tools/run/run_doosan_virtual_m0609.sh" >&2 + exit 1 + fi +fi + +if [[ "${START_ROBOT_DESCRIPTION}" == "true" ]]; then + DSR_DESCRIPTION_PREFIX="$(ros2 pkg prefix dsr_description2)" + DSR_XACRO="${DSR_DESCRIPTION_PREFIX}/share/dsr_description2/xacro/m0609.urdf.xacro" + ROBOT_URDF="${LOG_DIR}/real_mirror_m0609.urdf" + xacro "${DSR_XACRO}" color:=white simple:=true >"${ROBOT_URDF}" + ros2 run robot_state_publisher robot_state_publisher "${ROBOT_URDF}" \ + >"${LOG_DIR}/real_mirror_robot_state_publisher.log" 2>&1 & + PIDS+=("$!") +fi + +if [[ "${START_RVIZ}" == "true" ]]; then + rviz2 -d "${RVIZ_CONFIG}" >"${LOG_DIR}/real_mirror_rviz.log" 2>&1 & + PIDS+=("$!") +fi + +if [[ "${USE_DEMO_CUP_POSE}" == "true" ]]; then + # Demo source only replaces the camera detection input. The motion nodes below + # are still the same nodes used by real execution. Controller-motion mode + # sends commands to the virtual Doosan services, not to RViz-only joints. + ros2 launch azas_bringup hardware_free_demo.launch.py \ + use_rviz:=false \ + use_robot_urdf:=false \ + enable_ik_preview:=false \ + run_live_stt:=false \ + run_recipe_mapper:=false \ + use_llm:=false \ + show_sequence_markers:=false \ + show_dispenser_markers:=false \ + show_animated_cup:=false \ + show_demo_arm:=false \ + selected_dispenser_id:="${SELECTED_DISPENSER_ID}" \ + grasp_x:="${GRASP_X}" \ + grasp_y:="${GRASP_Y}" \ + grasp_z:="${GRASP_Z}" \ + mouth_x:="${MOUTH_X}" \ + mouth_y:="${MOUTH_Y}" \ + mouth_z:="${MOUTH_Z}" \ + >"${LOG_DIR}/real_mirror_demo_pose.log" 2>&1 & + PIDS+=("$!") + TUMBLER_POSE_TOPIC="/azas/demo/tumbler_pose" +else + TUMBLER_POSE_TOPIC="/jarvis/tumbler_dispenser/tumbler_pose" +fi + +if [[ "${EXECUTE_CONTROLLER_MOTION}" == "true" ]]; then + FLOOR_ENABLE_HARDWARE=true + FLOOR_ALLOW_SERVICE=true +else + FLOOR_ENABLE_HARDWARE=false + FLOOR_ALLOW_SERVICE=false +fi + +ros2 launch azas_bringup tumbler_floor_place.launch.py \ + selected_dispenser_id:="${SELECTED_DISPENSER_ID}" \ + delivery_mode:=hold_under_outlet \ + execution_stage:=full \ + use_tumbler_pose_topic:=true \ + tumbler_pose_topic:="${TUMBLER_POSE_TOPIC}" \ + enable_hardware:="${FLOOR_ENABLE_HARDWARE}" \ + hardware_confirm:="${HARDWARE_CONFIRM}" \ + allow_service_control_without_moveit:="${FLOOR_ALLOW_SERVICE}" \ + service_prefix:="${SERVICE_PREFIX}" \ + disable_gripper_commands:="${DISABLE_GRIPPER_COMMANDS}" \ + motion_response_timeout_sec:="${MOTION_RESPONSE_TIMEOUT_SEC}" \ + allow_demo_tumbler_position_fallback:=false \ + >"${LOG_DIR}/real_mirror_floor_place.log" 2>&1 & +FLOOR_PID="$!" +PIDS+=("${FLOOR_PID}") + +# 교안 원칙: one controller trajectory at a time. Wait for the dispenser +# transfer stage to finish before sending the shake sequence. +if [[ "${EXECUTE_CONTROLLER_MOTION}" == "true" ]]; then + floor_deadline=$((SECONDS + 120)) + while (( SECONDS < floor_deadline )); do + if grep -q "DONE" "${LOG_DIR}/real_mirror_floor_place.log" 2>/dev/null; then + break + fi + if grep -q "FAILED\|REJECTED\|STALE" "${LOG_DIR}/real_mirror_floor_place.log" 2>/dev/null; then + echo "[Azas] Floor/dispenser transfer failed; not starting shake." >&2 + exit 1 + fi + sleep 0.5 + done + if ! grep -q "DONE" "${LOG_DIR}/real_mirror_floor_place.log" 2>/dev/null; then + echo "[Azas] Floor/dispenser transfer did not finish before timeout; not starting shake." >&2 + exit 1 + fi +else + sleep 4 +fi + +if [[ "${EXECUTE_CONTROLLER_MOTION}" == "true" ]]; then + SHAKE_ENABLE_HARDWARE=true + SHAKE_ALLOW_SERVICE=true +else + SHAKE_ENABLE_HARDWARE=false + SHAKE_ALLOW_SERVICE=false +fi + +ros2 launch azas_bringup tumbler_shake_sequence.launch.py \ + enable_hardware:="${SHAKE_ENABLE_HARDWARE}" \ + hardware_confirm:="${HARDWARE_CONFIRM}" \ + allow_service_control_without_moveit:="${SHAKE_ALLOW_SERVICE}" \ + service_prefix:="${SERVICE_PREFIX}" \ + shake_control_mode:="${SHAKE_CONTROL_MODE}" \ + verify_joint_targets:="${VERIFY_JOINT_TARGETS}" \ + motion_response_timeout_sec:="${MOTION_RESPONSE_TIMEOUT_SEC}" \ + shake_joint_velocity:="${SHAKE_JOINT_VELOCITY}" \ + shake_joint_acceleration:="${SHAKE_JOINT_ACCELERATION}" \ + use_visualizer:=false \ + shake_center_x:="${SHAKE_CENTER_X}" \ + shake_center_y:="${SHAKE_CENTER_Y}" \ + shake_center_z:="${SHAKE_CENTER_Z}" \ + shake_amplitude_x:="${SHAKE_AMPLITUDE_X}" \ + shake_amplitude_y:="${SHAKE_AMPLITUDE_Y}" \ + shake_amplitude_z:="${SHAKE_AMPLITUDE_Z}" \ + shake_cycles:="${SHAKE_CYCLES}" \ + shake_twist_rx_deg:="${SHAKE_TWIST_RX_DEG}" \ + shake_twist_rz_deg:="${SHAKE_TWIST_RZ_DEG}" \ + approach_line_time:="${APPROACH_LINE_TIME}" \ + shake_line_time:="${SHAKE_LINE_TIME}" \ + min_shake_z:="${MIN_SHAKE_Z}" \ + dispenser_keepout_radius:="${DISPENSER_KEEPOUT_RADIUS}" \ + >"${LOG_DIR}/real_mirror_shake.log" 2>&1 & +SHAKE_PID="$!" +PIDS+=("${SHAKE_PID}") + +echo "[Azas] real-mirror RViz is running." +echo "[Azas] No fake joint animation is active. Robot movement in RViz must come from controller/driver /joint_states." +echo "[Azas] EXECUTE_CONTROLLER_MOTION=${EXECUTE_CONTROLLER_MOTION} SERVICE_PREFIX=${SERVICE_PREFIX} SHAKE_CONTROL_MODE=${SHAKE_CONTROL_MODE}" +echo "[Azas] Plans: /jarvis/tumbler_floor_place/plan and /jarvis/tumbler_shake_sequence/plan" +echo "[Azas] Logs: ${LOG_DIR}/real_mirror_*.log" +if [[ "${START_RVIZ}" == "true" ]]; then + wait +else + shake_deadline=$((SECONDS + 120)) + while (( SECONDS < shake_deadline )); do + if grep -q "DONE" "${LOG_DIR}/real_mirror_shake.log" 2>/dev/null; then + exit 0 + fi + if grep -q "FAILED\|REJECTED\|STALE" "${LOG_DIR}/real_mirror_shake.log" 2>/dev/null; then + echo "[Azas] Shake sequence failed." >&2 + exit 1 + fi + sleep 0.5 + done + echo "[Azas] Shake sequence did not finish before timeout." >&2 + exit 1 +fi diff --git a/tools/run/run_doosan_real_m0609.sh b/tools/run/run_doosan_real_m0609.sh new file mode 100755 index 0000000..d53fbba --- /dev/null +++ b/tools/run/run_doosan_real_m0609.sh @@ -0,0 +1,70 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Start Doosan M0609 ROS 2 / MoveIt bringup against the real controller for +# supervised real-motion panel runs. This intentionally avoids the legacy +# "no_motion" entrypoint name so panel logs cannot be mistaken for a virtual or +# motion-blocked run. + +ROBOT_NAME="${ROBOT_NAME:-}" +ROBOT_HOST="${ROBOT_HOST:-}" +ROBOT_PORT="${ROBOT_PORT:-12345}" +MODEL="${MODEL:-m0609}" +COLOR="${COLOR:-white}" +RT_HOST="${RT_HOST:-192.168.137.50}" +DOOSAN_REAL_MOTION_CONFIRM="${DOOSAN_REAL_MOTION_CONFIRM:-}" +SHOW_ARGS_ONLY="${SHOW_ARGS_ONLY:-false}" + +if [[ "${SHOW_ARGS_ONLY}" == "true" ]]; then + set +u + source /opt/ros/humble/setup.bash + source /home/ssu/ros2_ws/install/setup.bash + source /home/ssu/Azas/install/setup.bash + set -u + exec ros2 launch dsr_bringup2 dsr_bringup2_moveit.launch.py --show-args +fi + +if [[ -z "${ROBOT_HOST}" ]]; then + echo "[Azas] Refusing Doosan real bringup: ROBOT_HOST is required." + echo "[Azas] Example:" + echo " ROBOT_HOST=192.168.1.100 DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP $0" + exit 1 +fi + +if [[ "${ROBOT_HOST}" == "127.0.0.1" || "${ROBOT_HOST}" == "localhost" ]]; then + echo "[Azas] Refusing Doosan real bringup: ROBOT_HOST points to localhost." + echo "[Azas] Use /home/ssu/Azas/tools/run/run_doosan_virtual_m0609.sh for virtual mode." + exit 1 +fi + +if [[ "${DOOSAN_REAL_MOTION_CONFIRM}" != "ENABLE_DOOSAN_REAL_MOTION_BRINGUP" ]]; then + echo "[Azas] Refusing Doosan real bringup without explicit real-motion confirmation." + echo "[Azas] Re-run with:" + echo " DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP" + exit 1 +fi + +set +u +source /opt/ros/humble/setup.bash +source /home/ssu/ros2_ws/install/setup.bash +source /home/ssu/Azas/install/setup.bash +set -u + +echo "[Azas] Starting Doosan ${MODEL} REAL MOTION bringup" +echo "[Azas] mode=real name=${ROBOT_NAME:-} host=${ROBOT_HOST} port=${ROBOT_PORT}" +echo "[Azas] This entrypoint is for supervised real robot motion. Keep E-stop reachable." + +launch_args=( + host:="${ROBOT_HOST}" \ + port:="${ROBOT_PORT}" \ + mode:=real \ + model:="${MODEL}" \ + color:="${COLOR}" \ + rt_host:="${RT_HOST}" +) + +if [[ -n "${ROBOT_NAME}" ]]; then + launch_args=(name:="${ROBOT_NAME}" "${launch_args[@]}") +fi + +exec ros2 launch dsr_bringup2 dsr_bringup2_moveit.launch.py "${launch_args[@]}" diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index e047448..f0d9903 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -24,11 +24,12 @@ import rclpy import tf2_ros from azas_interfaces.srv import SetGripper -from dsr_msgs2.srv import GetCurrentPosx, Ikin, MoveLine +from dsr_msgs2.srv import Fkin, GetCurrentPosj, GetCurrentPosx, Ikin, MoveJoint, MoveLine, MoveWait ROOT = Path("/home/ssu/Azas") DEFAULT_CONFIG = ROOT / "src" / "azas_bringup" / "config" / "measured_dispenser_collision.yaml" +CALIBRATION_CONFIG = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" MOVE_FRONT_HOLD = ROOT / "tools" / "run" / "move_to_measured_dispenser_front_hold.py" PICK_FRONT_HOLD = ROOT / "tools" / "run" / "pick_from_measured_dispenser_front_hold.py" RG2_OPEN = ROOT / "tools" / "run" / "rg2_full_open_verify.sh" @@ -51,7 +52,25 @@ def parse_dispenser_ids(raw: str) -> list[str]: - values = [item.strip() for item in raw.replace(";", ",").split(",") if item.strip()] + values: list[str] = [] + for part in raw.replace(";", ",").split(","): + item = part.strip().lower() + if not item: + continue + if "x" in item: + dispenser_id, count_raw = item.split("x", 1) + elif ":" in item: + dispenser_id, count_raw = item.split(":", 1) + else: + dispenser_id, count_raw = item, "1" + dispenser_id = dispenser_id.strip() + try: + count = int(count_raw.strip()) + except ValueError as exc: + raise ValueError(f"invalid count for dispenser {dispenser_id}: {count_raw!r}") from exc + if count < 1: + raise ValueError(f"count must be >= 1 for dispenser {dispenser_id}") + values.extend([dispenser_id] * count) if not values: raise ValueError("at least one dispenser id is required") invalid = [value for value in values if value not in DISPENSER_TARGETS] @@ -60,6 +79,16 @@ def parse_dispenser_ids(raw: str) -> list[str]: return values +def parse_float_list(raw: str, *, expected_count: int, label: str) -> list[float]: + values = [part.strip() for part in raw.replace(";", ",").split(",") if part.strip()] + if len(values) != expected_count: + raise ValueError(f"{label} must contain {expected_count} comma-separated values") + try: + return [float(value) for value in values] + except ValueError as exc: + raise ValueError(f"{label} contains a non-numeric value: {raw!r}") from exc + + def service_name(prefix: str, suffix: str) -> str: clean_prefix = prefix.strip("/") clean_suffix = suffix.strip("/") @@ -157,6 +186,52 @@ def load_front_hold_pose(config_path: Path, dispenser_id: str) -> tuple[list[flo return position, quaternion, matrix_to_doosan_zyz_deg(quaternion_to_matrix_xyzw(quaternion)) +def load_press_pose(dispenser_id: str) -> tuple[list[float], list[float]]: + data = yaml.safe_load(CALIBRATION_CONFIG.read_text(encoding="utf-8")) or {} + outlets = data.get("dispenser_outlets") or {} + block = outlets.get(str(dispenser_id)) + if not isinstance(block, dict): + raise ValueError(f"dispenser_outlets.{dispenser_id} is missing in {CALIBRATION_CONFIG}") + position = numeric_list( + block.get("press_pose_xyz_m"), + f"dispenser_outlets.{dispenser_id}.press_pose_xyz_m", + 3, + ) + rpy_deg = numeric_list( + block.get("press_pose_rpy_deg"), + f"dispenser_outlets.{dispenser_id}.press_pose_rpy_deg", + 3, + ) + return position, rpy_deg + + +def load_press_ready_joints_deg(dispenser_id: str) -> list[float] | None: + data = yaml.safe_load(CALIBRATION_CONFIG.read_text(encoding="utf-8")) or {} + outlets = data.get("dispenser_outlets") or {} + block = outlets.get(str(dispenser_id)) + if not isinstance(block, dict): + raise ValueError(f"dispenser_outlets.{dispenser_id} is missing in {CALIBRATION_CONFIG}") + raw_joints = block.get("press_contact_joints_deg", block.get("press_ready_joints_deg")) + if raw_joints is None: + return None + return numeric_list( + raw_joints, + f"dispenser_outlets.{dispenser_id}.press_contact_joints_deg", + 6, + ) + + +def group_consecutive_dispenser_ids(dispenser_ids: list[str]) -> list[tuple[str, int]]: + groups: list[tuple[str, int]] = [] + for dispenser_id in dispenser_ids: + if groups and groups[-1][0] == dispenser_id: + previous_id, count = groups[-1] + groups[-1] = (previous_id, count + 1) + else: + groups.append((dispenser_id, 1)) + return groups + + class IntegratedRecipeMotion: """Keep ROS service clients alive across release/re-grasp loops. @@ -172,7 +247,11 @@ def __init__(self, args: argparse.Namespace) -> None: self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) self.move_line = self.node.create_client(MoveLine, service_name(args.service_prefix, "motion/move_line")) + self.move_joint = self.node.create_client(MoveJoint, service_name(args.service_prefix, "motion/move_joint")) + self.move_wait = self.node.create_client(MoveWait, service_name(args.service_prefix, "motion/move_wait")) + self.fkin = self.node.create_client(Fkin, service_name(args.service_prefix, "motion/fkin")) self.ikin = self.node.create_client(Ikin, service_name(args.service_prefix, "motion/ikin")) + self.get_posj = self.node.create_client(GetCurrentPosj, service_name(args.service_prefix, "aux_control/get_current_posj")) self.get_posx = self.node.create_client(GetCurrentPosx, service_name(args.service_prefix, "aux_control/get_current_posx")) self.gripper = self.node.create_client(SetGripper, args.gripper_service) @@ -181,6 +260,25 @@ def close(self) -> None: if rclpy.ok(): rclpy.shutdown() + def preflight(self) -> None: + required = [ + (self.move_line, "MoveLine"), + (self.move_joint, "MoveJoint"), + (self.move_wait, "MoveWait"), + (self.fkin, "Fkin"), + (self.ikin, "Ikin"), + (self.get_posj, "GetCurrentPosj"), + (self.get_posx, "GetCurrentPosx"), + (self.gripper, "RG2 set_width"), + ] + missing = [ + f"{label} ({getattr(client, 'srv_name', '')})" + for client, label in required + if not client.wait_for_service(timeout_sec=max(self.args.wait_service_sec, 0.1)) + ] + if missing: + raise RuntimeError("required service(s) unavailable before motion: " + ", ".join(missing)) + def _call(self, client: Any, request: Any, *, timeout_sec: float, label: str) -> Any: if not client.wait_for_service(timeout_sec=max(self.args.wait_service_sec, 0.1)): raise RuntimeError(f"{label} service not available: {getattr(client, 'srv_name', '')}") @@ -195,6 +293,16 @@ def _call(self, client: Any, request: Any, *, timeout_sec: float, label: str) -> raise RuntimeError(f"{label} returned no response") return response + def wait_motion_done(self, label: str, *, timeout_sec: float) -> None: + response = self._call( + self.move_wait, + MoveWait.Request(), + timeout_sec=timeout_sec, + label=f"MoveWait {label}", + ) + if not response.success: + raise RuntimeError(f"MoveWait returned success=false for {label}") + def current_posx(self, timeout_sec: float | None = None) -> list[float]: req = GetCurrentPosx.Request() req.ref = DR_BASE @@ -211,6 +319,18 @@ def current_posx(self, timeout_sec: float | None = None) -> list[float]: raise RuntimeError(f"GetCurrentPosx returned too few values: {values}") return [float(value) for value in values[:6]] + def current_posj(self, timeout_sec: float | None = None) -> list[float]: + response = self._call( + self.get_posj, + GetCurrentPosj.Request(), + timeout_sec=timeout_sec or self.args.wait_service_sec, + label="GetCurrentPosj", + ) + values = list(response.pos) + if not response.success or len(values) < 6: + raise RuntimeError("GetCurrentPosj returned success=false or too few joint values") + return [float(value) for value in values[:6]] + def current_tcp_pose(self) -> Pose: values = self.current_posx() return [values[index] / 1000.0 for index in range(3)], doosan_zyz_deg_to_matrix(values[3:6]) @@ -279,8 +399,111 @@ def move_front_hold( response = self._call(self.move_line, req, timeout_sec=self.args.move_timeout_sec, label=f"MoveLine {label}") if not response.success: raise RuntimeError(f"MoveLine returned success=false for {label}") + self.wait_motion_done(label, timeout_sec=self.args.move_timeout_sec) + self.wait_for_target(pos, label=label) + + def move_posx( + self, + pos: list[float], + *, + label: str, + velocity: float, + acceleration: float, + timeout_sec: float, + ) -> None: + print( + f"[Azas] {label}: posx=[{pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f}, " + f"{pos[3]:.1f}, {pos[4]:.1f}, {pos[5]:.1f}]" + ) + req = MoveLine.Request() + req.pos = pos + req.vel = [velocity, velocity] + req.acc = [acceleration, acceleration] + req.time = 0.0 + req.radius = 0.0 + req.ref = DR_BASE + req.mode = MOVE_MODE_ABSOLUTE + req.blend_type = BLENDING_SPEED_TYPE_DUPLICATE + req.sync_type = SYNC + response = self._call(self.move_line, req, timeout_sec=timeout_sec, label=f"MoveLine {label}") + if not response.success: + raise RuntimeError(f"MoveLine returned success=false for {label}") + self.wait_motion_done(label, timeout_sec=timeout_sec) self.wait_for_target(pos, label=label) + def movej(self, joints_deg: list[float], *, label: str, velocity: float, acceleration: float) -> None: + print( + "[Azas] " + + label + + ": movej_deg=[" + + ", ".join(f"{value:.1f}" for value in joints_deg) + + "]" + ) + req = MoveJoint.Request() + req.pos = [float(value) for value in joints_deg] + req.vel = float(velocity) + req.acc = float(acceleration) + req.time = 0.0 + req.radius = 0.0 + req.mode = MOVE_MODE_ABSOLUTE + req.blend_type = BLENDING_SPEED_TYPE_DUPLICATE + req.sync_type = SYNC + response = self._call(self.move_joint, req, timeout_sec=self.args.press_timeout_sec, label=f"MoveJoint {label}") + if not response.success: + raise RuntimeError(f"MoveJoint returned success=false for {label}") + self.wait_motion_done(label, timeout_sec=self.args.press_timeout_sec) + self.wait_for_joint_target(joints_deg, label=label) + + def fkin_posx(self, joints_deg: list[float], *, label: str) -> list[float]: + req = Fkin.Request() + req.pos = [float(value) for value in joints_deg] + req.ref = DR_BASE + response = self._call(self.fkin, req, timeout_sec=self.args.wait_service_sec, label=f"Fkin {label}") + if not response.success: + raise RuntimeError(f"Fkin returned success=false for {label}") + values = [float(value) for value in response.conv_posx[:6]] + if len(values) < 6: + raise RuntimeError(f"Fkin returned too few posx values for {label}: {values}") + print( + f"[Azas] {label}: fkin_posx=[{values[0]:.1f}, {values[1]:.1f}, {values[2]:.1f}, " + f"{values[3]:.1f}, {values[4]:.1f}, {values[5]:.1f}]" + ) + return values + + def ikin_posj(self, posx_mm_deg: list[float], *, label: str) -> list[float]: + req = Ikin.Request() + req.pos = [float(value) for value in posx_mm_deg] + req.sol_space = int(self.args.ikin_sol_space) + req.ref = DR_BASE + response = self._call(self.ikin, req, timeout_sec=self.args.wait_service_sec, label=f"Ikin {label}") + if not response.success: + raise RuntimeError(f"Ikin returned success=false for {label}") + values = [float(value) for value in response.conv_posj[:6]] + if len(values) < 6: + raise RuntimeError(f"Ikin returned too few posj values for {label}: {values}") + print( + f"[Azas] {label}: ikin_posj=[" + + ", ".join(f"{value:.1f}" for value in values) + + "]" + ) + return values + + def wait_for_joint_target(self, target_joints_deg: list[float], *, label: str) -> None: + deadline = time.monotonic() + max(self.args.verify_timeout_sec, 0.1) + last_error = 999999.0 + while time.monotonic() < deadline: + actual = self.current_posj(timeout_sec=5.0) + errors = [abs(actual[index] - target_joints_deg[index]) for index in range(6)] + last_error = max(errors) + print( + f"[Azas] verify {label}: max_joint_error={last_error:.2f}deg " + f"j6={actual[5]:.2f}deg tolerance={self.args.joint_target_tolerance_deg:.2f}deg" + ) + if last_error <= max(self.args.joint_target_tolerance_deg, 0.1): + return + time.sleep(max(self.args.verify_poll_seconds, 0.05)) + raise RuntimeError(f"joint target verification timeout for {label}; max_error={last_error:.2f}deg") + def wait_for_target(self, target_pos_mm_deg: list[float], *, label: str) -> None: deadline = time.monotonic() + max(self.args.verify_timeout_sec, 0.1) last_distance = 999999.0 @@ -290,7 +513,7 @@ def wait_for_target(self, target_pos_mm_deg: list[float], *, label: str) -> None print(f"[Azas] verify {label}: distance={last_distance:.1f}mm tolerance={self.args.target_tolerance_mm:.1f}mm") if last_distance <= max(self.args.target_tolerance_mm, 0.1): return - time.sleep(1.0) + time.sleep(max(self.args.verify_poll_seconds, 0.05)) raise RuntimeError(f"target verification timeout for {label}; distance={last_distance:.1f}mm") def gripper_command(self, command: str, *, width_m: float, force_n: float, label: str) -> None: @@ -302,6 +525,13 @@ def gripper_command(self, command: str, *, width_m: float, force_n: float, label if not response.success: raise RuntimeError(f"{label} returned success=false: {response.message}") print(f"[Azas] {label}: {response.message}") + settle_sec = max( + self.args.gripper_open_settle_seconds if command == "open" else self.args.gripper_settle_seconds, + 0.0, + ) + if settle_sec > 0.0: + print(f"[Azas] {label}: waiting {settle_sec:.2f}s for physical RG2 motion to settle") + time.sleep(settle_sec) def move_and_release(self, dispenser_id: str) -> None: stages = [ @@ -337,6 +567,7 @@ def move_and_release(self, dispenser_id: str) -> None: force_n=self.args.gripper_open_force_n, label="RG2 full-open release", ) + print("[Azas] RG2 full-open release complete; continuing only after open settle wait") def regrasp_and_lift(self, dispenser_id: str) -> None: self.gripper_command( @@ -375,8 +606,224 @@ def regrasp_and_lift(self, dispenser_id: str) -> None: response = self._call(self.move_line, req, timeout_sec=self.args.pick_timeout_sec, label="post-grasp lift") if not response.success: raise RuntimeError("post-grasp lift returned success=false") + self.wait_motion_done("post-grasp lift", timeout_sec=self.args.pick_timeout_sec) self.wait_for_target(target, label="post-grasp lift") + def press_dispenser(self, dispenser_id: str, press_count: int) -> None: + press_xyz_m, press_rpy_deg = load_press_pose(dispenser_id) + current_pose = self.current_posx() + contact_joints = load_press_ready_joints_deg(dispenser_id) + joint_space_press = contact_joints is not None + if contact_joints is None: + x_mm = press_xyz_m[0] * 1000.0 + y_mm = press_xyz_m[1] * 1000.0 + contact_z = press_xyz_m[2] * 1000.0 + rx, ry, rz = press_rpy_deg + print( + f"[Azas] dispenser {dispenser_id}: no press contact joints in calibration; " + "falling back to press_pose_xyz_m/rpy_deg" + ) + else: + contact_joints = list(contact_joints) + if self.args.press_force_joint6_zero: + before_j6 = contact_joints[5] + contact_joints[5] = 0.0 + print( + f"[Azas] dispenser {dispenser_id}: forcing press contact joint_6/link_6 " + f"from {before_j6:.2f}deg to 0.00deg before FK" + ) + else: + print( + f"[Azas] dispenser {dispenser_id}: using measured press contact joints exactly " + f"(joint_6/link_6={contact_joints[5]:.2f}deg)" + ) + x_mm = press_xyz_m[0] * 1000.0 + y_mm = press_xyz_m[1] * 1000.0 + contact_z = press_xyz_m[2] * 1000.0 + rx, ry, rz = press_rpy_deg + if joint_space_press: + # Do not depend on /motion/fkin here. The Doosan FK service can + # block on this setup, and the measured joint position itself is + # the authoritative press contact pose. MoveJoint to the measured + # pose first, then read the live TCP as the contact reference. + transit_z = current_pose[2] + max( + self.args.press_transit_height_m, + self.args.press_pre_lift_m, + 0.0, + ) * 1000.0 + pre_z = contact_z + max(self.args.press_pre_lift_m, 0.0) * 1000.0 + pressed_z = contact_z - max(self.args.press_depth_m, 0.0) * 1000.0 + print( + "[Azas] integrated press: " + f"dispenser={dispenser_id} count={press_count} " + f"configured_contact=({x_mm:.1f}, {y_mm:.1f}, {contact_z:.1f}) " + f"configured_pre_z={pre_z:.1f} configured_pressed_z={pressed_z:.1f} " + f"z_descent={contact_z - pressed_z:.1f}mm transit_z={transit_z:.1f} " + "source=calibration pre before measured MoveJoint" + ) + else: + pre_z = contact_z + max(self.args.press_pre_lift_m, 0.0) * 1000.0 + pressed_z = contact_z - max(self.args.press_depth_m, 0.0) * 1000.0 + transit_z = max(current_pose[2], pre_z) + max(self.args.press_transit_height_m, 0.0) * 1000.0 + print( + "[Azas] integrated press: " + f"dispenser={dispenser_id} count={press_count} " + f"contact=({x_mm:.1f}, {y_mm:.1f}, {contact_z:.1f}) " + f"pre_z={pre_z:.1f} pressed_z={pressed_z:.1f} transit_z={transit_z:.1f}" + ) + safe_lift = [ + current_pose[0], + current_pose[1], + transit_z, + current_pose[3], + current_pose[4], + current_pose[5], + ] + self.move_posx( + safe_lift, + label="safe lift away from released cup before press", + velocity=self.args.press_travel_velocity, + acceleration=self.args.press_travel_acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + self.gripper_command( + "set_width", + width_m=self.args.press_gripper_close_width_m, + force_n=self.args.press_gripper_force_n, + label="RG2 close empty gripper for dispenser press", + ) + if self.args.press_reset_before_press: + # The operator requirement is explicit: after releasing the cup and + # closing the empty RG2, rotate the gripper/link_6 back to 0 deg + # from a safe high pose. Do not use a Cartesian orientation move + # for this reset, because Doosan IK may swing wrist joint 4/5. + safe_joints = self.current_posj() + reset_joints = list(safe_joints) + reset_joints[5] = 0.0 + self.movej( + reset_joints, + label="reset link_6/joint_6 to 0 at safe height", + velocity=self.args.press_reset_joint_velocity, + acceleration=self.args.press_reset_joint_acceleration, + ) + + steps: list[tuple[list[float], str, float, float]] = [] + if joint_space_press: + self.move_posx( + [x_mm, y_mm, pre_z, rx, ry, rz], + label="high pre pose before measured press joint", + velocity=self.args.press_travel_velocity, + acceleration=self.args.press_travel_acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + self.movej( + contact_joints, + label="move to measured press contact joints exactly", + velocity=self.args.press_contact_joint_velocity, + acceleration=self.args.press_contact_joint_acceleration, + ) + contact_posx = self.current_posx(timeout_sec=self.args.wait_service_sec) + x_mm, y_mm, contact_z, rx, ry, rz = contact_posx + pre_z = contact_z + max(self.args.press_pre_lift_m, 0.0) * 1000.0 + pressed_z = contact_z - max(self.args.press_depth_m, 0.0) * 1000.0 + print( + "[Azas] integrated press: " + f"dispenser={dispenser_id} count={press_count} " + f"contact=({x_mm:.1f}, {y_mm:.1f}, {contact_z:.1f}) " + f"pre_z={pre_z:.1f} pressed_z={pressed_z:.1f} " + f"z_descent={contact_z - pressed_z:.1f}mm transit_z={transit_z:.1f} " + "source=live TCP after measured MoveJoint" + ) + steps.append( + ( + [x_mm, y_mm, pre_z, rx, ry, rz], + "pre pose above dispenser head", + self.args.press_line_velocity, + self.args.press_line_acceleration, + ) + ) + else: + steps.extend( + [ + ( + [x_mm, y_mm, transit_z, rx, ry, rz], + "align above measured press contact", + self.args.press_travel_velocity, + self.args.press_travel_acceleration, + ), + ( + [x_mm, y_mm, pre_z, rx, ry, rz], + "pre pose above dispenser head", + self.args.press_travel_velocity, + self.args.press_travel_acceleration, + ), + ] + ) + for press_index in range(1, max(int(press_count), 1) + 1): + suffix = f" {press_index}/{press_count}" if press_count > 1 else "" + if max(self.args.press_depth_m, 0.0) == 0.0: + steps.append( + ( + [x_mm, y_mm, contact_z, rx, ry, rz], + f"press dispenser pump{suffix}", + self.args.press_line_velocity, + self.args.press_line_acceleration, + ) + ) + else: + steps.extend( + [ + ( + [x_mm, y_mm, contact_z, rx, ry, rz], + f"move to measured contact pose{suffix}", + self.args.press_line_velocity, + self.args.press_line_acceleration, + ), + ( + [x_mm, y_mm, pressed_z, rx, ry, rz], + f"press dispenser pump{suffix}", + self.args.press_line_velocity, + self.args.press_line_acceleration, + ), + ] + ) + steps.append( + ( + [x_mm, y_mm, pre_z, rx, ry, rz], + f"retreat above dispenser{suffix}", + self.args.press_line_velocity, + self.args.press_line_acceleration, + ) + ) + if self.args.press_post_retreat_after_sequence: + steps.append( + ( + [ + x_mm + self.args.press_post_retreat_dx_m * 1000.0, + y_mm + self.args.press_post_retreat_dy_m * 1000.0, + pre_z, + rx, + ry, + rz, + ], + "retreat away from dispenser", + self.args.press_travel_velocity, + self.args.press_travel_acceleration, + ) + ) + for pos, label, velocity, acceleration in steps: + self.move_posx( + pos, + label=label, + velocity=velocity, + acceleration=acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + if label.startswith("press dispenser pump") and self.args.press_hold_seconds > 0.0: + time.sleep(self.args.press_hold_seconds) + if self.args.press_post_retreat_wait_seconds > 0.0: + time.sleep(self.args.press_post_retreat_wait_seconds) + def run_command(label: str, cmd: list[str] | str) -> int: print(f"[Azas] === {label} ===") @@ -482,21 +929,39 @@ def move_and_release_cmd(args: argparse.Namespace, dispenser_id: str) -> str: return " && ".join(shlex.join(command) for command in commands) -def press_cmd(args: argparse.Namespace, dispenser_id: str) -> str: - target = DISPENSER_TARGETS[dispenser_id] +def press_cmd(args: argparse.Namespace, dispenser_id: str, press_count: int) -> str: + press_xyz_m, press_rpy_deg = load_press_pose(dispenser_id) service_prefix = shlex.quote(args.service_prefix) tcp_name = shlex.quote(args.dispenser_tcp_name) - target_q = shlex.quote(target) return ( + "echo " + + shlex.quote( + "[Azas] measured recipe press pose dispenser_" + f"{dispenser_id}: xyz_m={press_xyz_m} rpy_deg={press_rpy_deg} " + f"press_count={press_count} source=calibration.yaml" + ) + + " && " "ros2 run azas_dispenser dispenser_press_node --ros-args " f"-p service_prefix:={service_prefix} " - "-p use_taught_posx:=true " + "-p use_taught_posx:=false " + "-p use_home_as_reference:=false " + "-p keep_home_orientation:=false " + f"-p dispenser_x:={press_xyz_m[0]:.6f} " + f"-p dispenser_y:={press_xyz_m[1]:.6f} " + "-p dispenser_y_offset:=0.0 " + f"-p dispenser_top_z:={press_xyz_m[2]:.6f} " + f"-p rx:={press_rpy_deg[0]:.6f} " + f"-p ry:={press_rpy_deg[1]:.6f} " + f"-p rz:={press_rpy_deg[2]:.6f} " + f"-p press_count:={int(press_count)} " + # calibration.yaml press_pose_xyz_m is the taught final press pose. + # Do not subtract an extra legacy pump depth here. + "-p press_depth:=0.0 " f"-p tcp_name:={tcp_name} " "-p require_tcp_for_taught_posx:=false " - "-p allow_tcp_set_failure:=true " - f"-p target_dispenser:={target_q} " - "-p move_home_first:=true " - "-p pre_home_retreat_before_home:=true " + "-p allow_tcp_set_failure:=false " + "-p move_home_first:=false " + "-p pre_home_retreat_before_home:=false " "-p pre_home_retreat_dx_mm:=-180.0 " "-p pre_home_retreat_dy_mm:=0.0 " "-p pre_home_retreat_min_z_mm:=520.0 -p pre_home_retreat_lift_first:=true " @@ -506,8 +971,12 @@ def press_cmd(args: argparse.Namespace, dispenser_id: str) -> str: "-p joint1_clearance_before_home:=false " "-p joint1_clearance_return_home:=false " "-p joint1_clearance_offset_deg:=12.0 " - "-p return_home:=true " - "-p close_gripper_at_home:=true " + "-p return_home:=false " + "-p close_gripper_at_home:=false " + "-p post_press_retreat_after_sequence:=true " + "-p post_press_retreat_dx_mm:=-120.0 " + "-p post_press_retreat_dy_mm:=0.0 " + "-p post_press_retreat_wait_seconds:=1.0 " "-p gripper_service:=/jarvis/rg2/set_width " "-p gripper_close_width:=0.0 " "-p gripper_close_force:=30.0 " @@ -582,40 +1051,102 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--config", type=Path, default=DEFAULT_CONFIG) parser.add_argument("--service-prefix", default="dsr01") parser.add_argument("--dispenser-tcp-name", default="GripperDA_v1_jarvis") - parser.add_argument("--move-velocity", type=float, default=30.0) - parser.add_argument("--move-acceleration", type=float, default=30.0) + parser.add_argument("--move-velocity", type=float, default=70.0) + parser.add_argument("--move-acceleration", type=float, default=90.0) parser.add_argument("--move-prehold-offset-x-m", type=float, default=0.0) parser.add_argument("--move-prehold-offset-y-m", type=float, default=0.0) parser.add_argument("--move-prehold-offset-z-m", type=float, default=0.0) - parser.add_argument("--move-prehold-velocity", type=float, default=12.0) - parser.add_argument("--move-prehold-acceleration", type=float, default=16.0) + parser.add_argument("--move-prehold-velocity", type=float, default=50.0) + parser.add_argument("--move-prehold-acceleration", type=float, default=70.0) parser.add_argument("--move-timeout-sec", type=float, default=180.0) - parser.add_argument("--pick-approach-velocity", type=float, default=15.0) - parser.add_argument("--pick-approach-acceleration", type=float, default=20.0) + parser.add_argument("--pick-approach-velocity", type=float, default=35.0) + parser.add_argument("--pick-approach-acceleration", type=float, default=50.0) parser.add_argument("--pick-pregrasp-offset-x-m", type=float, default=0.0) parser.add_argument("--pick-pregrasp-offset-y-m", type=float, default=0.0) parser.add_argument("--pick-pregrasp-offset-z-m", type=float, default=0.0) parser.add_argument("--pick-pregrasp-staging-velocity", type=float, default=12.0) parser.add_argument("--pick-pregrasp-staging-acceleration", type=float, default=16.0) parser.add_argument("--pick-lift-m", type=float, default=0.100) - parser.add_argument("--pick-lift-velocity", type=float, default=12.0) - parser.add_argument("--pick-lift-acceleration", type=float, default=16.0) + parser.add_argument("--pick-lift-velocity", type=float, default=35.0) + parser.add_argument("--pick-lift-acceleration", type=float, default=50.0) parser.add_argument("--pick-timeout-sec", type=float, default=120.0) + parser.add_argument( + "--press-depth-m", + type=float, + default=0.080, + help="Z descent below the measured dispenser-head contact pose.", + ) + parser.add_argument( + "--press-pre-lift-m", + type=float, + default=0.300, + help="Z lift above the measured dispenser-head contact pose before descending to press.", + ) + parser.add_argument("--press-approach-height-m", type=float, default=0.100) + parser.add_argument("--press-transit-height-m", type=float, default=0.300) + parser.add_argument("--press-line-velocity", type=float, default=10.0) + parser.add_argument("--press-line-acceleration", type=float, default=15.0) + parser.add_argument("--press-travel-velocity", type=float, default=20.0) + parser.add_argument("--press-travel-acceleration", type=float, default=30.0) + parser.add_argument("--press-timeout-sec", type=float, default=120.0) + parser.add_argument("--press-hold-seconds", type=float, default=0.25) + parser.add_argument("--press-gripper-close-width-m", type=float, default=0.0) + parser.add_argument("--press-gripper-force-n", type=float, default=30.0) + parser.add_argument("--press-reset-before-press", action=argparse.BooleanOptionalAction, default=False) + parser.add_argument( + "--press-reset-joints-deg", + default="0,0,90,0,90,0", + help="Joint reset pose used after safe lift and RG2 close, before moving above the press pose.", + ) + parser.add_argument("--press-reset-joint-velocity", type=float, default=40.0) + parser.add_argument("--press-reset-joint-acceleration", type=float, default=50.0) + parser.add_argument("--press-contact-joint-velocity", type=float, default=12.0) + parser.add_argument("--press-contact-joint-acceleration", type=float, default=18.0) + parser.add_argument("--press-post-retreat-after-sequence", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument("--press-post-retreat-dx-m", type=float, default=-0.120) + parser.add_argument("--press-post-retreat-dy-m", type=float, default=0.0) + parser.add_argument("--press-post-retreat-wait-seconds", type=float, default=0.10) parser.add_argument("--wait-service-sec", type=float, default=8.0) parser.add_argument("--verify-timeout-sec", type=float, default=70.0) + parser.add_argument("--verify-poll-seconds", type=float, default=0.15) parser.add_argument("--target-tolerance-mm", type=float, default=15.0) + parser.add_argument("--joint-target-tolerance-deg", type=float, default=2.0) parser.add_argument("--gripper-service", default="/jarvis/rg2/set_width") parser.add_argument("--gripper-open-width-m", type=float, default=0.110) parser.add_argument("--gripper-open-force-n", type=float, default=12.0) parser.add_argument("--gripper-grasp-width-m", type=float, default=0.075) parser.add_argument("--gripper-force-n", type=float, default=25.0) parser.add_argument("--gripper-timeout-sec", type=float, default=12.0) + parser.add_argument( + "--gripper-settle-seconds", + type=float, + default=2.0, + help="Physical wait after every non-open RG2 command before the next robot motion.", + ) + parser.add_argument( + "--gripper-open-settle-seconds", + type=float, + default=5.0, + help="Physical wait after every RG2 open command before the next robot motion.", + ) + parser.add_argument( + "--press-force-joint6-zero", + action=argparse.BooleanOptionalAction, + default=False, + help="Force measured press contact joint_6/link_6 to 0 deg before FK-derived pre/press poses. Default is false: use measured joints exactly.", + ) parser.add_argument("--precheck-ikin", action=argparse.BooleanOptionalAction, default=True) parser.add_argument("--ikin-sol-space", type=int, default=2) parser.add_argument("--legacy-subprocess-primitives", action="store_true", help="use the old helper-script-per-step implementation for fallback/debugging") parser.add_argument("--execute", action="store_true") parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} when --execute is used") - return parser.parse_args() + args = parser.parse_args() + args.press_reset_joints_deg = parse_float_list( + args.press_reset_joints_deg, + expected_count=6, + label="--press-reset-joints-deg", + ) + return args def main() -> int: @@ -639,22 +1170,36 @@ def main() -> int: print("[Azas] Measured dispenser recipe sequence") print(f"[Azas] dispenser_ids={','.join(dispenser_ids)}") + grouped_dispenser_ids = group_consecutive_dispenser_ids(dispenser_ids) + print( + "[Azas] grouped_press_counts=" + + ",".join(f"{dispenser_id}x{count}" for dispenser_id, count in grouped_dispenser_ids) + ) print(f"[Azas] service_prefix={args.service_prefix}") print(f"[Azas] dispenser_tcp_name={args.dispenser_tcp_name}") - print("[Azas] source=existing measured front_hold poses and taught dispenser press poses") + print("[Azas] source=existing measured front_hold poses and calibration.yaml press poses") motion: IntegratedRecipeMotion | None = None if args.execute and not args.legacy_subprocess_primitives: print("[Azas] integrated_motion=true (persistent ROS clients for move/release/re-grasp)") - motion = IntegratedRecipeMotion(args) + try: + motion = IntegratedRecipeMotion(args) + motion.preflight() + except RuntimeError as exc: + print(f"[FAIL] integrated preflight failed: {exc}") + return 1 elif args.execute: print("[Azas] integrated_motion=false (legacy subprocess primitives requested)") try: - for index, dispenser_id in enumerate(dispenser_ids, start=1): - label_prefix = f"recipe {index}/{len(dispenser_ids)} dispenser {dispenser_id}" + total_groups = len(grouped_dispenser_ids) + for index, (dispenser_id, press_count) in enumerate(grouped_dispenser_ids, start=1): + label_prefix = f"recipe group {index}/{total_groups} dispenser {dispenser_id} x{press_count}" if not args.execute: - print(f"[PLAN] {label_prefix}: integrated move/release -> press -> integrated re-grasp/lift (move/release -> press -> re-grasp/lift)") + print( + f"[PLAN] {label_prefix}: integrated move/release -> " + f"integrated press {press_count} time(s) -> integrated re-grasp/lift" + ) continue if motion is None: @@ -671,19 +1216,30 @@ def main() -> int: print(f"[FAIL] {label_prefix}: integrated move/release failed: {exc}") return 1 - rc = run_command( - f"{label_prefix}: mark tumbler world object at dispenser", - tumbler_scene_cmd( - "add_dispenser", - object_id=f"tumbler_at_dispenser_{dispenser_id}", - dispenser_id=dispenser_id, - ), - ) - if rc != 0: - return rc - rc = run_command(f"{label_prefix}: press dispenser", press_cmd(args, dispenser_id)) - if rc != 0: - return rc + if motion is None: + rc = run_command( + f"{label_prefix}: mark tumbler world object at dispenser", + tumbler_scene_cmd( + "add_dispenser", + object_id=f"tumbler_at_dispenser_{dispenser_id}", + dispenser_id=dispenser_id, + ), + ) + if rc != 0: + return rc + if motion is None: + rc = run_command( + f"{label_prefix}: press dispenser {press_count} time(s)", + press_cmd(args, dispenser_id, press_count), + ) + if rc != 0: + return rc + else: + try: + motion.press_dispenser(dispenser_id, press_count) + except RuntimeError as exc: + print(f"[FAIL] {label_prefix}: integrated press failed: {exc}") + return 1 if motion is None: rc = run_command(f"{label_prefix}: re-grasp cup from front-hold", pick_cmd(args, dispenser_id)) @@ -696,22 +1252,23 @@ def main() -> int: print(f"[FAIL] {label_prefix}: integrated re-grasp/lift failed: {exc}") return 1 - rc = run_command( - f"{label_prefix}: remove dispenser world object", - tumbler_scene_cmd( - "remove_world", - object_id=f"tumbler_at_dispenser_{dispenser_id}", - dispenser_id=dispenser_id, - ), - ) - if rc != 0: - return rc - rc = run_command( - f"{label_prefix}: attach carried tumbler object", - tumbler_scene_cmd("attach", object_id="carried_tumbler", dispenser_id=dispenser_id), - ) - if rc != 0: - return rc + if motion is None: + rc = run_command( + f"{label_prefix}: remove dispenser world object", + tumbler_scene_cmd( + "remove_world", + object_id=f"tumbler_at_dispenser_{dispenser_id}", + dispenser_id=dispenser_id, + ), + ) + if rc != 0: + return rc + rc = run_command( + f"{label_prefix}: attach carried tumbler object", + tumbler_scene_cmd("attach", object_id="carried_tumbler", dispenser_id=dispenser_id), + ) + if rc != 0: + return rc finally: if motion is not None: motion.close() diff --git a/tools/run/run_smooth_orange_robot_rviz.sh b/tools/run/run_smooth_orange_robot_rviz.sh new file mode 100755 index 0000000..a74e9f2 --- /dev/null +++ b/tools/run/run_smooth_orange_robot_rviz.sh @@ -0,0 +1,68 @@ +#!/usr/bin/env bash +set -euo pipefail + +# RViz robot-motion preview only: no MoveIt path display, no controller, no fake high-frequency shake. +# Shows the orange M0609 model itself moving smoothly from /joint_states. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" +RVIZ_CONFIG="${RVIZ_CONFIG:-${ROOT_DIR}/src/azas_bringup/rviz/azas_dispenser_sequence_clean.rviz}" +PUBLISH_RATE="${PUBLISH_RATE:-60.0}" +SHAKE_CYCLES_PER_SECOND="${SHAKE_CYCLES_PER_SECOND:-0.55}" +PREVIEW_MODE="${PREVIEW_MODE:-side_grasp_move_then_shake}" +LOOP_MOTION="${LOOP_MOTION:-true}" +ROBOT_COLOR="${ROBOT_COLOR:-orange}" +mkdir -p "${LOG_DIR}" + +cleanup() { + for pid in "${PIDS[@]:-}"; do + if [[ -n "${pid}" ]] && kill -0 "${pid}" 2>/dev/null; then + kill "${pid}" 2>/dev/null || true + wait "${pid}" 2>/dev/null || true + fi + done +} +trap cleanup EXIT +PIDS=() + +set +u +source /opt/ros/humble/setup.bash +source /home/ssu/ros2_ws/install/setup.bash +source "${ROOT_DIR}/install/setup.bash" +set -u + +# Kill only old RViz-only demo joint publishers so there is exactly one /joint_states source. +pkill -f 'm0609_shake_joint_state_node' 2>/dev/null || true +pkill -f 'side_grasp_ik_preview_node' 2>/dev/null || true + +ros2 launch azas_bringup hardware_free_demo.launch.py \ + use_rviz:=false \ + use_robot_urdf:=true \ + robot_color:="${ROBOT_COLOR}" \ + enable_ik_preview:=false \ + run_live_stt:=false \ + run_recipe_mapper:=false \ + use_llm:=false \ + show_sequence_markers:=false \ + show_dispenser_markers:=false \ + show_animated_cup:=false \ + show_demo_arm:=false \ + >"${LOG_DIR}/smooth_robot_description.log" 2>&1 & +PIDS+=("$!") + +ros2 run azas_motion m0609_shake_joint_state_node \ + --ros-args \ + -p publish_rate:="${PUBLISH_RATE}" \ + -p shake_cycles_per_second:="${SHAKE_CYCLES_PER_SECOND}" \ + -p preview_mode:="${PREVIEW_MODE}" \ + -p loop_motion:="${LOOP_MOTION}" \ + >"${LOG_DIR}/smooth_robot_joint_states.log" 2>&1 & +PIDS+=("$!") + +rviz2 -d "${RVIZ_CONFIG}" >"${LOG_DIR}/smooth_robot_rviz.log" 2>&1 & +PIDS+=("$!") + +echo "[Azas] Smooth orange robot motion is running in RViz." +echo "[Azas] Visual source: robot_state_publisher + one /joint_states publisher. No path display, no controller simulation." +echo "[Azas] Logs: ${LOG_DIR}/smooth_robot_*.log" +wait From 70bbbca268dc068491ab8b8e2401e6477d7f1645 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sun, 7 Jun 2026 19:34:11 +0900 Subject: [PATCH 14/88] Model dispenser and gripper occupancy conservatively Merge the four measured nozzle/head boxes into one upright cuboid so the empty gaps between outlets are also treated as occupied during planning review. Add a link_6-attached RG2-style collision envelope so MoveIt previews no longer assume an empty flange. Constraint: Do not invent cup coordinates or measured calibration values; derive nozzle span only from existing measured nozzle boxes and keep real-motion enable flags false. Rejected: Keeping four tilted nozzle boxes | it left gaps and orientation artifacts the operator explicitly wanted removed. Confidence: high Scope-risk: moderate Directive: Keep the gripper envelope synchronized with rg2_link6_tcp.urdf.xacro if dimensions change. Tested: python3 -m py_compile for changed Python/launch files; YAML merged-block assertion; xacro rg2_link6_tcp.urdf.xacro; colcon build --packages-select azas_motion azas_bringup --symlink-install; ros2 run azas_motion link6_gripper_collision_node --ros-args -p publish_once:=true; ros2 launch azas_bringup rg2_link6_tcp.launch.py --show-args; python3 tools/checks/check_measured_dispenser_geometry.py Not-tested: live robot motion and physical collision clearance. --- .../config/measured_dispenser_collision.yaml | 80 +++------ .../launch/rg2_link6_tcp.launch.py | 9 ++ .../urdf/rg2_link6_tcp.urdf.xacro | 70 ++++++-- .../link6_gripper_collision_node.py | 153 ++++++++++++++++++ ...measured_dispenser_collision_scene_node.py | 1 + src/azas_motion/setup.py | 1 + .../config/measured_dispenser_collision.yaml | 80 +++------ 7 files changed, 258 insertions(+), 136 deletions(-) create mode 100644 src/azas_motion/azas_motion/link6_gripper_collision_node.py diff --git a/src/azas_bringup/config/measured_dispenser_collision.yaml b/src/azas_bringup/config/measured_dispenser_collision.yaml index 43a24ec..4455bae 100644 --- a/src/azas_bringup/config/measured_dispenser_collision.yaml +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml @@ -2,13 +2,15 @@ metadata: frame_id: base_link measured_target_frame: link_6 source: operator_teaching_tf2_echo - status: measured_draft_single_box_not_enabled + status: measured_draft_merged_vertical_nozzle_block_not_enabled body_bottom_z_m: 0.0 body_bottom_reason: dispenser bottles start on same floor plane as robot base margin_m: x: 0.02 y: 0.02 z: 0.02 + nozzle_merge_policy: four tilted nozzle/head boxes are represented as one upright + axis-aligned cuboid spanning all nozzle lanes and the empty gaps between them front_hold_poses: dispenser_1: position_xyz_m: @@ -123,77 +125,33 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - dispenser_1_head_nozzle_box: + dispenser_head_nozzle_merged_vertical_box: type: box frame_id: base_link center_xyz_m: - 0.718 - - 0.084 + - 0.017 - 0.432 size_xyz_m: - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - -0.1368 - - 0.9906 - publish_to_planning_scene: true - enabled_for_real_motion: false - '# note': x is aligned to the front face of dispenser_combined_body_box; z/y keep - measured lane/height. - dispenser_2_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - 0.043 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - -0.1368 - - 0.9906 - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_3_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - -0.002 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 + - 0.144 - 0.04 + bounds_xyz_m: + min: + - 0.693 + - -0.055 + - 0.412 + max: + - 0.743 + - 0.089 + - 0.452 orientation_xyzw: - 0.0 - 0.0 - - -0.1368 - - 0.9906 - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_4_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - -0.05 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - 0.0 - - 0.0 - - -0.1368 - - 0.9906 + - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false + '# note': Merged from measured dispenser_1..4 nozzle/head boxes; yaw was intentionally + straightened to vertical/base_link axes and gaps between outlets are occupied + by this single long cuboid for conservative planning review. diff --git a/src/azas_bringup/launch/rg2_link6_tcp.launch.py b/src/azas_bringup/launch/rg2_link6_tcp.launch.py index d75177d..c562bc0 100644 --- a/src/azas_bringup/launch/rg2_link6_tcp.launch.py +++ b/src/azas_bringup/launch/rg2_link6_tcp.launch.py @@ -1,5 +1,6 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue @@ -28,6 +29,7 @@ def generate_launch_description(): [ DeclareLaunchArgument("open_tcp_offset_m", default_value="0.15"), DeclareLaunchArgument("closed_tcp_offset_m", default_value="0.25"), + DeclareLaunchArgument("publish_gripper_collision", default_value="true"), Node( package="robot_state_publisher", executable="robot_state_publisher", @@ -38,5 +40,12 @@ def generate_launch_description(): ], parameters=[{"robot_description": robot_description}], ), + Node( + package="azas_motion", + executable="link6_gripper_collision_node", + name="link6_gripper_collision_node", + output="screen", + condition=IfCondition(LaunchConfiguration("publish_gripper_collision")), + ), ] ) diff --git a/src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro b/src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro index 7a8ffa3..b2513e2 100644 --- a/src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro +++ b/src/azas_bringup/urdf/rg2_link6_tcp.urdf.xacro @@ -6,6 +6,9 @@ + + + @@ -27,16 +30,29 @@ - + - + + + + + + + + - + + + + + + + - + @@ -44,21 +60,34 @@ - + - + + + + + + + + - + - + - + + + + + + + @@ -66,21 +95,34 @@ - + - + - + + + + + + + + - + + + + + + + - + diff --git a/src/azas_motion/azas_motion/link6_gripper_collision_node.py b/src/azas_motion/azas_motion/link6_gripper_collision_node.py new file mode 100644 index 0000000..e3cb7d7 --- /dev/null +++ b/src/azas_motion/azas_motion/link6_gripper_collision_node.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python3 +"""Publish a conservative RG2-style gripper envelope attached to link_6. + +The Doosan model in this project often exposes only the robot flange, so +planning previews can behave as if no gripper occupies space. This node adds a +fixed, link_6-relative collision envelope matching the supplemental +``rg2_link6_tcp.urdf.xacro`` preview. It does not create robot poses or +calibration values; all geometry is local to link_6. +""" + +from __future__ import annotations + +import rclpy +from geometry_msgs.msg import Pose +from moveit_msgs.msg import AttachedCollisionObject, CollisionObject +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from shape_msgs.msg import SolidPrimitive +from std_msgs.msg import Header + + +def transient_qos(depth: int = 10) -> QoSProfile: + return QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=depth, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + + +def make_pose(xyz: tuple[float, float, float]) -> Pose: + pose = Pose() + pose.position.x = xyz[0] + pose.position.y = xyz[1] + pose.position.z = xyz[2] + pose.orientation.w = 1.0 + return pose + + +def box(size_xyz: tuple[float, float, float]) -> SolidPrimitive: + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.BOX + primitive.dimensions = [float(value) for value in size_xyz] + return primitive + + +def cylinder_z(height_m: float, radius_m: float) -> SolidPrimitive: + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.CYLINDER + primitive.dimensions = [float(height_m), float(radius_m)] + return primitive + + +class Link6GripperCollisionNode(Node): + def __init__(self) -> None: + super().__init__("link6_gripper_collision_node") + self.declare_parameter("object_id", "azas_rg2_gripper_on_link6") + self.declare_parameter("attached_link_name", "link_6") + self.declare_parameter( + "touch_links", + [ + "link_6", + "GripperDA_v1_jarvis", + "rg2_left_finger_visual", + "rg2_right_finger_visual", + "rg2_open_tcp", + "rg2_closed_tcp", + "dispenser_press_tcp", + ], + ) + self.declare_parameter("publish_period_sec", 1.0) + self.declare_parameter("publish_once", False) + + self.publisher = self.create_publisher( + AttachedCollisionObject, + "/attached_collision_object", + transient_qos(), + ) + self._logged = False + self._publish() + + period = float(self.get_parameter("publish_period_sec").value) + if not bool(self.get_parameter("publish_once").value): + self.timer = self.create_timer(max(period, 0.2), self._publish) + + def _attached_object(self) -> AttachedCollisionObject: + link_name = str(self.get_parameter("attached_link_name").value) + attached = AttachedCollisionObject() + attached.link_name = link_name + attached.touch_links = [str(item) for item in self.get_parameter("touch_links").value] + + obj = CollisionObject() + obj.id = str(self.get_parameter("object_id").value) + obj.header = Header() + obj.header.frame_id = link_name + obj.header.stamp = self.get_clock().now().to_msg() + obj.operation = CollisionObject.ADD + + # Same envelope as rg2_link6_tcp.urdf.xacro: + # flange/mount cylinder, palm, two long fingers, and inward blue pads. + obj.primitives.extend( + [ + cylinder_z(0.050, 0.040), + box((0.090, 0.140, 0.050)), + box((0.035, 0.018, 0.160)), + box((0.035, 0.018, 0.160)), + box((0.025, 0.012, 0.035)), + box((0.025, 0.012, 0.035)), + ] + ) + obj.primitive_poses.extend( + [ + make_pose((0.0, 0.0, 0.025)), + make_pose((0.0, 0.0, 0.075)), + make_pose((0.0, 0.055, 0.155)), + make_pose((0.0, -0.055, 0.155)), + make_pose((0.0, 0.040, 0.245)), + make_pose((0.0, -0.040, 0.245)), + ] + ) + attached.object = obj + return attached + + def _publish(self) -> None: + self.publisher.publish(self._attached_object()) + if not self._logged: + self.get_logger().info( + "Publishing RG2-style attached collision envelope on link_6" + ) + self._logged = True + + +def main(args: list[str] | None = None) -> None: + rclpy.init(args=args) + node = Link6GripperCollisionNode() + try: + if bool(node.get_parameter("publish_once").value): + import time + + time.sleep(0.35) + return + rclpy.spin(node) + except (ExternalShutdownException, KeyboardInterrupt): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py index 75d6c3c..914f311 100644 --- a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py +++ b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py @@ -26,6 +26,7 @@ "dispenser_3_body_box_v2", "dispenser_4_body_box_v2", "dispenser_head_box", + "dispenser_head_nozzle_merged_vertical_box", "dispenser_1_head_nozzle_box", "dispenser_2_head_nozzle_box", "dispenser_3_head_nozzle_box", diff --git a/src/azas_motion/setup.py b/src/azas_motion/setup.py index 17cd7c3..1bb032f 100644 --- a/src/azas_motion/setup.py +++ b/src/azas_motion/setup.py @@ -33,6 +33,7 @@ "doosan_moveit_grasped_tumbler_to_dispenser_node = azas_motion.doosan_moveit_grasped_tumbler_to_dispenser_node:main", "gear_assembly_legacy = azas_motion.gear_assembly_legacy:main", "m0609_shake_joint_state_node = azas_motion.m0609_shake_joint_state_node:main", + "link6_gripper_collision_node = azas_motion.link6_gripper_collision_node:main", "dispenser_press_cycle_moveit_node = azas_motion.dispenser_press_cycle_moveit_node:main", "measured_dispenser_collision_scene_node = azas_motion.measured_dispenser_collision_scene_node:main", "mp_basic_legacy = azas_motion.mp_basic_legacy:main", diff --git a/src/azas_perception/config/measured_dispenser_collision.yaml b/src/azas_perception/config/measured_dispenser_collision.yaml index 43a24ec..4455bae 100644 --- a/src/azas_perception/config/measured_dispenser_collision.yaml +++ b/src/azas_perception/config/measured_dispenser_collision.yaml @@ -2,13 +2,15 @@ metadata: frame_id: base_link measured_target_frame: link_6 source: operator_teaching_tf2_echo - status: measured_draft_single_box_not_enabled + status: measured_draft_merged_vertical_nozzle_block_not_enabled body_bottom_z_m: 0.0 body_bottom_reason: dispenser bottles start on same floor plane as robot base margin_m: x: 0.02 y: 0.02 z: 0.02 + nozzle_merge_policy: four tilted nozzle/head boxes are represented as one upright + axis-aligned cuboid spanning all nozzle lanes and the empty gaps between them front_hold_poses: dispenser_1: position_xyz_m: @@ -123,77 +125,33 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - dispenser_1_head_nozzle_box: + dispenser_head_nozzle_merged_vertical_box: type: box frame_id: base_link center_xyz_m: - 0.718 - - 0.084 + - 0.017 - 0.432 size_xyz_m: - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - -0.1368 - - 0.9906 - publish_to_planning_scene: true - enabled_for_real_motion: false - '# note': x is aligned to the front face of dispenser_combined_body_box; z/y keep - measured lane/height. - dispenser_2_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - 0.043 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - -0.1368 - - 0.9906 - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_3_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - -0.002 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 + - 0.144 - 0.04 + bounds_xyz_m: + min: + - 0.693 + - -0.055 + - 0.412 + max: + - 0.743 + - 0.089 + - 0.452 orientation_xyzw: - 0.0 - 0.0 - - -0.1368 - - 0.9906 - publish_to_planning_scene: true - enabled_for_real_motion: false - dispenser_4_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - -0.05 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - 0.0 - - 0.0 - - -0.1368 - - 0.9906 + - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false + '# note': Merged from measured dispenser_1..4 nozzle/head boxes; yaw was intentionally + straightened to vertical/base_link axes and gaps between outlets are occupied + by this single long cuboid for conservative planning review. From 862187ff938040e0a303764fc29c3b0b16f9ea59 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sun, 7 Jun 2026 19:48:34 +0900 Subject: [PATCH 15/88] Restore visible horizontal dispenser spouts Replace the merged vertical nozzle block with four separate horizontal spout boxes so RViz shows each outlet and leaves inter-outlet gaps visible again. Constraint: Keep measured calibration and outlet/press coordinates unchanged; restore geometry from the pre-merge measured draft instead of inventing new robot coordinates. Rejected: Single conservative merged vertical block | it hid the physical spout shape and overfilled the gaps between outlets. Confidence: high Scope-risk: narrow Directive: Use separate visual/collision objects when operator review depends on seeing individual dispenser nozzles. Tested: YAML assertion for no merged block and four horizontal nozzle boxes in bringup/perception configs; python3 tools/checks/check_measured_dispenser_geometry.py; colcon build --packages-select azas_bringup azas_motion --symlink-install; timeout 4s ros2 run azas_motion measured_dispenser_collision_scene_node loaded and published dispenser_1..4_head_nozzle_box. Not-tested: full RViz visual inspection and live robot motion. --- .../config/measured_dispenser_collision.yaml | 88 +++++++++++++++---- .../config/measured_dispenser_collision.yaml | 88 +++++++++++++++---- 2 files changed, 140 insertions(+), 36 deletions(-) diff --git a/src/azas_bringup/config/measured_dispenser_collision.yaml b/src/azas_bringup/config/measured_dispenser_collision.yaml index 4455bae..166ee60 100644 --- a/src/azas_bringup/config/measured_dispenser_collision.yaml +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml @@ -2,15 +2,16 @@ metadata: frame_id: base_link measured_target_frame: link_6 source: operator_teaching_tf2_echo - status: measured_draft_merged_vertical_nozzle_block_not_enabled + status: measured_draft_horizontal_spout_boxes_not_enabled body_bottom_z_m: 0.0 body_bottom_reason: dispenser bottles start on same floor plane as robot base margin_m: x: 0.02 y: 0.02 z: 0.02 - nozzle_merge_policy: four tilted nozzle/head boxes are represented as one upright - axis-aligned cuboid spanning all nozzle lanes and the empty gaps between them + nozzle_model_policy: four outlets are visualized as separate horizontal x-axis spout + boxes; nozzle lane/height are restored from the measured draft boxes and gaps + are left open front_hold_poses: dispenser_1: position_xyz_m: @@ -125,26 +126,78 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - dispenser_head_nozzle_merged_vertical_box: + dispenser_1_head_nozzle_box: type: box frame_id: base_link center_xyz_m: - 0.718 - - 0.017 + - 0.084 - 0.432 size_xyz_m: - 0.05 - - 0.144 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# note': Restored as a separate horizontal spout box from the pre-merge measured + draft; x span protrudes from the dispenser body front face and inter-outlet + gaps remain visible/open. + dispenser_2_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - 0.043 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# note': Restored separate horizontal spout box; gaps between outlets remain + visible/open. + dispenser_3_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - -0.002 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# note': Restored separate horizontal spout box; gaps between outlets remain + visible/open. + dispenser_4_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - -0.05 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 - 0.04 - bounds_xyz_m: - min: - - 0.693 - - -0.055 - - 0.412 - max: - - 0.743 - - 0.089 - - 0.452 orientation_xyzw: - 0.0 - 0.0 @@ -152,6 +205,5 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - '# note': Merged from measured dispenser_1..4 nozzle/head boxes; yaw was intentionally - straightened to vertical/base_link axes and gaps between outlets are occupied - by this single long cuboid for conservative planning review. + '# note': Restored separate horizontal spout box; gaps between outlets remain + visible/open. diff --git a/src/azas_perception/config/measured_dispenser_collision.yaml b/src/azas_perception/config/measured_dispenser_collision.yaml index 4455bae..166ee60 100644 --- a/src/azas_perception/config/measured_dispenser_collision.yaml +++ b/src/azas_perception/config/measured_dispenser_collision.yaml @@ -2,15 +2,16 @@ metadata: frame_id: base_link measured_target_frame: link_6 source: operator_teaching_tf2_echo - status: measured_draft_merged_vertical_nozzle_block_not_enabled + status: measured_draft_horizontal_spout_boxes_not_enabled body_bottom_z_m: 0.0 body_bottom_reason: dispenser bottles start on same floor plane as robot base margin_m: x: 0.02 y: 0.02 z: 0.02 - nozzle_merge_policy: four tilted nozzle/head boxes are represented as one upright - axis-aligned cuboid spanning all nozzle lanes and the empty gaps between them + nozzle_model_policy: four outlets are visualized as separate horizontal x-axis spout + boxes; nozzle lane/height are restored from the measured draft boxes and gaps + are left open front_hold_poses: dispenser_1: position_xyz_m: @@ -125,26 +126,78 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - dispenser_head_nozzle_merged_vertical_box: + dispenser_1_head_nozzle_box: type: box frame_id: base_link center_xyz_m: - 0.718 - - 0.017 + - 0.084 - 0.432 size_xyz_m: - 0.05 - - 0.144 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# note': Restored as a separate horizontal spout box from the pre-merge measured + draft; x span protrudes from the dispenser body front face and inter-outlet + gaps remain visible/open. + dispenser_2_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - 0.043 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# note': Restored separate horizontal spout box; gaps between outlets remain + visible/open. + dispenser_3_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - -0.002 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 + - 0.04 + orientation_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + publish_to_planning_scene: true + enabled_for_real_motion: false + '# note': Restored separate horizontal spout box; gaps between outlets remain + visible/open. + dispenser_4_head_nozzle_box: + type: box + frame_id: base_link + center_xyz_m: + - 0.718 + - -0.05 + - 0.432 + size_xyz_m: + - 0.05 + - 0.01 - 0.04 - bounds_xyz_m: - min: - - 0.693 - - -0.055 - - 0.412 - max: - - 0.743 - - 0.089 - - 0.452 orientation_xyzw: - 0.0 - 0.0 @@ -152,6 +205,5 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - '# note': Merged from measured dispenser_1..4 nozzle/head boxes; yaw was intentionally - straightened to vertical/base_link axes and gaps between outlets are occupied - by this single long cuboid for conservative planning review. + '# note': Restored separate horizontal spout box; gaps between outlets remain + visible/open. From 6c0149f690441d9f83040aa44c730a060295c7f6 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sun, 7 Jun 2026 20:28:01 +0900 Subject: [PATCH 16/88] Merge dispenser nozzles into one horizontal spout block Represent the four measured nozzle lanes as one horizontal cuboid so RViz shows a single long outlet block spanning the four nozzles and the gaps between them. Constraint: Keep calibration outlet and press poses unchanged; derive the merged block only from the existing measured nozzle boxes. Rejected: Four separate nozzle boxes | user wanted the nozzle area combined into one block, not individually visible. Confidence: high Scope-risk: narrow Directive: Do not turn this into a vertical column; the merged nozzle must remain a horizontal spout block. Tested: YAML assertion for one merged horizontal spout and no per-nozzle boxes; python3 -m py_compile measured_dispenser_collision_scene_node.py; python3 tools/checks/check_measured_dispenser_geometry.py; colcon build --packages-select azas_bringup azas_motion --symlink-install; timeout 4s ros2 run azas_motion measured_dispenser_collision_scene_node published dispenser_head_nozzle_merged_horizontal_spout_box. Not-tested: full RViz visual inspection and live robot motion. --- .../config/measured_dispenser_collision.yaml | 89 ++++--------------- ...measured_dispenser_collision_scene_node.py | 1 + .../config/measured_dispenser_collision.yaml | 89 ++++--------------- 3 files changed, 39 insertions(+), 140 deletions(-) diff --git a/src/azas_bringup/config/measured_dispenser_collision.yaml b/src/azas_bringup/config/measured_dispenser_collision.yaml index 166ee60..c13c86f 100644 --- a/src/azas_bringup/config/measured_dispenser_collision.yaml +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml @@ -2,16 +2,16 @@ metadata: frame_id: base_link measured_target_frame: link_6 source: operator_teaching_tf2_echo - status: measured_draft_horizontal_spout_boxes_not_enabled + status: measured_draft_merged_horizontal_spout_block_not_enabled body_bottom_z_m: 0.0 body_bottom_reason: dispenser bottles start on same floor plane as robot base margin_m: x: 0.02 y: 0.02 z: 0.02 - nozzle_model_policy: four outlets are visualized as separate horizontal x-axis spout - boxes; nozzle lane/height are restored from the measured draft boxes and gaps - are left open + nozzle_merge_policy: four nozzle lanes are represented as one horizontal x-axis + spout cuboid; the block spans all four outlet positions and fills the gaps between + them front_hold_poses: dispenser_1: position_xyz_m: @@ -126,78 +126,26 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - dispenser_1_head_nozzle_box: + dispenser_head_nozzle_merged_horizontal_spout_box: type: box frame_id: base_link center_xyz_m: - 0.718 - - 0.084 + - 0.017 - 0.432 size_xyz_m: - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - 0.0 - - 1.0 - publish_to_planning_scene: true - enabled_for_real_motion: false - '# note': Restored as a separate horizontal spout box from the pre-merge measured - draft; x span protrudes from the dispenser body front face and inter-outlet - gaps remain visible/open. - dispenser_2_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - 0.043 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - 0.0 - - 1.0 - publish_to_planning_scene: true - enabled_for_real_motion: false - '# note': Restored separate horizontal spout box; gaps between outlets remain - visible/open. - dispenser_3_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - -0.002 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - 0.0 - - 1.0 - publish_to_planning_scene: true - enabled_for_real_motion: false - '# note': Restored separate horizontal spout box; gaps between outlets remain - visible/open. - dispenser_4_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - -0.05 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 + - 0.144 - 0.04 + bounds_xyz_m: + min: + - 0.693 + - -0.055 + - 0.412 + max: + - 0.743 + - 0.089 + - 0.452 orientation_xyzw: - 0.0 - 0.0 @@ -205,5 +153,6 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - '# note': Restored separate horizontal spout box; gaps between outlets remain - visible/open. + '# note': Merged from the four measured nozzle lane boxes into one horizontal + spout block. This intentionally fills the empty spaces between outlets while + keeping the spout as a horizontal cuboid, not a vertical column. diff --git a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py index 914f311..1b02dab 100644 --- a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py +++ b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py @@ -27,6 +27,7 @@ "dispenser_4_body_box_v2", "dispenser_head_box", "dispenser_head_nozzle_merged_vertical_box", + "dispenser_head_nozzle_merged_horizontal_spout_box", "dispenser_1_head_nozzle_box", "dispenser_2_head_nozzle_box", "dispenser_3_head_nozzle_box", diff --git a/src/azas_perception/config/measured_dispenser_collision.yaml b/src/azas_perception/config/measured_dispenser_collision.yaml index 166ee60..c13c86f 100644 --- a/src/azas_perception/config/measured_dispenser_collision.yaml +++ b/src/azas_perception/config/measured_dispenser_collision.yaml @@ -2,16 +2,16 @@ metadata: frame_id: base_link measured_target_frame: link_6 source: operator_teaching_tf2_echo - status: measured_draft_horizontal_spout_boxes_not_enabled + status: measured_draft_merged_horizontal_spout_block_not_enabled body_bottom_z_m: 0.0 body_bottom_reason: dispenser bottles start on same floor plane as robot base margin_m: x: 0.02 y: 0.02 z: 0.02 - nozzle_model_policy: four outlets are visualized as separate horizontal x-axis spout - boxes; nozzle lane/height are restored from the measured draft boxes and gaps - are left open + nozzle_merge_policy: four nozzle lanes are represented as one horizontal x-axis + spout cuboid; the block spans all four outlet positions and fills the gaps between + them front_hold_poses: dispenser_1: position_xyz_m: @@ -126,78 +126,26 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - dispenser_1_head_nozzle_box: + dispenser_head_nozzle_merged_horizontal_spout_box: type: box frame_id: base_link center_xyz_m: - 0.718 - - 0.084 + - 0.017 - 0.432 size_xyz_m: - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - 0.0 - - 1.0 - publish_to_planning_scene: true - enabled_for_real_motion: false - '# note': Restored as a separate horizontal spout box from the pre-merge measured - draft; x span protrudes from the dispenser body front face and inter-outlet - gaps remain visible/open. - dispenser_2_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - 0.043 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - 0.0 - - 1.0 - publish_to_planning_scene: true - enabled_for_real_motion: false - '# note': Restored separate horizontal spout box; gaps between outlets remain - visible/open. - dispenser_3_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - -0.002 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 - - 0.04 - orientation_xyzw: - - 0.0 - - 0.0 - - 0.0 - - 1.0 - publish_to_planning_scene: true - enabled_for_real_motion: false - '# note': Restored separate horizontal spout box; gaps between outlets remain - visible/open. - dispenser_4_head_nozzle_box: - type: box - frame_id: base_link - center_xyz_m: - - 0.718 - - -0.05 - - 0.432 - size_xyz_m: - - 0.05 - - 0.01 + - 0.144 - 0.04 + bounds_xyz_m: + min: + - 0.693 + - -0.055 + - 0.412 + max: + - 0.743 + - 0.089 + - 0.452 orientation_xyzw: - 0.0 - 0.0 @@ -205,5 +153,6 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - '# note': Restored separate horizontal spout box; gaps between outlets remain - visible/open. + '# note': Merged from the four measured nozzle lane boxes into one horizontal + spout block. This intentionally fills the empty spaces between outlets while + keeping the spout as a horizontal cuboid, not a vertical column. From 110dfc7b917961d39c55cc5802ebed41313edd24 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sun, 7 Jun 2026 20:51:45 +0900 Subject: [PATCH 17/88] Remove stale workspace walls for dispenser course path The dispenser press course path was failing before the press-contact move because a side_grip workspace wall collided with link_2, unrelated to the dispenser nozzle geometry. Have the course script request removal of those wall objects before planning while keeping the measured dispenser body and merged horizontal spout visible. Constraint: Preserve measured dispenser calibration and do not relax dispenser collision geometry. Rejected: Changing nozzle geometry again | the reported collision was side_grip_workspace_x_min_wall vs link_2, not a nozzle/body contact. Confidence: high Scope-risk: narrow Directive: Keep REMOVE_COURSE_WORKSPACE_WALLS enabled for this RViz course script unless the workspace walls are revalidated for the full manipulator sweep. Tested: python3 -m py_compile measured_dispenser_collision_scene_node.py; bash -n run_course_dispenser_press_cycle_rviz.sh; python3 tools/checks/check_measured_dispenser_geometry.py; colcon build --packages-select azas_motion azas_bringup --symlink-install; timeout 4s ros2 run azas_motion measured_dispenser_collision_scene_node --ros-args -p remove_course_workspace_collision_objects:=true logged removal of side_grip_workspace_* walls and published dispenser_head_nozzle_merged_horizontal_spout_box. Not-tested: full dispenser press cycle execution in RViz after wall removal. --- ...measured_dispenser_collision_scene_node.py | 23 ++++++++++++++++++- .../run_course_dispenser_press_cycle_rviz.sh | 3 +++ 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py index 1b02dab..856d01d 100644 --- a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py +++ b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py @@ -34,6 +34,13 @@ "dispenser_4_head_nozzle_box", ) +COURSE_WORKSPACE_COLLISION_OBJECT_IDS = ( + "side_grip_workspace_x_min_wall", + "side_grip_workspace_x_max_wall", + "side_grip_workspace_y_min_wall", + "side_grip_workspace_y_max_wall", +) + def transient_qos(depth: int = 10) -> QoSProfile: return QoSProfile( @@ -128,6 +135,7 @@ def __init__(self) -> None: self.declare_parameter("publish_rviz_visual_tools_compat", True) self.declare_parameter("publish_debug_labels", True) self.declare_parameter("remove_legacy_collision_objects", True) + self.declare_parameter("remove_course_workspace_collision_objects", False) self.declare_parameter("clear_markers_before_publish", True) config_path = Path( @@ -167,6 +175,11 @@ def __init__(self) -> None: .get_parameter_value() .bool_value ) + self.remove_course_workspace_collision_objects = ( + self.get_parameter("remove_course_workspace_collision_objects") + .get_parameter_value() + .bool_value + ) self.clear_markers_before_publish = ( self.get_parameter("clear_markers_before_publish") .get_parameter_value() @@ -240,9 +253,17 @@ def _publish_scene(self) -> None: self.remove_legacy_collision_objects and not self._legacy_collision_objects_removed ): - for object_id in LEGACY_DISPENSER_COLLISION_OBJECT_IDS: + remove_ids = list(LEGACY_DISPENSER_COLLISION_OBJECT_IDS) + if self.remove_course_workspace_collision_objects: + remove_ids.extend(COURSE_WORKSPACE_COLLISION_OBJECT_IDS) + for object_id in remove_ids: self.collision_pub.publish(self._make_remove_collision_object(object_id)) self._legacy_collision_objects_removed = True + if self.remove_course_workspace_collision_objects: + self.get_logger().info( + "Requested removal of course workspace wall collision objects: " + + ", ".join(COURSE_WORKSPACE_COLLISION_OBJECT_IDS) + ) published_ids = [] for object_id, object_config in collision_objects.items(): if not object_config.get("publish_to_planning_scene", True): diff --git a/tools/run/run_course_dispenser_press_cycle_rviz.sh b/tools/run/run_course_dispenser_press_cycle_rviz.sh index 91d6044..0cfd9da 100755 --- a/tools/run/run_course_dispenser_press_cycle_rviz.sh +++ b/tools/run/run_course_dispenser_press_cycle_rviz.sh @@ -26,6 +26,7 @@ DISPENSER_COLLISION_ENABLED="${DISPENSER_COLLISION_ENABLED:-1}" # Keep markers visible in RViz by default, but do not feed this draft body box into # MoveIt collision checking for the press stroke unless explicitly requested. DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS:-1}" +REMOVE_COURSE_WORKSPACE_WALLS="${REMOVE_COURSE_WORKSPACE_WALLS:-1}" DISPENSER_COLLISION_CONFIG="${DISPENSER_COLLISION_CONFIG:-${ROOT_DIR}/install/azas_bringup/share/azas_bringup/config/measured_dispenser_collision.yaml}" if [[ ! -f "${DISPENSER_COLLISION_CONFIG}" ]]; then DISPENSER_COLLISION_CONFIG="${ROOT_DIR}/src/azas_bringup/config/measured_dispenser_collision.yaml" @@ -109,11 +110,13 @@ if [[ "${DISPENSER_COLLISION_ENABLED}" == "1" || "${DISPENSER_COLLISION_ENABLED} -p config_path:="${DISPENSER_COLLISION_CONFIG}" \ -p publish_period_sec:=1.0 \ -p publish_collision_objects:="${DISPENSER_COLLISION_OBJECTS_BOOL}" \ + -p remove_course_workspace_collision_objects:="${REMOVE_COURSE_WORKSPACE_WALLS}" \ -p publish_markers:=true \ >"${LOG_DIR}/measured_dispenser_collision_scene.log" 2>&1 & PIDS+=("$!") echo "[Azas] Dispenser combined box represents bottle/body only; press pre/contact is derived from press_contact_joints_deg FK, not from this box." echo "[Azas] DISPENSER_COLLISION_OBJECTS=${DISPENSER_COLLISION_OBJECTS} (1=add to MoveIt collision scene, 0=RViz markers only)." + echo "[Azas] REMOVE_COURSE_WORKSPACE_WALLS=${REMOVE_COURSE_WORKSPACE_WALLS} (1=remove stale side_grip_workspace_* walls that can collide with link_2 in this course path)." sleep 2 if [[ "${DISPENSER_COLLISION_OBJECTS}" == "1" || "${DISPENSER_COLLISION_OBJECTS}" == "true" ]]; then timeout 8 ros2 topic echo /collision_object >"${LOG_DIR}/collision_object_samples.txt" 2>/dev/null || true From d882f9df393834061906f4db78c6e9c1f5f094a9 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sun, 7 Jun 2026 21:11:52 +0900 Subject: [PATCH 18/88] Align merged nozzle block with dispenser body Lengthen the merged horizontal nozzle block to the full Y span of dispenser_combined_body_box and keep its orientation identical to the combined body so the two boxes are axis-aligned in RViz. Constraint: Preserve measured calibration outlet and press poses; only adjust draft collision/visual geometry. Rejected: Keeping the shorter four-nozzle span | user requested the combined portion be longer and angle-aligned with the combined box. Confidence: high Scope-risk: narrow Directive: Keep the merged nozzle block horizontal and aligned with dispenser_combined_body_box unless new measured geometry supersedes it. Tested: YAML assertions that nozzle Y center/size/bounds and orientation match dispenser_combined_body_box; python3 tools/checks/check_measured_dispenser_geometry.py; colcon build --packages-select azas_bringup --symlink-install; timeout 4s ros2 run azas_motion measured_dispenser_collision_scene_node --ros-args -p remove_course_workspace_collision_objects:=true. Not-tested: full RViz visual inspection and live robot motion. --- .../config/measured_dispenser_collision.yaml | 19 +++++++++---------- .../config/measured_dispenser_collision.yaml | 19 +++++++++---------- 2 files changed, 18 insertions(+), 20 deletions(-) diff --git a/src/azas_bringup/config/measured_dispenser_collision.yaml b/src/azas_bringup/config/measured_dispenser_collision.yaml index c13c86f..d749077 100644 --- a/src/azas_bringup/config/measured_dispenser_collision.yaml +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml @@ -9,9 +9,8 @@ metadata: x: 0.02 y: 0.02 z: 0.02 - nozzle_merge_policy: four nozzle lanes are represented as one horizontal x-axis - spout cuboid; the block spans all four outlet positions and fills the gaps between - them + nozzle_merge_policy: single horizontal spout cuboid spans the combined body Y width + and uses the same orientation as dispenser_combined_body_box front_hold_poses: dispenser_1: position_xyz_m: @@ -131,20 +130,20 @@ estimated_collision_objects: frame_id: base_link center_xyz_m: - 0.718 - - 0.017 + - -0.0155 - 0.432 size_xyz_m: - 0.05 - - 0.144 + - 0.215 - 0.04 bounds_xyz_m: min: - 0.693 - - -0.055 + - -0.123 - 0.412 max: - 0.743 - - 0.089 + - 0.092 - 0.452 orientation_xyzw: - 0.0 @@ -153,6 +152,6 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - '# note': Merged from the four measured nozzle lane boxes into one horizontal - spout block. This intentionally fills the empty spaces between outlets while - keeping the spout as a horizontal cuboid, not a vertical column. + '# note': Merged horizontal nozzle/spout block lengthened to match the dispenser_combined_body_box + Y span; orientation is kept identical to the combined body box so both blocks + are axis-aligned together. diff --git a/src/azas_perception/config/measured_dispenser_collision.yaml b/src/azas_perception/config/measured_dispenser_collision.yaml index c13c86f..d749077 100644 --- a/src/azas_perception/config/measured_dispenser_collision.yaml +++ b/src/azas_perception/config/measured_dispenser_collision.yaml @@ -9,9 +9,8 @@ metadata: x: 0.02 y: 0.02 z: 0.02 - nozzle_merge_policy: four nozzle lanes are represented as one horizontal x-axis - spout cuboid; the block spans all four outlet positions and fills the gaps between - them + nozzle_merge_policy: single horizontal spout cuboid spans the combined body Y width + and uses the same orientation as dispenser_combined_body_box front_hold_poses: dispenser_1: position_xyz_m: @@ -131,20 +130,20 @@ estimated_collision_objects: frame_id: base_link center_xyz_m: - 0.718 - - 0.017 + - -0.0155 - 0.432 size_xyz_m: - 0.05 - - 0.144 + - 0.215 - 0.04 bounds_xyz_m: min: - 0.693 - - -0.055 + - -0.123 - 0.412 max: - 0.743 - - 0.089 + - 0.092 - 0.452 orientation_xyzw: - 0.0 @@ -153,6 +152,6 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - '# note': Merged from the four measured nozzle lane boxes into one horizontal - spout block. This intentionally fills the empty spaces between outlets while - keeping the spout as a horizontal cuboid, not a vertical column. + '# note': Merged horizontal nozzle/spout block lengthened to match the dispenser_combined_body_box + Y span; orientation is kept identical to the combined body box so both blocks + are axis-aligned together. From 5bdfdd167a707b5e4351d754642b3136d61800d7 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sun, 7 Jun 2026 21:33:28 +0900 Subject: [PATCH 19/88] Show link6 gripper and match nozzle footprint Make the merged nozzle block match the combined dispenser body footprint in X and Y while keeping the spout in front of the body. Launch link_6 gripper visualization in the course RViz path and publish explicit RG2 markers so the gripper remains visible even when the supplemental RobotModel is not obvious. Constraint: Preserve measured outlet/press calibration; adjust only draft collision/visual geometry and RViz visualization helpers. Rejected: Relying only on the supplemental RobotModel | the course RViz path did not show the gripper reliably. Confidence: high Scope-risk: moderate Directive: Keep /azas/link6_gripper/markers enabled in the course RViz config when debugging link_6 TCP/gripper clearance. Tested: python3 -m py_compile link6_gripper_collision_node.py measured_dispenser_collision_scene_node.py; bash -n run_course_dispenser_press_cycle_rviz.sh; YAML assertions for nozzle X/Y footprint matching combined body and touching body front face; python3 tools/checks/check_measured_dispenser_geometry.py; colcon build --packages-select azas_motion azas_bringup --symlink-install; ros2 run azas_motion link6_gripper_collision_node --ros-args -p publish_once:=true; ros2 launch azas_bringup rg2_link6_tcp.launch.py --show-args; xacro rg2_link6_tcp.urdf.xacro. Not-tested: full RViz visual inspection and live robot motion. --- .../config/measured_dispenser_collision.yaml | 16 +++---- .../rviz/link6_gripper_tcp_debug.rviz | 12 ++++- .../link6_gripper_collision_node.py | 44 ++++++++++++++++++- .../config/measured_dispenser_collision.yaml | 16 +++---- .../run_course_dispenser_press_cycle_rviz.sh | 13 +++++- 5 files changed, 81 insertions(+), 20 deletions(-) diff --git a/src/azas_bringup/config/measured_dispenser_collision.yaml b/src/azas_bringup/config/measured_dispenser_collision.yaml index d749077..e7470ab 100644 --- a/src/azas_bringup/config/measured_dispenser_collision.yaml +++ b/src/azas_bringup/config/measured_dispenser_collision.yaml @@ -9,8 +9,8 @@ metadata: x: 0.02 y: 0.02 z: 0.02 - nozzle_merge_policy: single horizontal spout cuboid spans the combined body Y width - and uses the same orientation as dispenser_combined_body_box + nozzle_merge_policy: single horizontal spout cuboid uses the same X/Y footprint + dimensions and orientation as dispenser_combined_body_box front_hold_poses: dispenser_1: position_xyz_m: @@ -129,16 +129,16 @@ estimated_collision_objects: type: box frame_id: base_link center_xyz_m: - - 0.718 + - 0.721 - -0.0155 - 0.432 size_xyz_m: - - 0.05 + - 0.044 - 0.215 - 0.04 bounds_xyz_m: min: - - 0.693 + - 0.699 - -0.123 - 0.412 max: @@ -152,6 +152,6 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - '# note': Merged horizontal nozzle/spout block lengthened to match the dispenser_combined_body_box - Y span; orientation is kept identical to the combined body box so both blocks - are axis-aligned together. + '# note': Merged horizontal nozzle block has the same X/Y footprint dimensions + and orientation as dispenser_combined_body_box, with its max X face touching + the body min X face. diff --git a/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz b/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz index 4e7d086..adf0f00 100644 --- a/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz +++ b/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz @@ -22,7 +22,7 @@ Visualization Manager: Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /dsr01/robot_description + Value: /robot_description Enabled: true Name: M0609 Robot TF Prefix: "" @@ -86,6 +86,16 @@ Visualization Manager: Value: /azas/dispenser_press/tcp_axes Name: Press TCP Axes Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/link6_gripper/markers + Name: Link6 RG2 Gripper Markers + Value: true Enabled: true Global Options: Background Color: 24; 24; 28 diff --git a/src/azas_motion/azas_motion/link6_gripper_collision_node.py b/src/azas_motion/azas_motion/link6_gripper_collision_node.py index e3cb7d7..a67c430 100644 --- a/src/azas_motion/azas_motion/link6_gripper_collision_node.py +++ b/src/azas_motion/azas_motion/link6_gripper_collision_node.py @@ -18,6 +18,7 @@ from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy from shape_msgs.msg import SolidPrimitive from std_msgs.msg import Header +from visualization_msgs.msg import Marker, MarkerArray def transient_qos(depth: int = 10) -> QoSProfile: @@ -71,12 +72,19 @@ def __init__(self) -> None: ) self.declare_parameter("publish_period_sec", 1.0) self.declare_parameter("publish_once", False) + self.declare_parameter("publish_markers", True) + self.declare_parameter("marker_topic", "/azas/link6_gripper/markers") self.publisher = self.create_publisher( AttachedCollisionObject, "/attached_collision_object", transient_qos(), ) + self.marker_publisher = self.create_publisher( + MarkerArray, + str(self.get_parameter("marker_topic").value), + transient_qos(), + ) self._logged = False self._publish() @@ -122,11 +130,45 @@ def _attached_object(self) -> AttachedCollisionObject: attached.object = obj return attached + def _marker_array(self) -> MarkerArray: + link_name = str(self.get_parameter("attached_link_name").value) + stamp = self.get_clock().now().to_msg() + specs = [ + ("mount", Marker.CYLINDER, (0.0, 0.0, 0.025), (0.080, 0.080, 0.050), (0.42, 0.43, 0.45, 0.95)), + ("palm", Marker.CUBE, (0.0, 0.0, 0.075), (0.090, 0.140, 0.050), (0.08, 0.08, 0.09, 0.95)), + ("left_finger", Marker.CUBE, (0.0, 0.055, 0.155), (0.035, 0.018, 0.160), (0.08, 0.08, 0.09, 0.95)), + ("right_finger", Marker.CUBE, (0.0, -0.055, 0.155), (0.035, 0.018, 0.160), (0.08, 0.08, 0.09, 0.95)), + ("left_pad", Marker.CUBE, (0.0, 0.040, 0.245), (0.025, 0.012, 0.035), (0.05, 0.35, 0.95, 0.95)), + ("right_pad", Marker.CUBE, (0.0, -0.040, 0.245), (0.025, 0.012, 0.035), (0.05, 0.35, 0.95, 0.95)), + ] + markers: list[Marker] = [] + for index, (name, marker_type, xyz, scale, rgba) in enumerate(specs): + marker = Marker() + marker.header.frame_id = link_name + marker.header.stamp = stamp + marker.ns = "azas_link6_rg2_gripper" + marker.id = index + marker.type = marker_type + marker.action = Marker.ADD + marker.pose = make_pose(xyz) + marker.scale.x = scale[0] + marker.scale.y = scale[1] + marker.scale.z = scale[2] + marker.color.r = rgba[0] + marker.color.g = rgba[1] + marker.color.b = rgba[2] + marker.color.a = rgba[3] + marker.text = name + markers.append(marker) + return MarkerArray(markers=markers) + def _publish(self) -> None: self.publisher.publish(self._attached_object()) + if bool(self.get_parameter("publish_markers").value): + self.marker_publisher.publish(self._marker_array()) if not self._logged: self.get_logger().info( - "Publishing RG2-style attached collision envelope on link_6" + "Publishing RG2-style attached collision envelope and markers on link_6" ) self._logged = True diff --git a/src/azas_perception/config/measured_dispenser_collision.yaml b/src/azas_perception/config/measured_dispenser_collision.yaml index d749077..e7470ab 100644 --- a/src/azas_perception/config/measured_dispenser_collision.yaml +++ b/src/azas_perception/config/measured_dispenser_collision.yaml @@ -9,8 +9,8 @@ metadata: x: 0.02 y: 0.02 z: 0.02 - nozzle_merge_policy: single horizontal spout cuboid spans the combined body Y width - and uses the same orientation as dispenser_combined_body_box + nozzle_merge_policy: single horizontal spout cuboid uses the same X/Y footprint + dimensions and orientation as dispenser_combined_body_box front_hold_poses: dispenser_1: position_xyz_m: @@ -129,16 +129,16 @@ estimated_collision_objects: type: box frame_id: base_link center_xyz_m: - - 0.718 + - 0.721 - -0.0155 - 0.432 size_xyz_m: - - 0.05 + - 0.044 - 0.215 - 0.04 bounds_xyz_m: min: - - 0.693 + - 0.699 - -0.123 - 0.412 max: @@ -152,6 +152,6 @@ estimated_collision_objects: - 1.0 publish_to_planning_scene: true enabled_for_real_motion: false - '# note': Merged horizontal nozzle/spout block lengthened to match the dispenser_combined_body_box - Y span; orientation is kept identical to the combined body box so both blocks - are axis-aligned together. + '# note': Merged horizontal nozzle block has the same X/Y footprint dimensions + and orientation as dispenser_combined_body_box, with its max X face touching + the body min X face. diff --git a/tools/run/run_course_dispenser_press_cycle_rviz.sh b/tools/run/run_course_dispenser_press_cycle_rviz.sh index 0cfd9da..fe0d46e 100755 --- a/tools/run/run_course_dispenser_press_cycle_rviz.sh +++ b/tools/run/run_course_dispenser_press_cycle_rviz.sh @@ -19,14 +19,15 @@ START_DELAY_SEC="${START_DELAY_SEC:-22}" JOINT_WAIT_SEC="${JOINT_WAIT_SEC:-60}" DISPENSER_ID="${DISPENSER_ID:-1}" PRESS_COUNT="${PRESS_COUNT:-2}" -RVIZ_MODE="${RVIZ_MODE:-bringup}" # bringup|clean|none -RVIZ_CONFIG="${RVIZ_CONFIG:-${ROOT_DIR}/src/azas_bringup/rviz/m0609_robot_only.rviz}" +RVIZ_MODE="${RVIZ_MODE:-clean}" # bringup|clean|none +RVIZ_CONFIG="${RVIZ_CONFIG:-${ROOT_DIR}/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz}" DISPENSER_COLLISION_ENABLED="${DISPENSER_COLLISION_ENABLED:-1}" # The measured combined box is the glass-bottle/body area, not the press button/head. # Keep markers visible in RViz by default, but do not feed this draft body box into # MoveIt collision checking for the press stroke unless explicitly requested. DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS:-1}" REMOVE_COURSE_WORKSPACE_WALLS="${REMOVE_COURSE_WORKSPACE_WALLS:-1}" +SHOW_LINK6_GRIPPER="${SHOW_LINK6_GRIPPER:-1}" DISPENSER_COLLISION_CONFIG="${DISPENSER_COLLISION_CONFIG:-${ROOT_DIR}/install/azas_bringup/share/azas_bringup/config/measured_dispenser_collision.yaml}" if [[ ! -f "${DISPENSER_COLLISION_CONFIG}" ]]; then DISPENSER_COLLISION_CONFIG="${ROOT_DIR}/src/azas_bringup/config/measured_dispenser_collision.yaml" @@ -140,6 +141,14 @@ if [[ "${DISPENSER_COLLISION_ENABLED}" == "1" || "${DISPENSER_COLLISION_ENABLED} fi fi +if [[ "${SHOW_LINK6_GRIPPER}" == "1" || "${SHOW_LINK6_GRIPPER}" == "true" ]]; then + ros2 launch azas_bringup rg2_link6_tcp.launch.py \ + publish_gripper_collision:=true \ + >"${LOG_DIR}/rg2_link6_tcp.log" 2>&1 & + PIDS+=("$!") + echo "[Azas] SHOW_LINK6_GRIPPER=${SHOW_LINK6_GRIPPER}: publishing RG2 link_6 TF/markers on /azas/link6_gripper/markers." +fi + if [[ "${RVIZ_MODE}" == "clean" ]]; then # dsr_bringup2_moveit launches its default RViz unconditionally. Replace only # the RViz processes that appeared after this script started, preserving any From 18cf286bb29c815decfd8e12da45f5864a085f07 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sun, 7 Jun 2026 21:58:56 +0900 Subject: [PATCH 20/88] Use standard Doosan virtual port for dispenser course Align the dispenser course RViz script with the project's standard Doosan virtual port so the emulator/controller stack can provide /joint_states consistently. Add explicit bringup-failure diagnostics when ros2_control dies before joint_state_broadcaster becomes available. Constraint: Do not change robot geometry or measured calibration; this only fixes launch defaults and diagnostics. Rejected: Treating the failure as a dispenser/gripper collision issue | logs show ros2_control hardware initialization failed before /joint_states existed. Confidence: high Scope-risk: narrow Directive: Keep virtual course scripts on port 12345 unless the Doosan emulator launch contract changes globally. Tested: bash -n tools/run/run_course_dispenser_press_cycle_rviz.sh; grep verified PORT default and diagnostic messages. Not-tested: full Doosan emulator launch after port change. --- tools/run/run_course_dispenser_press_cycle_rviz.sh | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tools/run/run_course_dispenser_press_cycle_rviz.sh b/tools/run/run_course_dispenser_press_cycle_rviz.sh index fe0d46e..040f19d 100755 --- a/tools/run/run_course_dispenser_press_cycle_rviz.sh +++ b/tools/run/run_course_dispenser_press_cycle_rviz.sh @@ -11,7 +11,7 @@ ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" MODE="${MODE:-virtual}" HOST="${HOST:-127.0.0.1}" -PORT="${PORT:-12347}" +PORT="${PORT:-12345}" MODEL="${MODEL:-m0609}" COLOR="${COLOR:-white}" RT_HOST="${RT_HOST:-192.168.137.50}" @@ -96,6 +96,11 @@ while (( SECONDS < joint_deadline )); do done if ! grep -q '^header:' "${LOG_DIR}/course_dispenser_joint_state_once.txt" 2>/dev/null; then echo '[Azas] No fresh /joint_states. MoveItPy cannot mirror the robot in RViz.' >&2 + if grep -qE 'Failed to initialize hardware|Wrong state or command interface configuration|INITIAL STATE CALL FAILURE|process has died' "${LOG_DIR}/course_dispenser_bringup.log" 2>/dev/null; then + echo '[Azas] Doosan virtual bringup failed before joint_state_broadcaster became available.' >&2 + echo "[Azas] Current launch args: MODE=${MODE} HOST=${HOST} PORT=${PORT} MODEL=${MODEL}" >&2 + echo '[Azas] If an emulator was already running, stop stale Doosan emulator/controller processes and rerun.' >&2 + fi tail -100 "${LOG_DIR}/course_dispenser_bringup.log" >&2 || true exit 1 fi From 67a49ecab22fb633e9efc0d4c5321b2447e42c35 Mon Sep 17 00:00:00 2001 From: ssarahstar Date: Mon, 8 Jun 2026 15:31:45 +0900 Subject: [PATCH 21/88] feat: add vision-based azas_cup_uprighting package --- .../azas_cup_uprighting/T_gripper2camera.npy | Bin 0 -> 256 bytes .../azas_cup_uprighting/__init__.py | 0 .../azas_cup_uprighting/_base_node.py | 378 ++++++++++++++++++ .../azas_cup_uprighting/_config.py | 67 ++++ .../azas_cup_uprighting/_motion.py | 122 ++++++ .../azas_cup_uprighting/_perception.py | 207 ++++++++++ .../azas_cup_uprighting/onrobot.py | 184 +++++++++ .../yolo_cup_uprighting_node.py | 253 ++++++++++++ .../config/measured_dispenser_collision.yaml | 66 +++ src/azas_cup_uprighting/config/moveit_py.yaml | 54 +++ src/azas_cup_uprighting/config/safety.yaml | 21 + .../launch/yolo_cup_uprighting.launch.py | 41 ++ src/azas_cup_uprighting/package.xml | 33 ++ .../resource/azas_cup_uprighting | 0 src/azas_cup_uprighting/setup.cfg | 4 + src/azas_cup_uprighting/setup.py | 35 ++ 16 files changed, 1465 insertions(+) create mode 100644 src/azas_cup_uprighting/azas_cup_uprighting/T_gripper2camera.npy create mode 100644 src/azas_cup_uprighting/azas_cup_uprighting/__init__.py create mode 100644 src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py create mode 100644 src/azas_cup_uprighting/azas_cup_uprighting/_config.py create mode 100644 src/azas_cup_uprighting/azas_cup_uprighting/_motion.py create mode 100644 src/azas_cup_uprighting/azas_cup_uprighting/_perception.py create mode 100644 src/azas_cup_uprighting/azas_cup_uprighting/onrobot.py create mode 100644 src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py create mode 100644 src/azas_cup_uprighting/config/measured_dispenser_collision.yaml create mode 100644 src/azas_cup_uprighting/config/moveit_py.yaml create mode 100644 src/azas_cup_uprighting/config/safety.yaml create mode 100644 src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py create mode 100644 src/azas_cup_uprighting/package.xml create mode 100644 src/azas_cup_uprighting/resource/azas_cup_uprighting create mode 100644 src/azas_cup_uprighting/setup.cfg create mode 100644 src/azas_cup_uprighting/setup.py diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/T_gripper2camera.npy b/src/azas_cup_uprighting/azas_cup_uprighting/T_gripper2camera.npy new file mode 100644 index 0000000000000000000000000000000000000000..f7e6a18a326078c957275738b36e575347d22790 GIT binary patch literal 256 zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(cT3DEP6dh= zXCxM+0{I#yItnJ5ItsN4WCJd>@0H>Q|G(e=V8yGbsK9Fb2HRuzUMLpW|GYVk@dC4> z!;D8CPG2#v-f#S4$J3QSbqoTUt3+%n?Qfr7TqZNi$Du|2o%&^;0{ez!pRPiMN_#a4 Y;rGY?zqgOmK5M;VlBxp(Uhu&l0RD$fKmY&$ literal 0 HcmV?d00001 diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/__init__.py b/src/azas_cup_uprighting/azas_cup_uprighting/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py new file mode 100644 index 0000000..5ed8869 --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -0,0 +1,378 @@ +"""MoveIt 기반 Pick 노드 베이스 클래스. + +공통 기능: + - MoveIt 초기화, plan 파라미터 + - RealSense 카메라 콜백 (color / depth / intrinsics) + - YOLO 모델 로드 + - Hand-Eye 변환, pixel→base 좌표 변환 + - Home 이동 + home_xyz/home_ori 캐싱 + - Approach + 재검출 루틴 + - cv2 메인 루프 (freeze 화면, 키 입력, 자동 모드) + +자식 노드는 주로 다음을 override / 구현: + - detect_and_pick(frame) — pick 시퀀스 + - _select_target(detections) — 다음 픽 대상 선정 + - _draw_detections(frame) — (optional) 시각화 + - on_ready() — Home 이후 추가 init (e.g. scan) + - is_auto_ready() — auto 모드 트리거 가능 조건 + - _handle_key_extra(key) — 추가 키 (e.g. 's' for box scan) +""" + +import threading +import time + +import cv2 +import numpy as np +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node + +from scipy.spatial.transform import Rotation + +from sensor_msgs.msg import CameraInfo, Image +from cv_bridge import CvBridge + +from moveit.core.robot_state import RobotState +from moveit.planning import MoveItPy, PlanRequestParameters + +from .onrobot import RG +from . import _config as cfg +from ._motion import get_ee_matrix, make_pose, plan_and_execute +from . import _perception as perc + +try: + from ultralytics import YOLO +except ImportError as e: + raise ImportError("pip install ultralytics") from e + + +class BaseMoveItPickNode(Node): + """MoveIt + RealSense + YOLO + RG2 그리퍼 통합 베이스.""" + + NODE_NAME = "yolo_pick_base" + MOVEIT_NODE_NAME = "yolo_pick_base_py" + WINDOW_NAME = "YOLO Pick" + + def __init__(self): + super().__init__(self.NODE_NAME) + log = self.get_logger() + + # ── 카메라 상태 ── + self.bridge = CvBridge() + self.color_image = None + self.depth_image = None + self.intrinsics = None + + # ── 픽 상태 ── + self.picking = False + self.home_xyz = None # (x, y, z) [m] — initialize_home 에서 설정 + self.home_ori = None # quat dict {x, y, z, w} + self._auto_mode = False + self._last_pick_time = 0.0 + self._detections: list[dict] = [] + self._frozen_frame = None + + # ── Hand-Eye ── + self.gripper2cam, calib_file = perc.load_hand_eye() + log.info(f"Hand-Eye 로드: {calib_file}") + + # ── 그리퍼 ── + self.gripper = RG(cfg.GRIPPER_NAME, cfg.TOOLCHARGER_IP, cfg.TOOLCHARGER_PORT) + + # ── MoveIt ── + log.info("MoveItPy 초기화 중...") + self.robot = MoveItPy(node_name=self.MOVEIT_NODE_NAME) + self.arm = self.robot.get_planning_component(cfg.GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + log.info("MoveItPy 초기화 완료") + + self.ompl_params = self._make_plan_params( + "ompl", "RRTConnect", vel=0.2, acc=0.1, time=2.0) + self.pilz_params = self._make_plan_params( + "pilz_industrial_motion_planner", "PTP", vel=0.15, acc=0.1, time=2.0) + + # ── YOLO ── + log.info(f"YOLO 모델 로드: {cfg.YOLO_MODEL_PATH}") + self.yolo = YOLO(cfg.YOLO_MODEL_PATH) + log.info("YOLO 모델 로드 완료") + + # ── 카메라 구독 ── + self.create_subscription(CameraInfo, cfg.TOPIC_CAM_INFO, + self._cam_info_cb, 10) + self.create_subscription(Image, cfg.TOPIC_COLOR, + self._color_cb, 10) + self.create_subscription(Image, cfg.TOPIC_DEPTH, + self._depth_cb, 10) + + # ════════════════════════════════════════════ + # 내부 헬퍼 + # ════════════════════════════════════════════ + def _make_plan_params(self, pipeline, planner_id, *, + vel: float, acc: float, time: float): + p = PlanRequestParameters(self.robot) + p.planning_pipeline = pipeline + p.planner_id = planner_id + p.max_velocity_scaling_factor = vel + p.max_acceleration_scaling_factor = acc + p.planning_time = time + return p + + # ── 콜백 ── + def _cam_info_cb(self, msg): + self.intrinsics = { + "fx": msg.k[0], "fy": msg.k[4], + "ppx": msg.k[2], "ppy": msg.k[5], + } + + def _color_cb(self, msg): + self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + + def _depth_cb(self, msg): + self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + + # ════════════════════════════════════════════ + # Perception 래퍼 + # ════════════════════════════════════════════ + def transform_to_base(self, cam_xyz_m): + return perc.transform_to_base(self.robot, self.gripper2cam, cam_xyz_m) + + def pixel_to_base(self, px, py): + return perc.pixel_to_base( + self.robot, self.gripper2cam, + self.depth_image, self.intrinsics, + px, py, self.get_logger()) + + def run_yolo(self, frame): + return perc.run_yolo(self.yolo, frame, self.depth_image) + + # ════════════════════════════════════════════ + # Motion 래퍼 + # ════════════════════════════════════════════ + def plan_pose(self, x, y, z, ori, params=None) -> bool: + return plan_and_execute( + self.robot, self.arm, self.get_logger(), + pose_goal=make_pose(x, y, z, ori), + params=params or self.pilz_params) + + def plan_state(self, state, params=None) -> bool: + return plan_and_execute( + self.robot, self.arm, self.get_logger(), + state_goal=state, + params=params or self.ompl_params) + + def go_home_pose(self) -> bool: + """관절 home 자세로 이동.""" + home_state = RobotState(self.robot_model) + home_state.joint_positions = cfg.HOME_JOINTS + home_state.update() + return self.plan_state(home_state) + + # ════════════════════════════════════════════ + # Approach + 재검출 + # ════════════════════════════════════════════ + def approach_and_redetect(self, target_cls_id: int, target_xy): + """target XY 위로 EE 미세 이동 → 재검출 → 화면 중앙 가장 가까운 동일 클래스 detection. + + 실패 시 None. + """ + log = self.get_logger() + ori = self.home_ori + ox, oy = cfg.APPROACH_OFFSET + cur_ee = get_ee_matrix(self.robot) + ax = target_xy[0] + ox + ay = target_xy[1] + oy + az = cur_ee[2, 3] + + log.info( + f"[Approach] target_xy=({target_xy[0]:.3f}, {target_xy[1]:.3f}) " + f"+ offset -> EE=({ax:.3f}, {ay:.3f}, {az:.3f})" + ) + if not self.plan_pose(ax, ay, az, ori): + log.error("Approach 실패") + return None + time.sleep(cfg.APPROACH_SETTLE) + + if self.color_image is None: + log.error("재검출 프레임 없음") + return None + new_frame = self.color_image.copy() + new_detections = self.run_yolo(new_frame) + self._detections = new_detections + self._frozen_frame = new_frame.copy() + + same_cls = [d for d in new_detections if d["cls_id"] == target_cls_id] + if not same_cls: + log.error(f"재검출 실패: cls={target_cls_id} 없음") + return None + + h, w = new_frame.shape[:2] + cx_img, cy_img = w // 2, h // 2 + return min( + same_cls, + key=lambda d: (d["cx"] - cx_img) ** 2 + (d["cy"] - cy_img) ** 2, + ) + + # ════════════════════════════════════════════ + # 시각화 (기본 구현 — 자식이 override 가능) + # ════════════════════════════════════════════ + def _draw_detections(self, frame: np.ndarray) -> np.ndarray: + """기본: 모든 detection 박스 + 다음 픽 대상은 녹색.""" + vis = frame.copy() + next_target = (self._select_target(self._detections) + if self._detections else None) + for det in self._detections: + x1, y1, x2, y2 = det["box"] + color = (0, 255, 0) if det is next_target else (255, 100, 0) + label = f"{det['cls_name']} {det['conf']:.2f}" + cv2.rectangle(vis, (x1, y1), (x2, y2), color, 2) + cv2.putText(vis, label, (x1, y1 - 8), + cv2.FONT_HERSHEY_SIMPLEX, 0.55, color, 2) + cv2.drawMarker(vis, (det["cx"], det["cy"]), color, + cv2.MARKER_CROSS, 20, 2) + self._draw_hud(vis) + return vis + + def _draw_hud(self, vis: np.ndarray): + """상단 HUD (mode, detections 수).""" + mode_txt = "AUTO" if self._auto_mode else "MANUAL" + mode_col = (0, 255, 255) if self._auto_mode else (200, 200, 200) + cv2.putText(vis, f"[{mode_txt}] {self._key_help_str()}", + (10, 26), cv2.FONT_HERSHEY_SIMPLEX, 0.55, mode_col, 2) + cv2.putText(vis, f"detections: {len(self._detections)}", + (10, 52), cv2.FONT_HERSHEY_SIMPLEX, + 0.5, (180, 180, 180), 1) + + def _key_help_str(self) -> str: + return "p:pick a:auto ESC:quit" + + # ════════════════════════════════════════════ + # Pick 백그라운드 + freeze + # ════════════════════════════════════════════ + def _pick_in_thread(self, frame: np.ndarray): + if self.picking: + return + self._frozen_frame = frame.copy() + + def _work(): + try: + self.detect_and_pick(frame) + finally: + self._frozen_frame = None + + threading.Thread(target=_work, daemon=True).start() + + # ════════════════════════════════════════════ + # 자식이 구현 / override 할 메서드 (hooks) + # ════════════════════════════════════════════ + def detect_and_pick(self, frame: np.ndarray): + raise NotImplementedError + + def _select_target(self, detections): + raise NotImplementedError + + def on_ready(self): + """Home 이동 완료 후 호출. 자식이 추가 init 가능 (e.g. scan_box).""" + pass + + def is_auto_ready(self) -> bool: + """auto 모드 트리거 전제 조건.""" + return True + + def _handle_key_extra(self, key: int): + """ESC, p, a 외 추가 키 처리. 자식 override (e.g. 's' for scan).""" + pass + + # ════════════════════════════════════════════ + # 메인 루프 + # ════════════════════════════════════════════ + def initialize_home(self) -> bool: + log = self.get_logger() + log.info("[Init] Home 이동") + if not self.go_home_pose(): + log.error("Home 실패") + return False + time.sleep(0.5) + + T = get_ee_matrix(self.robot) + self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) + qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() + self.home_ori = {"x": float(qx), "y": float(qy), + "z": float(qz), "w": float(qw)} + log.info(f"[Init] Home = ({T[0,3]:.3f}, {T[1,3]:.3f}, {T[2,3]:.3f}) m") + + self.gripper.open_gripper() + time.sleep(1.0) + return True + + def run(self): + log = self.get_logger() + cv2.namedWindow(self.WINDOW_NAME) + + executor = MultiThreadedExecutor() + executor.add_node(self) + spin_thread = threading.Thread(target=executor.spin, daemon=True) + spin_thread.start() + + if not self.initialize_home(): + return + self.on_ready() + log.info(f"=== Ready === {self._key_help_str()}") + + while rclpy.ok(): + # ── Freeze 분기 (pick / scan 진행 중) ── + if self._frozen_frame is not None: + vis = self._draw_detections(self._frozen_frame) + cv2.putText(vis, "[BUSY... CAMERA FROZEN]", + (10, 102), cv2.FONT_HERSHEY_SIMPLEX, + 0.65, (0, 0, 255), 2) + cv2.imshow(self.WINDOW_NAME, vis) + key = cv2.waitKey(30) & 0xFF + if key == 27: + break + continue + + # ── Live 분기 ── + if self.color_image is None: + time.sleep(0.01) + continue + + frame = self.color_image.copy() + self._detections = self.run_yolo(frame) + + now = time.time() + if (self._auto_mode + and not self.picking + and self.is_auto_ready() + and (now - self._last_pick_time) >= cfg.AUTO_PICK_INTERVAL): + if self._select_target(self._detections) is not None: + self._last_pick_time = now + self._pick_in_thread(frame) + continue + + vis = self._draw_detections(frame) + cv2.imshow(self.WINDOW_NAME, vis) + + key = cv2.waitKey(1) & 0xFF + if key == 27: + break + elif key == ord("p"): + log.info("[KEY] manual pick") + self._pick_in_thread(frame) + elif key == ord("a"): + self._auto_mode = not self._auto_mode + log.info(f"[KEY] auto {'ON' if self._auto_mode else 'OFF'}") + else: + self._handle_key_extra(key) + + cv2.destroyAllWindows() + + +def run_node(node_cls): + """공통 main() 헬퍼.""" + rclpy.init() + node = node_cls() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_config.py b/src/azas_cup_uprighting/azas_cup_uprighting/_config.py new file mode 100644 index 0000000..4079399 --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_config.py @@ -0,0 +1,67 @@ + +import math +import os +import yaml +from ament_index_python.packages import get_package_share_directory + + +PKG_SHARE = get_package_share_directory('azas_cup_uprighting') + + +def load_yaml(file_name): + file_path = os.path.join(PKG_SHARE, 'config', file_name) + with open(file_path, 'r', encoding='utf-8') as f: + return yaml.safe_load(f) + + +try: + SAFETY_CFG = load_yaml('safety.yaml') +except Exception as e: + print(f"[경고] safety.yaml을 불러오지 못했습니다: {e}") + SAFETY_CFG = None + + +try: + DISPENSER_CFG = load_yaml('measured_dispenser_collision.yaml') +except Exception as e: + print(f"[경고] measured_dispenser_collision.yaml을 불러오지 못했습니다: {e}") + DISPENSER_CFG = None + + +# ── MoveIt ───────────────────────────────────────── +GROUP_NAME = "manipulator" +BASE_FRAME = "base_link" +EE_LINK = "link_6" + +HOME_JOINTS = { + "joint_1": math.radians(3.0), + "joint_2": math.radians(-12.7), + "joint_3": math.radians(44.0), + "joint_4": math.radians(-9.0), + "joint_5": math.radians(133.0), + "joint_6": math.radians(90.0), +} + + +# ── Pick 파라미터 (m) ──────────────────────────────── +Z_OFFSET = 0.20 # gripper tip ↔ link_6 (depth 측정 base z + 이 값 = pick_z) + + +# ── Approach (재검출 직전 EE 미세 이동) ────────────── +APPROACH_OFFSET = (-0.05, -0.05) # (dx, dy) m, Z 는 현재 유지 +APPROACH_SETTLE = 0.5 # 이동 후 카메라 안정화 [s] + +# ── 그리퍼 ────────────────────────────────────────── +GRIPPER_NAME = "rg2" +TOOLCHARGER_IP = "192.168.1.1" +TOOLCHARGER_PORT = 502 + +# ── YOLO ──────────────────────────────────────────── +YOLO_MODEL_PATH = os.path.join(PKG_SHARE, 'config', 'best.pt') +YOLO_CONF_THRESH = 0.5 +AUTO_PICK_INTERVAL = 3.0 # 자동 모드 픽 간격 [s] + +# ── 카메라 토픽 ────────────────────────────────────── +TOPIC_CAM_INFO = "/camera/camera/color/camera_info" +TOPIC_COLOR = "/camera/camera/color/image_raw" +TOPIC_DEPTH = "/camera/camera/aligned_depth_to_color/image_raw" diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_motion.py b/src/azas_cup_uprighting/azas_cup_uprighting/_motion.py new file mode 100644 index 0000000..89dc46c --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_motion.py @@ -0,0 +1,122 @@ +"""MoveIt 모션 유틸 (순수 함수).""" + +import numpy as np +from geometry_msgs.msg import PoseStamped +from scipy.spatial.transform import Rotation as R +from . import _config as cfg + + + +def clamp_to_safe_workspace(x, y, z, logger): + """safety.yaml 범위로 클램핑하고 경고 로그 (X, Y, Z 상/하한 모두 적용).""" + + # 안전 설정 파일이 제대로 로드되지 않았을 경우를 대비한 방어 코드 + if not cfg.SAFETY_CFG or 'motion' not in cfg.SAFETY_CFG: + logger.error("SAFETY_CFG가 로드되지 않아 클램핑을 건너뜁니다.") + return x, y, z + + # YAML 데이터에서 작업 영역 경계선 가져오기 + bounds = cfg.SAFETY_CFG['motion']['workspace_bounds_m'] + + safe_x_min, safe_x_max = bounds['x_min'], bounds['x_max'] + safe_y_min, safe_y_max = bounds['y_min'], bounds['y_max'] + safe_z_min, safe_z_max = bounds['z_min'], bounds['z_max'] + + # X축 클램핑 + if x < safe_x_min: + logger.warning(f"x={x:.3f} -> {safe_x_min} (X 최소 한계 도달)") + x = safe_x_min + elif x > safe_x_max: + logger.warning(f"x={x:.3f} -> {safe_x_max} (X 최대 한계 도달)") + x = safe_x_max + + # Y축 클램핑 + if y < safe_y_min: + logger.warning(f"y={y:.3f} -> {safe_y_min} (Y 최소 한계 도달)") + y = safe_y_min + elif y > safe_y_max: + logger.warning(f"y={y:.3f} -> {safe_y_max} (Y 최대 한계 도달)") + y = safe_y_max + + # Z축 클램핑 + if z < safe_z_min: + logger.warning(f"z={z:.3f} -> {safe_z_min} (Z 최소 한계 도달)") + z = safe_z_min + elif z > safe_z_max: + logger.warning(f"z={z:.3f} -> {safe_z_max} (Z 최대 한계 도달)") + z = safe_z_max + + return x, y, z + +def make_pose(x, y, z, ori) -> PoseStamped: + """(x, y, z) + orientation dict → PoseStamped(base_link).""" + p = PoseStamped() + p.header.frame_id = cfg.BASE_FRAME + p.pose.position.x = float(x) + p.pose.position.y = float(y) + p.pose.position.z = float(z) + p.pose.orientation.x = ori["x"] + p.pose.orientation.y = ori["y"] + p.pose.orientation.z = ori["z"] + p.pose.orientation.w = ori["w"] + return p + + +def get_ee_matrix(moveit_robot) -> np.ndarray: + """현재 base_link → EE_LINK 4x4 변환행렬.""" + psm = moveit_robot.get_planning_scene_monitor() + with psm.read_only() as scene: + T = scene.current_state.get_global_link_transform(cfg.EE_LINK) + return np.asarray(T, dtype=float) + + +def plan_and_execute(robot, arm, logger, + pose_goal=None, state_goal=None, params=None) -> bool: + """Pose 또는 RobotState 목표로 plan + execute. 실패 시 False.""" + arm.set_start_state_to_current_state() + + if pose_goal is not None: + x = pose_goal.pose.position.x + y = pose_goal.pose.position.y + z = pose_goal.pose.position.z + sx, sy, sz = clamp_to_safe_workspace(x, y, z, logger) + pose_goal.pose.position.x = sx + pose_goal.pose.position.y = sy + pose_goal.pose.position.z = sz + arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=cfg.EE_LINK) + elif state_goal is not None: + arm.set_goal_state(robot_state=state_goal) + else: + logger.error("plan_and_execute: pose/state 없음") + return False + + plan_result = arm.plan(parameters=params) if params is not None else arm.plan() + if not plan_result: + logger.error("Planning 실패") + return False + + result = robot.execute(group_name=cfg.GROUP_NAME, + robot_trajectory=plan_result.trajectory, + blocking=True) + return bool(result) + + +def get_gripper_pose_by_cup(cup_theta): + """ + 컵의 주축 각도(theta)를 받아 그리퍼가 옆면(허리)을 수직 진입하여 + 파지할 수 있도록 쿼터니언 반환 + """ + + yaw = cup_theta + + # 오일러 각을 쿼터니언으로 변환 + # (Roll=180, Pitch=0 상태에서 Yaw축만 조향) + quat = R.from_euler('xyz', [180, 0, np.degrees(yaw)], degrees=True).as_quat() + + ori_dict = { + "x": float(quat[0]), + "y": float(quat[1]), + "z": float(quat[2]), + "w": float(quat[3]) + } + return ori_dict diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_perception.py b/src/azas_cup_uprighting/azas_cup_uprighting/_perception.py new file mode 100644 index 0000000..152f5e3 --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_perception.py @@ -0,0 +1,207 @@ +"""YOLO 추론 + 카메라 좌표 변환. + +Hand-Eye 행렬, pixel→base 변환, YOLO 검출을 함수로 제공. +""" + +from pathlib import Path + +import cv2 +import numpy as np + +from ament_index_python.packages import get_package_share_directory + +from . import _config as cfg +from ._motion import get_ee_matrix + + +def bbox_size(box) -> int: + """bbox max(w, h) — 길이/지름 대표값.""" + x1, y1, x2, y2 = box + return max(x2 - x1, y2 - y1) + + +def load_hand_eye(): + """T_gripper2camera.npy 로드 (mm → m).""" + calib_file = ( + Path(get_package_share_directory("azas_cup_uprighting")) + / "config" / "T_gripper2camera.npy" + ) + g2c = np.load(str(calib_file)).astype(float) + g2c[:3, 3] /= 1000.0 # mm → m + return g2c, calib_file + + +def transform_to_base(robot, gripper2cam, cam_xyz_m): + """카메라 좌표 (m) → base 좌표 (m). 현재 EE 자세 기준.""" + coord = np.append(np.array(cam_xyz_m, dtype=float), 1.0) + base2ee = get_ee_matrix(robot) + base2cam = base2ee @ gripper2cam + return (base2cam @ coord)[:3] + + +def pixel_to_base(robot, gripper2cam, depth_image, intrinsics, + px: int, py: int, logger): + """픽셀 + depth 이미지 → base 좌표 (m). 실패 시 None.""" + if depth_image is None or intrinsics is None: + logger.warn("frame/intrinsics 아직 준비 안됨") + return None + + h, w = depth_image.shape[:2] + if not (0 <= px < w and 0 <= py < h): + logger.warn("pixel 범위 초과") + return None + + z_raw = depth_image[py, px] + if z_raw == 0: + logger.warn(f"depth=0 at ({px}, {py})") + return None + + z_m = (float(z_raw) / 1000.0 + if depth_image.dtype == np.uint16 else float(z_raw)) + + fx, fy = intrinsics["fx"], intrinsics["fy"] + ppx, ppy = intrinsics["ppx"], intrinsics["ppy"] + + cam_x = (px - ppx) * z_m / fx + cam_y = (py - ppy) * z_m / fy + cam_z = z_m + + base = transform_to_base(robot, gripper2cam, (cam_x, cam_y, cam_z)) + logger.info( + f"pixel({px},{py}) cam({cam_x:.3f},{cam_y:.3f},{cam_z:.3f}) " + f"-> base({base[0]:.3f},{base[1]:.3f},{base[2]:.3f}) m" + ) + return tuple(float(v) for v in base) + + +def _depth_at(depth_image, cx: int, cy: int) -> float: + """픽셀의 depth (m). 없으면 inf.""" + if depth_image is None: + return float("inf") + h, w = depth_image.shape[:2] + if not (0 <= cx < w and 0 <= cy < h): + return float("inf") + z_raw = depth_image[cy, cx] + if z_raw == 0: + return float("inf") + return (float(z_raw) / 1000.0 + if depth_image.dtype == np.uint16 else float(z_raw)) + + +def run_yolo(yolo, frame: np.ndarray, depth_image=None) -> list[dict]: + """YOLO 추론. 각 detection 에 cx/cy/conf/cls/bbox/size/depth 포함.""" + results = yolo(frame, verbose=False)[0] + detections = [] + + for box in results.boxes: + conf = float(box.conf[0]) + cls_id = int(box.cls[0]) + if conf < cfg.YOLO_CONF_THRESH: + continue + + x1, y1, x2, y2 = map(int, box.xyxy[0].tolist()) + cx = (x1 + x2) // 2 + cy = (y1 + y2) // 2 + cls_name = yolo.names.get(cls_id, str(cls_id)) + + detections.append({ + "cx": cx, "cy": cy, + "conf": conf, + "cls_id": cls_id, + "cls_name": cls_name, + "box": (x1, y1, x2, y2), + "size": bbox_size((x1, y1, x2, y2)), + "depth": _depth_at(depth_image, cx, cy), + }) + + return detections + + +def calculate_cup_orientation(depth_image, bbox, frame=None): + """ + OpenCV를 이용해 Bounding Box 내부의 실제 컵 기울기(theta)를 정밀 추출 + """ + if frame is None: + return 0.0 + + # 1. Bounding Box 좌표를 정수로 변환하여 ROI(관심 영역) 자르기 + x1, y1, x2, y2 = map(int, bbox) + roi = frame[y1:y2, x1:x2] + + if roi.size == 0: + return 0.0 + + # 2. 이미지를 흑백으로 변환하고 이진화(Threshold)하여 컵과 배경 분리 + gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY) + _, thresh = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU) + + # 3. 외곽선(Contours) 찾기 + contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if not contours: + return 0.0 + + # 4. 가장 넓은 외곽선을 컵의 본체로 간주 + c = max(contours, key=cv2.contourArea) + + # 5. 외곽선을 감싸는 기울어진 사각형(RotatedRect) 생성 + rect = cv2.minAreaRect(c) + (cx, cy), (w, h), angle = rect + + # 6. OpenCV angle 보정 (긴 축이 컵의 방향이 되도록 기준 정렬) + if w < h: + angle += 90.0 + + # 7. Degree를 Radian으로 변환하여 반환 + theta = np.deg2rad(angle) + return theta + + +def is_top_pointing_towards_theta(frame, bbox, theta): + """ + 컵의 빨간 스티커(입구 부분)가 theta 방향에 있는지, 반대 방향인지 판별 + """ + if frame is None: + return True + + x1, y1, x2, y2 = map(int, bbox) + x1, y1 = max(0, x1), max(0, y1) + x2, y2 = min(frame.shape[1], x2), min(frame.shape[0], y2) + + roi = frame[y1:y2, x1:x2] + if roi.size == 0: + return True + + hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) + + # 빨간색 마스크 추출 + lower_red1 = np.array([0, 100, 100]) + upper_red1 = np.array([10, 255, 255]) + lower_red2 = np.array([160, 100, 100]) + upper_red2 = np.array([180, 255, 255]) + + mask = cv2.inRange(hsv, lower_red1, upper_red1) + cv2.inRange(hsv, lower_red2, upper_red2) + + # 빨간색 픽셀들의 무게중심(Center of Mass) 계산 + M = cv2.moments(mask) + if M["m00"] == 0: + return True + + # ROI 내에서의 무게중심 좌표 + cm_x = int(M["m10"] / M["m00"]) + cm_y = int(M["m01"] / M["m00"]) + + # ROI의 기하학적 중심 좌표 + center_x = roi.shape[1] / 2.0 + center_y = roi.shape[0] / 2.0 + + # 1. 컵 중심에서 스티커(무게중심)를 향하는 벡터 생성 + vec_sticker = np.array([cm_x - center_x, cm_y - center_y]) + + # 2. theta 각도가 가리키는 단위 벡터 생성 + vec_theta = np.array([np.cos(theta), np.sin(theta)]) + + # 3. 두 벡터의 내적(Dot Product) 계산 + + dot_product = np.dot(vec_sticker, vec_theta) + + return dot_product > 0 diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/onrobot.py b/src/azas_cup_uprighting/azas_cup_uprighting/onrobot.py new file mode 100644 index 0000000..22ae003 --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/onrobot.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python3 + +from pymodbus.client.sync import ModbusTcpClient as ModbusClient + + +class RG(): + + def __init__(self, gripper, ip, port): + self.client = ModbusClient( + ip, + port=port, + stopbits=1, + bytesize=8, + parity='E', + baudrate=115200, + timeout=1) + if gripper not in ['rg2', 'rg6']: + print("Please specify either rg2 or rg6.") + return + self.gripper = gripper # RG2/6 + if self.gripper == 'rg2': + self.max_width = 1100 + self.max_force = 400 + elif self.gripper == 'rg6': + self.max_width = 1600 + self.max_force = 1200 + self.open_connection() + + def open_connection(self): + """Opens the connection with a gripper.""" + self.client.connect() + + def close_connection(self): + """Closes the connection with the gripper.""" + self.client.close() + + def get_fingertip_offset(self): + """Reads the current fingertip offset in 1/10 millimeters. + Please note that the value is a signed two's complement number. + """ + result = self.client.read_holding_registers( + address=258, count=1, unit=65) + offset_mm = result.registers[0] / 10.0 + return offset_mm + + def get_width(self): + """Reads current width between gripper fingers in 1/10 millimeters. + Please note that the width is provided without any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.read_holding_registers( + address=267, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def get_status(self): + """Reads current device status. + This status field indicates the status of the gripper and its motion. + It is composed of 7 flags, described in the table below. + + Bit Name Description + 0 (LSB): busy High (1) when a motion is ongoing, + low (0) when not. + The gripper will only accept new commands + when this flag is low. + 1: grip detected High (1) when an internal- or + external grip is detected. + 2: S1 pushed High (1) when safety switch 1 is pushed. + 3: S1 trigged High (1) when safety circuit 1 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 4: S2 pushed High (1) when safety switch 2 is pushed. + 5: S2 trigged High (1) when safety circuit 2 is activated. + The gripper will not move + while this flag is high; + can only be reset by power cycling. + 6: safety error High (1) when on power on any of + the safety switch is pushed. + 10-16: reserved Not used. + """ + # address : register number + # count : number of registers to be read + # unit : slave device address + result = self.client.read_holding_registers( + address=268, count=1, unit=65) + status = format(result.registers[0], '016b') + status_list = [0] * 7 + if int(status[-1]): + print("A motion is ongoing so new commands are not accepted.") + status_list[0] = 1 + if int(status[-2]): + print("An internal- or external grip is detected.") + status_list[1] = 1 + if int(status[-3]): + print("Safety switch 1 is pushed.") + status_list[2] = 1 + if int(status[-4]): + print("Safety circuit 1 is activated so it will not move.") + status_list[3] = 1 + if int(status[-5]): + print("Safety switch 2 is pushed.") + status_list[4] = 1 + if int(status[-6]): + print("Safety circuit 2 is activated so it will not move.") + status_list[5] = 1 + if int(status[-7]): + print("Any of the safety switch is pushed.") + status_list[6] = 1 + + return status_list + + def get_width_with_offset(self): + """Reads current width between gripper fingers in 1/10 millimeters. + The set fingertip offset is considered. + """ + result = self.client.read_holding_registers( + address=275, count=1, unit=65) + width_mm = result.registers[0] / 10.0 + return width_mm + + def set_control_mode(self, command): + """The control field is used to start and stop gripper motion. + Only one option should be set at a time. + Please note that the gripper will not start a new motion + before the one currently being executed is done + (see busy flag in the Status field). + The valid flags are: + + 1 (0x0001): grip + Start the motion, with the target force and width. + Width is calculated without the fingertip offset. + Please note that the gripper will ignore this command + if the busy flag is set in the status field. + 8 (0x0008): stop + Stop the current motion. + 16 (0x0010): grip_w_offset + Same as grip, but width is calculated + with the set fingertip offset. + """ + result = self.client.write_register( + address=2, value=command, unit=65) + + def set_target_force(self, force_val): + """Writes the target force to be reached + when gripping and holding a workpiece. + It must be provided in 1/10th Newtons. + The valid range is 0 to 400 for the RG2 and 0 to 1200 for the RG6. + """ + result = self.client.write_register( + address=0, value=force_val, unit=65) + + def set_target_width(self, width_val): + """Writes the target width between + the finger to be moved to and maintained. + It must be provided in 1/10th millimeters. + The valid range is 0 to 1100 for the RG2 and 0 to 1600 for the RG6. + Please note that the target width should be provided + corrected for any fingertip offset, + as it is measured between the insides of the aluminum fingers. + """ + result = self.client.write_register( + address=1, value=width_val, unit=65) + + def close_gripper(self, force_val=400): + """Closes gripper.""" + params = [force_val, 0, 16] + print("Start closing gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def open_gripper(self, force_val=400): + """Opens gripper.""" + params = [force_val, self.max_width, 16] + print("Start opening gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) + + def move_gripper(self, width_val, force_val=400): + """Moves gripper to the specified width.""" + params = [force_val, width_val, 16] + print("Start moving gripper.") + result = self.client.write_registers( + address=0, values=params, unit=65) \ No newline at end of file diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py new file mode 100644 index 0000000..134072d --- /dev/null +++ b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py @@ -0,0 +1,253 @@ + +#!/usr/bin/env python3 +""" +쓰러진 컵을 인식하고 보정된 오프셋으로 똑바로 세우는(Uprighting) 시나리오 노드. +""" + +import time +import numpy as np + +from . import _config as cfg +from ._base_node import BaseMoveItPickNode, run_node +from ._perception import calculate_cup_orientation, is_top_pointing_towards_theta +from ._motion import get_gripper_pose_by_cup +from scipy.spatial.transform import Rotation as R + +from moveit_msgs.msg import CollisionObject +from shape_msgs.msg import SolidPrimitive +from geometry_msgs.msg import Pose + + + +CUP_LENGTH_M = 0.12 +CUP_DIAMETER_M = 0.072 +CUP_RADIUS_M = CUP_DIAMETER_M / 2.0 + + +class YoloCupUprightingNode(BaseMoveItPickNode): + NODE_NAME = "yolo_cup_uprighting_node" + MOVEIT_NODE_NAME = "yolo_cup_uprighting_py" + WINDOW_NAME = "Cup Uprighting" + + def __init__(self): + super().__init__() + self.setup_safety_environment() + + def setup_safety_environment(self): + log = self.get_logger() + log.info("🚧 [안전망] YAML 기반 안전 환경(Keep-out Zone) 구축을 시작합니다...") + + pub = self.create_publisher(CollisionObject, '/collision_object', 10) + time.sleep(1.0) + + + if cfg.SAFETY_CFG and 'motion' in cfg.SAFETY_CFG: + bounds = cfg.SAFETY_CFG['motion']['workspace_bounds_m'] + + self.arm.set_workspace( + min_x=bounds['x_min'], min_y=bounds['y_min'], min_z=bounds['z_min'], + max_x=bounds['x_max'], max_y=bounds['y_max'], max_z=bounds['z_max'] + ) + log.info(f"-> 작업 영역 동적 제한 완료 (Z_min: {bounds['z_min']}m)") + else: + log.warn("-> safety.yaml을 찾을 수 없어 기본 작업 영역 제한을 건너뜁니다.") + + + + if cfg.DISPENSER_CFG and 'estimated_collision_objects' in cfg.DISPENSER_CFG: + + disp_data = cfg.DISPENSER_CFG['estimated_collision_objects']['dispenser_combined_body_box'] + + dispenser = CollisionObject() + dispenser.header.frame_id = disp_data.get('frame_id', 'base_link') + dispenser.id = "dispenser_combined_body_box" + dispenser.operation = CollisionObject.ADD + + disp_box = SolidPrimitive() + disp_box.type = SolidPrimitive.BOX + + disp_box.dimensions = disp_data['size_xyz_m'] + + disp_pose = Pose() + disp_pose.position.x = disp_data['center_xyz_m'][0] + disp_pose.position.y = disp_data['center_xyz_m'][1] + disp_pose.position.z = disp_data['center_xyz_m'][2] + + disp_pose.orientation.x = disp_data['orientation_xyzw'][0] + disp_pose.orientation.y = disp_data['orientation_xyzw'][1] + disp_pose.orientation.z = disp_data['orientation_xyzw'][2] + disp_pose.orientation.w = disp_data['orientation_xyzw'][3] + + dispenser.primitives.append(disp_box) + dispenser.primitive_poses.append(disp_pose) + + pub.publish(dispenser) + log.info("-> YAML 기반 디스펜서 장애물 동적 등록 완료!") + else: + log.warn("-> 디스펜서 설정 파일을 찾을 수 없어 장애물 등록을 건너뜁니다.") + + + def _select_target(self, detections): + """ + 현재 YOLO 모델의 실제 클래스 이름('cup')을 찾아 신뢰도가 가장 높은 객체를 선택 + """ + if not detections: + return None + + target_candidates = [d for d in detections if d["cls_name"] == "cup"] + + if not target_candidates: + return None + + return max(target_candidates, key=lambda d: d["conf"]) + + + + def detect_and_pick(self, frame: np.ndarray): + log = self.get_logger() + if self.picking: + log.warn("이미 시퀀스 실행 중입니다.") + return + + detections = self.run_yolo(frame) + self._detections = detections + target = self._select_target(detections) + + if target is None: + log.warn("쓰러진 컵을 찾을 수 없습니다.") + return + + base = self.pixel_to_base(target["cx"], target["cy"]) + if base is None: + log.error("픽셀 -> 베이스 3D 좌표 변환 실패.") + return + bx, by, bz = base + + + cup_theta = calculate_cup_orientation(self.depth_image, target["box"], frame) + + + # ========================================================== + + is_top = is_top_pointing_towards_theta(frame, target["box"], cup_theta) + + if not is_top: + log.info("[VISION] 컵이 반대로 누워있습니다. 카메라 상향 유지를 위해 파지 방향을 180도 뒤집습니다.") + cup_theta += np.pi # 180도 회전 + else: + log.info("[VISION] 컵이 정방향입니다. 기본 파지 방향을 유지합니다.") + # ========================================================== + + self.picking = True + try: + self._pick_and_straighten(bx, by, bz, cup_theta) + finally: + self.picking = False + + + + + def _pick_and_straighten(self, bx, by, bz, cup_theta): + log = self.get_logger() + + target_ori = get_gripper_pose_by_cup(cup_theta) + + + TABLE_Z = 0.0 + floor_z = TABLE_Z + + + Z_OFFSET = cfg.Z_OFFSET # 0.20m (20cm) + + PICK_CLEARANCE = 0.02 + + pick_z = floor_z + CUP_RADIUS_M + Z_OFFSET + PICK_CLEARANCE + place_z = floor_z + (CUP_LENGTH_M / 2.0) + + safe_z = floor_z + 0.25 + Z_OFFSET + + log.info(f"== 컵 구출 시퀀스 준비 (각도: {np.degrees(cup_theta):.1f}도) ==") + + + + log.info("[1-1] 상공 진입 (Z=25cm)") + + arm_component = self.robot.get_planning_component("manipulator") + arm_component.set_start_state_to_current_state() + current_state = arm_component.get_start_state() + + # 'link_6' 끝단의 현재 공간 좌표와 방향(Quaternion) 추출 + current_pose = current_state.get_pose("link_6") + + current_ori = { + "x": current_pose.orientation.x, + "y": current_pose.orientation.y, + "z": current_pose.orientation.z, + "w": current_pose.orientation.w + } + + # 추출한 현재 방향(current_ori)을 유지하면서 Z축만 상공으로 이동 + self.plan_pose(bx, by, safe_z, current_ori) + time.sleep(1.0) + + + log.info("[1-2] 상공에서 컵 방향으로 정렬") + self.plan_pose(bx, by, safe_z, target_ori) + time.sleep(1.0) + + log.info("[2] 컵 집기 시작") + self.plan_pose(bx, by, pick_z, target_ori) + self.gripper.close_gripper() + log.info("[2] 컵 집기 완료") + time.sleep(1.0) + + log.info("[3] Lift Up (다시 바닥 기준 25cm 상공으로 리프트업)") + self.plan_pose(bx, by, safe_z, target_ori) + time.sleep(1.0) + + # 직립화 실행 (항상 카메라가 위를 향하는 Roll=90 고정) + log.info("[4] 컵 직립화 궤적 탐색 (카메라 상향 고정)...") + + dx = (CUP_LENGTH_M / 2.0) * np.cos(cup_theta) + dy = (CUP_LENGTH_M / 2.0) * np.sin(cup_theta) + place_x = bx - dx + place_y = by - dy + + + + # 무조건 카메라가 위를 보는 자세(Roll=90) 쿼터니언 생성 + target_roll = 90 + quat_target = R.from_euler('xyz', [target_roll, 0, np.degrees(cup_theta)], degrees=True).as_quat() + ori_target = {"x": float(quat_target[0]), "y": float(quat_target[1]), "z": float(quat_target[2]), "w": float(quat_target[3])} + + log.info("-> 카메라 상향(Roll=90) 궤적 플래닝 시도 중...") + success = self.plan_pose(place_x, place_y, place_z + 0.15, ori_target) + + if success: + log.info("=> 카메라 상향 직립화 궤적 채택 성공!") + best_ori = ori_target + else: + log.error("=> 치명적 오류: 관절 한계로 인해 직립화 궤적 생성에 실패했습니다.") + return + + log.info("[4-1] 공중에서 컵 수직 정렬 완료") + + log.info(f"[4-2] Z-Height Adjustment (Z: {place_z:.3f})") + self.plan_pose(place_x, place_y, place_z + 0.02, best_ori) + + + log.info("[5] Place & Release") + self.plan_pose(place_x, place_y, place_z, best_ori) + self.gripper.open_gripper() + time.sleep(1.0) + + log.info("[6] Retract") + self.plan_pose(place_x, place_y, place_z + 0.15, best_ori) + log.info("== 시퀀스 완료 ==") + +def main(args=None): + run_node(YoloCupUprightingNode) + + +if __name__ == "__main__": + main() diff --git a/src/azas_cup_uprighting/config/measured_dispenser_collision.yaml b/src/azas_cup_uprighting/config/measured_dispenser_collision.yaml new file mode 100644 index 0000000..850fd95 --- /dev/null +++ b/src/azas_cup_uprighting/config/measured_dispenser_collision.yaml @@ -0,0 +1,66 @@ +# Measured dispenser collision draft from real robot teaching. +# +# IMPORTANT: +# - These values were measured as base_link -> link_6 probe poses while the +# gripper/link_6 assembly was placed against the dispenser. They are not +# direct surface coordinates and must be reviewed with TCP/gripper offset +# before enabling hard real-motion collision enforcement. +# - The boxes below are conservative primitive estimates for MoveIt Planning +# Scene. Keep them disabled until verified in RViz against the real workcell. + +metadata: + frame_id: base_link + measured_target_frame: link_6 + source: operator_teaching_tf2_echo + status: measured_draft_single_box_not_enabled + body_bottom_z_m: 0.000 + body_bottom_reason: dispenser bottles start on same floor plane as robot base + margin_m: + x: 0.020 + y: 0.020 + z: 0.020 + +front_hold_poses: + # Cup hold/front limit poses. Cup pose itself still comes from vision. + dispenser_1: + position_xyz_m: [0.609000, 0.070000, 0.087000] + quaternion_xyzw: [0.489000, 0.517000, 0.517000, 0.475000] + rpy_deg: [90.798, -0.845, 93.984] + dispenser_2: + position_xyz_m: [0.617000, 0.028000, 0.082000] + quaternion_xyzw: [0.504000, 0.504000, 0.500000, 0.491000] + rpy_deg: [90.946, -0.512, 90.501] + dispenser_3: + position_xyz_m: [0.616000, -0.026000, 0.079000] + quaternion_xyzw: [0.504000, 0.504000, 0.498000, 0.494000] + rpy_deg: [90.926, -0.278, 90.234] + dispenser_4: + position_xyz_m: [0.607000, -0.083000, 0.075000] + quaternion_xyzw: [0.511000, 0.498000, 0.492000, 0.499000] + rpy_deg: [91.042, -0.280, 88.871] + +raw_probe_poses: + left_front_bottom_probe: + position_xyz_m: [0.767, 0.072, 0.219] + quaternion_xyzw: [0.642, 0.732, 0.215, 0.079] + rpy_deg: [155.003, -9.233, 99.561] + right_back_top_probe: + position_xyz_m: [0.763, -0.103, 0.412] + quaternion_xyzw: [0.658, 0.654, 0.200, 0.313] + rpy_deg: [137.024, 8.372, 86.350] + +estimated_collision_objects: + dispenser_combined_body_box: + type: box + frame_id: base_link + # Single measured draft box from left-front-bottom and right-back-top + # link_6 probe poses. X/Y are expanded by metadata.margin_m on each side; + # Z uses body_bottom_z_m as the dispenser rests on the base/table plane. + center_xyz_m: [0.7650, -0.0155, 0.2160] + size_xyz_m: [0.0440, 0.2150, 0.4320] + bounds_xyz_m: + min: [0.7430, -0.1230, 0.0000] + max: [0.7870, 0.0920, 0.4320] + orientation_xyzw: [0.0, 0.0, 0.0, 1.0] + publish_to_planning_scene: true + enabled_for_real_motion: false diff --git a/src/azas_cup_uprighting/config/moveit_py.yaml b/src/azas_cup_uprighting/config/moveit_py.yaml new file mode 100644 index 0000000..0e89b41 --- /dev/null +++ b/src/azas_cup_uprighting/config/moveit_py.yaml @@ -0,0 +1,54 @@ +/**: + ros__parameters: + planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + + planning_pipelines: + pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "ompl_rrt_star"] + + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 0.1 + max_acceleration_scaling_factor: 0.1 + + ompl_rrtc: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + planner_id: "RRTConnectkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.0 + + ompl_rrt_star: + plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl_rrt_star + planner_id: "RRTstarkConfigDefault" + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 + + pilz_lin: + plan_request_params: + planning_attempts: 1 + planning_pipeline: pilz_industrial_motion_planner + planner_id: "PTP" + max_velocity_scaling_factor: 0.1 + max_acceleration_scaling_factor: 0.1 + planning_time: 0.8 + + chomp: + plan_request_params: + planning_attempts: 1 + planning_pipeline: chomp + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + planning_time: 1.5 \ No newline at end of file diff --git a/src/azas_cup_uprighting/config/safety.yaml b/src/azas_cup_uprighting/config/safety.yaml new file mode 100644 index 0000000..4194897 --- /dev/null +++ b/src/azas_cup_uprighting/config/safety.yaml @@ -0,0 +1,21 @@ +motion: + max_velocity_scale: 0.1 # 초기 dry-run 안전 제한; 실제 운영 전 확인 필요 + max_acceleration_scale: 0.1 # 초기 dry-run 안전 제한; 실제 운영 전 확인 필요 + # Base_link workspace derived from the measured table bounds in + # calibration.yaml plus a 0.10 m perimeter margin in X/Y. + workspace_bounds_m: + x_min: -0.250 + x_max: 1.150 + y_min: -0.600 + y_max: 0.600 + z_min: 0.070 + z_max: 0.800 + min_z_m: 0.070 # link_6/EE target lower bound for side-grip motion +gripper: + default_width_m: null # 확인 필요: RG2 단위/명령 범위 + default_force_n: null # 확인 필요 + timeout_s: 5.0 +failure_behavior: + on_detection_failure: abort_without_motion + on_tf_failure: abort_without_motion + on_plan_failure: stop_before_execution diff --git a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py new file mode 100644 index 0000000..912cdd6 --- /dev/null +++ b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py @@ -0,0 +1,41 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + # 1. 두산 M0609 로봇의 MoveIt 파라미터 빌드 (URDF, SRDF, Kinematics 등) + moveit_config = ( + MoveItConfigsBuilder( + robot_name="m0609", + package_name="dsr_moveit_config_m0609", + ) + .robot_description() + .robot_description_semantic(file_path="config/dsr.srdf") + .robot_description_kinematics() + .joint_limits() + .trajectory_execution() + .planning_scene_monitor() + .sensors_3d() + .to_moveit_configs() + ) + + # 2. 패키지 내 config/moveit_py.yaml 경로 설정 + moveit_py_params = PathJoinSubstitution( + [FindPackageShare("azas_cup_uprighting"), "config", "moveit_py.yaml"] + ) + + # 3. 컵 직립화(Uprighting) 노드 실행 및 파라미터 주입 + yolo_cup_uprighting_node = Node( + package="azas_cup_uprighting", + executable="yolo_cup_uprighting", + name="yolo_cup_uprighting_py", + output="screen", + parameters=[ + moveit_config.to_dict(), + moveit_py_params, + ], + ) + + return LaunchDescription([yolo_cup_uprighting_node]) \ No newline at end of file diff --git a/src/azas_cup_uprighting/package.xml b/src/azas_cup_uprighting/package.xml new file mode 100644 index 0000000..59c6c88 --- /dev/null +++ b/src/azas_cup_uprighting/package.xml @@ -0,0 +1,33 @@ + + + + azas_cup_uprighting + 0.0.0 + YOLO-based pick and place for Doosan M0609 with RealSense depth camera + deeptree + TODO: License declaration + + rclpy + sensor_msgs + geometry_msgs + cv_bridge + dsr_msgs2 + moveit_py + moveit_configs_utils + dsr_moveit_config_m0609 + + python3-pymodbus + python3-opencv + python3-numpy + shape_msgs + moveit_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/azas_cup_uprighting/resource/azas_cup_uprighting b/src/azas_cup_uprighting/resource/azas_cup_uprighting new file mode 100644 index 0000000..e69de29 diff --git a/src/azas_cup_uprighting/setup.cfg b/src/azas_cup_uprighting/setup.cfg new file mode 100644 index 0000000..cb62885 --- /dev/null +++ b/src/azas_cup_uprighting/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/azas_cup_uprighting +[install] +install_scripts=$base/lib/azas_cup_uprighting diff --git a/src/azas_cup_uprighting/setup.py b/src/azas_cup_uprighting/setup.py new file mode 100644 index 0000000..2eed834 --- /dev/null +++ b/src/azas_cup_uprighting/setup.py @@ -0,0 +1,35 @@ +from setuptools import find_packages, setup +from glob import glob + +package_name = 'azas_cup_uprighting' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/launch', glob('launch/*.launch.py')), + ( + 'share/' + package_name + '/config', + glob('config/*.yaml') + glob('config/*.pt') + glob('azas_cup_uprighting/*.npy') + ), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='deeptree', + maintainer_email='deeptree@todo.todo', + description='YOLO-based pick and place for Doosan M0609 with RealSense depth camera', + license='TODO: License declaration', + extras_require={ + 'test': ['pytest'], + }, + entry_points={ + 'console_scripts': [ + + 'yolo_cup_uprighting = azas_cup_uprighting.yolo_cup_uprighting_node:main', + ], + }, +) From eeabf8161bc451e28d9281dc5310579a63e2e9ae Mon Sep 17 00:00:00 2001 From: suuu0719 Date: Fri, 5 Jun 2026 18:00:29 +0900 Subject: [PATCH 22/88] fix: expose dispenser simulation launch parameters --- .../dispenser_press_moveit_node.py | 18 +++-- .../launch/dispenser_press.launch.py | 34 ++++++++- .../launch/dispenser_press_moveit.launch.py | 73 ++++++++++++++++++- 3 files changed, 116 insertions(+), 9 deletions(-) diff --git a/src/azas_dispenser/azas_dispenser/dispenser_press_moveit_node.py b/src/azas_dispenser/azas_dispenser/dispenser_press_moveit_node.py index 730bc89..e00e773 100644 --- a/src/azas_dispenser/azas_dispenser/dispenser_press_moveit_node.py +++ b/src/azas_dispenser/azas_dispenser/dispenser_press_moveit_node.py @@ -8,11 +8,13 @@ from moveit_msgs.action import MoveGroup from moveit_msgs.msg import Constraints, JointConstraint, MoveItErrorCodes from moveit_msgs.srv import GetPositionIK +from rcl_interfaces.msg import ParameterDescriptor from rclpy.action import ActionClient def get_param(node, name, default): - node.declare_parameter(name, default) + descriptor = ParameterDescriptor(dynamic_typing=True) + node.declare_parameter(name, default, descriptor) return node.get_parameter(name).value @@ -123,6 +125,9 @@ def __init__(self): self.dispenser_top_z = float(get_param(self.node, "dispenser_top_z", 0.38)) # Support taught PRESS poses (x, y, z, rx, ry, rz) in millimetres/degrees self.use_taught_posx = bool(get_param(self.node, "use_taught_posx", False)) + self.use_taught_orientation = bool( + get_param(self.node, "use_taught_orientation", False) + ) self.target_dispenser = str(get_param(self.node, "target_dispenser", "red")) self.taught_posx_by_name = { "red": [float(v) for v in get_param(self.node, "red_top_posx", [])], @@ -361,15 +366,16 @@ def build_steps(self): y_m = top_pose[1] / 1000.0 top_z_m = top_pose[2] / 1000.0 rx_deg, ry_deg, rz_deg = top_pose[3:6] + taught_rpy = [rx_deg, ry_deg, rz_deg] if self.use_taught_orientation else None approach_z = top_z_m + self.approach_height pressed_z = top_z_m - self.press_depth steps = [ (self.home_tcp[0], self.home_tcp[1], self.home_tcp[2] + self.home_lift_height, "lift above HOME", None), - (x_m, y_m, approach_z, "approach above dispenser", [rx_deg, ry_deg, rz_deg]), - (x_m, y_m, top_z_m, "move to dispenser top", [rx_deg, ry_deg, rz_deg]), - (x_m, y_m, pressed_z, "press dispenser pump", [rx_deg, ry_deg, rz_deg]), - (x_m, y_m, approach_z, "retreat above dispenser", [rx_deg, ry_deg, rz_deg]), + (x_m, y_m, approach_z, "approach above dispenser", taught_rpy), + (x_m, y_m, top_z_m, "move to dispenser top", taught_rpy), + (x_m, y_m, pressed_z, "press dispenser pump", taught_rpy), + (x_m, y_m, approach_z, "retreat above dispenser", taught_rpy), ] for x_value, y_value, z_value, label, _ in steps: self.logger.info( @@ -394,7 +400,7 @@ def build_steps(self): (x, y, pressed_z, "press dispenser pump", None), (x, y, approach_z, "retreat above dispenser", None), ] - for x_value, y_value, z_value, label in steps: + for x_value, y_value, z_value, label, _ in steps: self.logger.info( f"Queued step: {label} -> " f"x={x_value:.3f}, y={y_value:.3f}, z={z_value:.3f}" diff --git a/src/azas_dispenser/launch/dispenser_press.launch.py b/src/azas_dispenser/launch/dispenser_press.launch.py index 6ae34f5..91da11f 100644 --- a/src/azas_dispenser/launch/dispenser_press.launch.py +++ b/src/azas_dispenser/launch/dispenser_press.launch.py @@ -2,19 +2,26 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): dispenser_press_params = { # Use "/" when Doosan services are not namespaced. "service_prefix": LaunchConfiguration("service_prefix"), + "tcp_name": LaunchConfiguration("tcp_name"), + "restore_tcp_after_run": LaunchConfiguration("restore_tcp_after_run"), + "require_tcp_for_taught_posx": LaunchConfiguration("require_tcp_for_taught_posx"), + "allow_tcp_set_failure": LaunchConfiguration("allow_tcp_set_failure"), "close_gripper_at_home": LaunchConfiguration("close_gripper_at_home"), "gripper_service": LaunchConfiguration("gripper_service"), "gripper_close_width": LaunchConfiguration("gripper_close_width"), "gripper_close_force": LaunchConfiguration("gripper_close_force"), "gripper_wait_timeout": LaunchConfiguration("gripper_wait_timeout"), # True이면 실제로 찍어둔 색상별 펌프 상단 TCP 좌표를 사용합니다. - "use_taught_posx": True, + "use_taught_posx": ParameterValue( + LaunchConfiguration("use_taught_posx"), value_type=bool + ), "target_dispenser": LaunchConfiguration("target_dispenser"), "red_top_posx": [ 732.1023559570312, @@ -94,6 +101,31 @@ def generate_launch_description(): default_value="red", description="Dispenser color to press: red, green, yellow, or blue.", ), + DeclareLaunchArgument( + "use_taught_posx", + default_value="true", + description="Use color-specific taught dispenser TCP poses.", + ), + DeclareLaunchArgument( + "tcp_name", + default_value="", + description="Doosan controller TCP name to activate before taught-posx press.", + ), + DeclareLaunchArgument( + "restore_tcp_after_run", + default_value="true", + description="Restore the previous Doosan TCP after the press sequence.", + ), + DeclareLaunchArgument( + "require_tcp_for_taught_posx", + default_value="true", + description="Require a named TCP when using taught dispenser posx targets.", + ), + DeclareLaunchArgument( + "allow_tcp_set_failure", + default_value="false", + description="Continue with the current controller TCP if tcp/set_current_tcp fails.", + ), DeclareLaunchArgument( "close_gripper_at_home", default_value="true", diff --git a/src/azas_dispenser/launch/dispenser_press_moveit.launch.py b/src/azas_dispenser/launch/dispenser_press_moveit.launch.py index 18f8113..91459a3 100644 --- a/src/azas_dispenser/launch/dispenser_press_moveit.launch.py +++ b/src/azas_dispenser/launch/dispenser_press_moveit.launch.py @@ -1,5 +1,8 @@ from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): @@ -9,8 +12,17 @@ def generate_launch_description(): "ee_link": "rg2_tcp", "ik_link_name": "link_6", "tool_offset_xyz": [0.0, 0.0, 0.27], - "service_prefix": "/", - "keep_home_pose_from_controller": True, + "service_prefix": LaunchConfiguration("service_prefix"), + "keep_home_pose_from_controller": ParameterValue( + LaunchConfiguration("keep_home_pose_from_controller"), value_type=bool + ), + "use_taught_posx": ParameterValue( + LaunchConfiguration("use_taught_posx"), value_type=bool + ), + "use_taught_orientation": ParameterValue( + LaunchConfiguration("use_taught_orientation"), value_type=bool + ), + "target_dispenser": LaunchConfiguration("target_dispenser"), "joint_names": [ "joint_1", "joint_2", @@ -24,6 +36,38 @@ def generate_launch_description(): # Position unit: meter, orientation unit: degree. "home_tcp": [0.368, 0.00625, 0.425], "home_rpy_deg": [45.0, 180.0, 45.0], + "red_top_posx": [ + 732.1023559570312, + 64.33094787597656, + 379.1507568359375, + 174.0473175048828, + -118.16372680664062, + -149.73670959472656, + ], + "green_top_posx": [ + 733.4710083007812, + 3.988441228866577, + 379.1507568359375, + 168.5689239501953, + -117.13253784179688, + -149.81581115722656, + ], + "yellow_top_posx": [ + 736.9231567382812, + -54.69612121582031, + 379.1507568359375, + 164.23757934570312, + -114.83785247802734, + -150.598876953125, + ], + "blue_top_posx": [ + 730.6580200195312, + -109.8679428100586, + 379.1507568359375, + 158.76589965820312, + -114.91173553466797, + -156.96270751953125, + ], # Dispenser pump target in base_link frame. "dispenser_x": 0.50, "dispenser_y": 0.00, @@ -44,6 +88,31 @@ def generate_launch_description(): return LaunchDescription( [ + DeclareLaunchArgument( + "service_prefix", + default_value="dsr01", + description="Doosan service namespace for optional controller pose reads.", + ), + DeclareLaunchArgument( + "keep_home_pose_from_controller", + default_value="false", + description="Read HOME TCP pose from Doosan services before MoveIt execution.", + ), + DeclareLaunchArgument( + "use_taught_posx", + default_value="true", + description="Use color-specific taught dispenser poses.", + ), + DeclareLaunchArgument( + "use_taught_orientation", + default_value="false", + description="Use taught dispenser RPY values instead of the HOME RPY.", + ), + DeclareLaunchArgument( + "target_dispenser", + default_value="red", + description="Dispenser color to press: red, green, yellow, or blue.", + ), Node( package="azas_dispenser", executable="dispenser_press_moveit_node", From ab7742fa7ceb8dd4b03c23951a111cf69800bc2e Mon Sep 17 00:00:00 2001 From: suuu0719 Date: Fri, 5 Jun 2026 18:00:55 +0900 Subject: [PATCH 23/88] feat: add trait-based voice recommendations --- src/azas_voice/azas_voice/command_parser.py | 133 ++++++++++- .../azas_voice/llm_recipe_mapper_node.py | 100 +++++++- src/azas_voice/azas_voice/recipe_catalog.py | 223 +++++++++++++++++- src/azas_voice/azas_voice/tts_node.py | 2 +- src/azas_voice/config/recipes.yaml | 18 ++ src/azas_voice/launch/azas_voice.launch.py | 9 +- src/azas_voice/test/test_command_parser.py | 112 ++++++++- src/azas_voice/test/test_llm_recipe_mapper.py | 205 +++++++++++++++- 8 files changed, 779 insertions(+), 23 deletions(-) diff --git a/src/azas_voice/azas_voice/command_parser.py b/src/azas_voice/azas_voice/command_parser.py index 43511b8..98fe270 100644 --- a/src/azas_voice/azas_voice/command_parser.py +++ b/src/azas_voice/azas_voice/command_parser.py @@ -4,14 +4,20 @@ import random from azas_voice.recipe_catalog import ( + AVOID_TRAIT_KEYWORDS, CANCEL_WORDS, COLOR_ALIASES, CONFIRM_WORDS, + DISPENSER_TRAITS, MOOD_WORDS, + PREFERENCE_WORDS, RANDOM_RECIPE_WORDS, + REROLL_RECOMMENDATION_WORDS, RECIPE_ALIASES, + RECIPE_DESCRIPTIONS, RECIPE_DISPENSERS, RECIPE_DISPLAY_NAMES, + TRAIT_KEYWORDS, ) @@ -25,9 +31,11 @@ class RecipeDecision: dispenser_ids: tuple[str, ...] confirmation: str error: str | None = None + profile: dict[str, str] | None = None + dispenser_amounts: dict[str, int] | None = None def to_dict(self) -> dict[str, object]: - return { + payload: dict[str, object] = { "valid": self.valid, "utterance": self.utterance, "normalized": self.normalized, @@ -37,6 +45,11 @@ def to_dict(self) -> dict[str, object]: "confirmation": self.confirmation, "error": self.error, } + if self.profile is not None: + payload["profile"] = self.profile + if self.dispenser_amounts is not None: + payload["dispenser_amounts"] = self.dispenser_amounts + return payload def normalize_text(text: str) -> str: @@ -74,16 +87,116 @@ def _recipe_name(recipe_id: str) -> str: return RECIPE_DISPLAY_NAMES.get(recipe_id, recipe_id) +def _recipe_description(recipe_id: str) -> str: + return RECIPE_DESCRIPTIONS.get(recipe_id, "") + + def _random_recipe_decision(utterance: str, normalized: str) -> RecipeDecision: recipe_id = random.choice(tuple(RECIPE_DISPENSERS)) dispenser_ids = RECIPE_DISPENSERS[recipe_id] + description = _recipe_description(recipe_id) confirmation = ( - f"오늘 기분에는 {_recipe_name(recipe_id)}를 추천합니다. " - f"진행할까요?" + f"{_recipe_name(recipe_id)}를 추천드릴게요. " + f"{description} 진행할까요?" ) return RecipeDecision(True, utterance, normalized, "make_cocktail", recipe_id, dispenser_ids, confirmation) +def _level_text(amount: int, zero: str, low: str, normal: str, high: str) -> str: + if amount <= 0: + return zero + if amount == 1: + return low + if amount == 2: + return normal + return high + + +def _extract_traits(normalized: str) -> tuple[tuple[str, ...], tuple[str, ...]]: + wanted = { + trait + for trait, keywords in TRAIT_KEYWORDS.items() + if _contains_any(normalized, keywords) + } + avoided = { + trait + for trait, keywords in AVOID_TRAIT_KEYWORDS.items() + if _contains_any(normalized, keywords) + } + wanted -= avoided + if "bitterness" in avoided: + wanted.update({"sweetness", "fruitiness"}) + wanted -= avoided + return tuple(sorted(wanted)), tuple(sorted(avoided)) + + +def _amount_from_score(score: float) -> int: + if score <= 0.0: + return 0 + if score <= 1.0: + return 1 + if score <= 2.0: + return 2 + return 3 + + +def amounts_from_traits( + wanted_traits: tuple[str, ...], + avoided_traits: tuple[str, ...], + normalized: str = "", +) -> dict[str, int]: + wanted = set(wanted_traits) + avoided = set(avoided_traits) + scores = {color: 1.0 for color in ("red", "yellow", "green", "blue")} + + for color, traits in DISPENSER_TRAITS.items(): + trait_set = set(traits) + scores[color] += 1.25 * len(wanted & trait_set) + scores[color] -= 0.75 * len(avoided & trait_set) + + amounts = { + color: max(1, _amount_from_score(score)) + for color, score in scores.items() + } + + if _contains_any(normalized, ("무알콜", "논알콜", "알코올없이", "술없이", "럼없이")): + amounts["blue"] = 0 + + return amounts + + +def profile_from_amounts(amounts: dict[str, int]) -> dict[str, str]: + return { + "rum": _level_text(amounts["blue"], "없음", "약하게", "보통", "강하게"), + "syrup": _level_text(amounts["yellow"], "없음", "적게", "보통", "많게"), + "liqueur": _level_text(amounts["green"], "없음", "적게", "보통", "많게"), + "juice": _level_text(amounts["red"], "없음", "적게", "보통", "많게"), + } + + +def _custom_preference_decision(utterance: str, normalized: str) -> RecipeDecision: + wanted_traits, avoided_traits = _extract_traits(normalized) + amounts = amounts_from_traits(wanted_traits, avoided_traits, normalized) + + dispenser_ids = tuple(color for color in ("red", "yellow", "green", "blue") if amounts[color] > 0) + profile = profile_from_amounts(amounts) + summary = ( + f"말씀하신 취향에는 럼 {profile['rum']}, 시럽 {profile['syrup']}, " + f"리큐르 {profile['liqueur']}, 주스 {profile['juice']} 조합을 추천드릴게요. 진행할까요?" + ) + return RecipeDecision( + True, + utterance, + normalized, + "make_cocktail", + "custom_preference_mix", + dispenser_ids, + summary, + profile=profile, + dispenser_amounts=amounts, + ) + + def parse_recipe_command(text: str) -> RecipeDecision: utterance = text.strip() normalized = normalize_text(utterance) @@ -91,18 +204,26 @@ def parse_recipe_command(text: str) -> RecipeDecision: if not normalized: return RecipeDecision(False, utterance, normalized, "unknown", None, (), "", "empty utterance") + if _contains_any(normalized, REROLL_RECOMMENDATION_WORDS): + return _random_recipe_decision(utterance, normalized) + if _contains_any(normalized, CANCEL_WORDS): return RecipeDecision(True, utterance, normalized, "cancel", None, (), "칵테일 제조 요청을 취소합니다.") - if _contains_any(normalized, CONFIRM_WORDS): - return RecipeDecision(True, utterance, normalized, "confirm", None, (), "선택한 칵테일 제조를 확인했습니다.") - recipe_id = _match_recipe(normalized) dispenser_ids = _match_colors(normalized) if recipe_id is None and not dispenser_ids and _is_random_recipe_request(normalized): + if _contains_any(normalized, PREFERENCE_WORDS): + return _custom_preference_decision(utterance, normalized) return _random_recipe_decision(utterance, normalized) + if recipe_id is None and not dispenser_ids and _contains_any(normalized, PREFERENCE_WORDS): + return _custom_preference_decision(utterance, normalized) + + if recipe_id is None and not dispenser_ids and _contains_any(normalized, CONFIRM_WORDS): + return RecipeDecision(True, utterance, normalized, "confirm", None, (), "선택한 칵테일 제조를 확인했습니다.") + if recipe_id is None and not dispenser_ids: return RecipeDecision( False, diff --git a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py index 6e92dde..00516f9 100644 --- a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py +++ b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py @@ -11,17 +11,25 @@ String = None Node = object -from azas_voice.command_parser import RecipeDecision, parse_recipe_command -from azas_voice.recipe_catalog import COLOR_ALIASES, RECIPE_DISPENSERS, RECIPE_DISPLAY_NAMES +from azas_voice.command_parser import RecipeDecision, amounts_from_traits, parse_recipe_command, profile_from_amounts +from azas_voice.recipe_catalog import ( + COLOR_ALIASES, + DISPENSER_TRAITS, + RECIPE_DESCRIPTIONS, + RECIPE_DISPENSERS, + RECIPE_DISPLAY_NAMES, +) ALLOWED_INTENTS = {"make_cocktail", "confirm", "cancel", "unknown"} +ALLOWED_CUSTOM_RECIPE_IDS = {"custom_color_selection", "custom_preference_mix"} DISPENSER_NUMBER_TO_COLOR = { "1": "red", "2": "yellow", "3": "green", "4": "blue", } +ALLOWED_TRAITS = set().union(*DISPENSER_TRAITS.values()) def _normalize_dispenser_id(value: object) -> str: @@ -37,6 +45,17 @@ def _normalize_dispenser_id(value: object) -> str: return "" +def _normalize_traits(value: object) -> tuple[str, ...]: + if not isinstance(value, list): + return () + traits: list[str] = [] + for item in value: + trait = str(item).strip().lower() + if trait in ALLOWED_TRAITS and trait not in traits: + traits.append(trait) + return tuple(traits) + + def _fallback_decision(text: str, reason: str = "") -> RecipeDecision: decision = parse_recipe_command(text) if decision.valid or not reason: @@ -58,16 +77,43 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: if intent not in ALLOWED_INTENTS: return _fallback_decision(text, f"invalid_intent:{intent}") + fallback = parse_recipe_command(text) + if fallback.valid and fallback.intent in {"confirm", "cancel"}: + return fallback + if fallback.valid and fallback.intent == "make_cocktail" and fallback.recipe_id in RECIPE_DISPENSERS and "추천" in fallback.confirmation: + return fallback + dispenser_ids = tuple( dispenser_id for dispenser_id in (_normalize_dispenser_id(item) for item in payload.get("dispenser_ids", [])) if dispenser_id ) + amounts_payload = payload.get("dispenser_amounts", {}) + dispenser_amounts: dict[str, int] = {} + if isinstance(amounts_payload, dict): + for color in ("red", "yellow", "green", "blue"): + try: + amount = int(amounts_payload.get(color, 0)) + except (TypeError, ValueError): + amount = 0 + dispenser_amounts[color] = max(0, min(amount, 3)) + + wanted_traits = _normalize_traits(payload.get("wanted_traits", [])) + avoided_traits = _normalize_traits(payload.get("avoided_traits", [])) + if intent == "make_cocktail" and (wanted_traits or avoided_traits): + dispenser_amounts = amounts_from_traits(wanted_traits, avoided_traits, fallback.normalized) + dispenser_ids = tuple(color for color in ("red", "yellow", "green", "blue") if dispenser_amounts[color] > 0) + + if not dispenser_ids and any(dispenser_amounts.values()): + dispenser_ids = tuple(color for color in ("red", "yellow", "green", "blue") if dispenser_amounts[color] > 0) + recipe_id = payload.get("recipe_id") recipe_id = str(recipe_id).strip() if recipe_id else None - if recipe_id and not recipe_id.startswith("recipe_") and recipe_id != "custom_color_selection": + if intent == "make_cocktail" and (wanted_traits or avoided_traits): + recipe_id = "custom_preference_mix" + if recipe_id and not recipe_id.startswith("recipe_") and recipe_id not in ALLOWED_CUSTOM_RECIPE_IDS: recipe_id = None - if recipe_id and recipe_id != "custom_color_selection" and not dispenser_ids: + if recipe_id and recipe_id not in ALLOWED_CUSTOM_RECIPE_IDS: dispenser_ids = RECIPE_DISPENSERS.get(recipe_id, ()) if intent == "make_cocktail" and recipe_id is None and not dispenser_ids: @@ -75,19 +121,48 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: valid = intent in {"make_cocktail", "confirm", "cancel"} if intent == "make_cocktail" and recipe_id is None: - recipe_id = "custom_color_selection" + recipe_id = "custom_preference_mix" if dispenser_amounts else "custom_color_selection" + + if intent == "make_cocktail" and fallback.recipe_id == "custom_preference_mix": + recipe_id = "custom_preference_mix" + if not any(dispenser_amounts.values()) and fallback.dispenser_amounts: + dispenser_amounts = dict(fallback.dispenser_amounts) + if any(dispenser_amounts.values()): + dispenser_ids = tuple( + color for color in ("red", "yellow", "green", "blue") if dispenser_amounts[color] > 0 + ) + elif not dispenser_ids and fallback.dispenser_ids: + dispenser_ids = fallback.dispenser_ids confirmation = str(payload.get("confirmation", "")).strip() + if confirmation.lower() in {"false", "none", "null"}: + confirmation = "" if valid and not confirmation: if intent == "cancel": confirmation = "칵테일 제조 요청을 취소합니다." elif intent == "confirm": confirmation = "선택한 칵테일 제조를 확인했습니다." + elif recipe_id == "custom_preference_mix" and fallback.confirmation: + confirmation = fallback.confirmation + elif fallback.confirmation and "추천" in fallback.confirmation: + recipe_name = RECIPE_DISPLAY_NAMES.get(str(recipe_id), str(recipe_id)) + description = RECIPE_DESCRIPTIONS.get(str(recipe_id), "") + confirmation = f"{recipe_name}를 추천드릴게요. {description} 진행할까요?" else: recipe_name = RECIPE_DISPLAY_NAMES.get(str(recipe_id), str(recipe_id)) confirmation = f"{recipe_name} 요청을 인식했습니다. 진행할까요?" - fallback = parse_recipe_command(text) + profile = payload.get("profile") + if recipe_id == "custom_preference_mix" and any(dispenser_amounts.values()): + profile = profile_from_amounts(dispenser_amounts) + if recipe_id != "custom_preference_mix": + profile = None + if not isinstance(profile, dict): + profile = fallback.profile if recipe_id == "custom_preference_mix" else None + if recipe_id == "custom_preference_mix" and fallback.profile: + expected_profile_keys = {"rum", "syrup", "liqueur", "juice"} + if not profile or set(profile.keys()) != expected_profile_keys: + profile = fallback.profile return RecipeDecision( valid, text.strip(), @@ -97,6 +172,8 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: dispenser_ids, confirmation, None if valid else "llm returned unknown intent", + profile={str(k): str(v) for k, v in profile.items()} if profile else None, + dispenser_amounts=dispenser_amounts if any(dispenser_amounts.values()) else None, ) @@ -184,17 +261,22 @@ def _call_chat_api(self, text: str, api_key: str) -> dict: "role": "system", "content": ( "Return only JSON for Azas cocktail intent parsing. " - "Allowed fields: valid, intent, recipe_id, dispenser_ids, confirmation. " + "Allowed fields: valid, intent, recipe_id, dispenser_ids, confirmation, wanted_traits, avoided_traits. " "Allowed intents: make_cocktail, confirm, cancel, unknown. " - "The user does not know dispenser colors; infer them internally. " - "If the user describes mood or asks for a recommendation, choose one recipe_01..recipe_04. " + "For descriptive preference or recommendation requests, extract wanted_traits and avoided_traits instead of calculating amounts. " + "Allowed traits: sweetness, fruitiness, freshness, aroma, alcohol, bitterness, softness, light, depth, herbal. " + "Examples of preferences: not too strong, light, easy to drink, rich aroma, sweet, not sweet, fruity. " + "For a plain recommendation with no preferences, choose one recipe_01..recipe_04. " + "Only choose recipe_01..recipe_04 when the user explicitly asks for a numbered/color menu or gives no preferences. " "Allowed dispenser_ids values: red, yellow, green, blue only. " + "Do not output dispenser_amounts; the application calculates amounts from traits. " "Never output robot coordinates, calibration values, trajectories, or safety approvals." ), }, {"role": "user", "content": text}, ], "temperature": 0.0, + "response_format": {"type": "json_object"}, } data = json.dumps(body).encode("utf-8") req = request.Request( diff --git a/src/azas_voice/azas_voice/recipe_catalog.py b/src/azas_voice/azas_voice/recipe_catalog.py index e506fed..688ae8b 100644 --- a/src/azas_voice/azas_voice/recipe_catalog.py +++ b/src/azas_voice/azas_voice/recipe_catalog.py @@ -9,8 +9,115 @@ # azas_dispenser launch parameters expect target_dispenser:=red|yellow|green|blue. COLOR_ALIASES = DISPENSER_ALIASES -# Recipe names and actual ingredients are intentionally symbolic until the team -# confirms which ingredient is loaded into each color-sticker dispenser. +# Ingredient roles are symbolic voice/order semantics. Robot coordinates and +# calibration values are intentionally not stored here. +DISPENSER_ROLES = { + "blue": { + "role": "rum", + "label": "럼", + "levels": ("없음", "약하게", "보통", "강하게"), + }, + "yellow": { + "role": "syrup", + "label": "시럽", + "levels": ("적게", "보통", "많게"), + }, + "green": { + "role": "liqueur", + "label": "리큐르", + "levels": ("적게", "보통", "많게"), + }, + "red": { + "role": "juice", + "label": "주스", + "levels": ("적게", "보통", "많게"), + }, +} + +DISPENSER_TRAITS = { + "red": ("fruitiness", "freshness", "light", "sweetness"), + "yellow": ("sweetness", "softness"), + "green": ("aroma", "herbal", "bitterness"), + "blue": ("alcohol", "depth", "bitterness"), +} + +TRAIT_DISPLAY_NAMES = { + "sweetness": "단맛", + "fruitiness": "과일감", + "freshness": "상큼함", + "aroma": "향", + "alcohol": "도수", + "bitterness": "쓴맛", + "softness": "부드러움", + "light": "가벼움", + "depth": "깊이감", + "herbal": "허브향", +} + +TRAIT_KEYWORDS = { + "sweetness": ( + "달달", + "달게", + "달콤", + "달아", + "시럽", + "기분안좋", + "우울", + "힘들", + "피곤", + "스트레스", + "답답", + ), + "fruitiness": ( + "과일", + "과일맛", + "주스", + "상큼", + "새콤", + "기분좋", + "행복", + "신나", + "기뻐", + "상쾌", + "설레", + "기분안좋", + "우울", + "힘들", + "피곤", + ), + "freshness": ("상큼", "새콤", "산미", "산뜻", "상쾌"), + "aroma": ("향", "향좋", "향진", "향강", "풍부", "리큐르", "기분좋", "행복", "신나"), + "alcohol": ("도수쎈", "도수센", "도수쌘", "도수높", "쎈술", "센술", "쌘술", "강한술", "술강", "럼강", "강하게"), + "bitterness": ("쓴맛", "쓴술", "쌉싸름", "허브", "드라이"), + "softness": ("부드럽", "순하게", "편한"), + "light": ("가볍", "부담", "편한", "세지않", "안세"), + "depth": ("깊", "묵직"), + "herbal": ("허브", "리큐르", "향"), +} + +AVOID_TRAIT_KEYWORDS = { + "sweetness": ("덜달", "안달", "달지않", "시럽적"), + "alcohol": ( + "무알콜", + "논알콜", + "알코올없이", + "술없이", + "럼없이", + "술약", + "럼약", + "도수낮", + "약하게", + "세지않", + "안세", + "술은싫", + "술싫", + "독한술싫", + "독한건싫", + ), + "bitterness": ("쓴맛싫", "쓴술싫", "쓴맛나는술은싫", "쓴맛나는술싫", "독한술싫", "독한건싫"), + "aroma": ("향약", "리큐르적"), +} + RECIPE_ALIASES = { "recipe_01": ("1번", "일번", "레시피1", "recipe1", "recipe_01", "레드메뉴", "빨강메뉴", "빨간색메뉴"), "recipe_02": ("2번", "이번", "레시피2", "recipe2", "recipe_02", "옐로우메뉴", "노랑메뉴", "노란색메뉴"), @@ -25,6 +132,13 @@ "recipe_04": "블루 메뉴", } +RECIPE_DESCRIPTIONS = { + "recipe_01": "주스 중심이라 과일감이 선명하고 가볍게 마시기 좋습니다.", + "recipe_02": "시럽 중심이라 달콤하고 부드러운 느낌이 강합니다.", + "recipe_03": "리큐르 중심이라 향이 선명하고 깔끔한 여운이 있습니다.", + "recipe_04": "럼 중심이라 칵테일다운 존재감과 깊이가 있습니다.", +} + RECIPE_DISPENSERS = { "recipe_01": ("red",), "recipe_02": ("yellow",), @@ -59,18 +173,123 @@ "알려줘", ) +REROLL_RECOMMENDATION_WORDS = ( + "다른거", + "다른것", + "다른메뉴", + "다른걸", + "다른걸로", + "다른거로", + "말고다른", + "말고다른거", + "말고다른메뉴", + "새로추천", + "다시추천", +) + +PREFERENCE_WORDS = ( + "덜달", + "안달", + "달달", + "달게", + "달콤", + "시럽", + "상큼", + "새콤", + "신맛", + "산미", + "주스", + "세지", + "세지않", + "안세", + "부담", + "가볍", + "편한", + "기분안좋", + "안좋", + "기분좋", + "행복", + "신나", + "우울", + "힘들", + "피곤", + "술약", + "약하게", + "술강", + "강하게", + "도수", + "럼", + "무알콜", + "논알콜", + "알코올없이", + "술없이", + "리큐르", + "쓴맛", + "쓴술", + "술은싫", + "술싫", + "독한술", + "독한건싫", + "과일", + "과일맛", + "향", + "풍부", + "진하게", + "깔끔", +) + CONFIRM_WORDS = ( "확인", + "확인해", + "확인해줘", + "확정", + "확정해", + "확정해줘", "맞아", + "맞아요", "맞습니다", "응", + "응응", "네", + "넵", + "넹", "예", + "예스", + "yes", + "ok", + "okay", + "오케이", + "오키", + "그래", + "그렇게", + "그렇게해", + "그렇게해줘", "좋아", + "좋아요", + "좋습니다", + "좋지", + "괜찮아", + "괜찮아요", + "괜찮습니다", + "알겠어", + "알겠어요", + "알겠습니다", + "알았어", + "알았어요", + "알았습니다", + "알겠", + "알았", + "오케", + "콜", + "가자", "시작", + "시작해", + "시작해줘", "진행", "진행해", "진행해줘", + "계속해", + "계속해줘", "계속", ) CANCEL_WORDS = ("취소", "아니", "아니요", "멈춰", "중지", "그만", "정지") diff --git a/src/azas_voice/azas_voice/tts_node.py b/src/azas_voice/azas_voice/tts_node.py index d27b82f..64234d7 100644 --- a/src/azas_voice/azas_voice/tts_node.py +++ b/src/azas_voice/azas_voice/tts_node.py @@ -117,7 +117,7 @@ def __init__(self): self.declare_parameter("language", "ko") self.declare_parameter("enable_audio", True) self.declare_parameter("speech_rate", 1.25) - self.declare_parameter("startup_prompt", "주문하시겠어요?") + self.declare_parameter("startup_prompt", "원하는 맛을 말씀해주시면 추천해드릴게요. 주문하시겠어요?") confirmation_topic = str(self.get_parameter("confirmation_topic").value) ui_state_topic = str(self.get_parameter("ui_state_topic").value) diff --git a/src/azas_voice/config/recipes.yaml b/src/azas_voice/config/recipes.yaml index 73a920e..fb66d9c 100644 --- a/src/azas_voice/config/recipes.yaml +++ b/src/azas_voice/config/recipes.yaml @@ -4,13 +4,31 @@ # stored here and must come from calibration/runtime gates. colors: red: + role: juice + ingredient_role: 주스 aliases: [빨강, 빨간색, 레드, red] + traits: [fruitiness, freshness, light, sweetness] yellow: + role: syrup + ingredient_role: 시럽 aliases: [노랑, 노란색, 옐로우, yellow] + traits: [sweetness, softness] green: + role: liqueur + ingredient_role: 리큐르 aliases: [초록, 초록색, 그린, green] + traits: [aroma, herbal, bitterness] blue: + role: rum + ingredient_role: 럼 aliases: [파랑, 파란색, 블루, blue] + traits: [alcohol, depth, bitterness] + +preference_levels: + rum: [없음, 약하게, 보통, 강하게] + syrup: [적게, 보통, 많게] + liqueur: [적게, 보통, 많게] + juice: [적게, 보통, 많게] recipes: recipe_01: name: 레드 메뉴 diff --git a/src/azas_voice/launch/azas_voice.launch.py b/src/azas_voice/launch/azas_voice.launch.py index e9781a2..7f16e23 100644 --- a/src/azas_voice/launch/azas_voice.launch.py +++ b/src/azas_voice/launch/azas_voice.launch.py @@ -24,11 +24,15 @@ def generate_launch_description(): DeclareLaunchArgument("use_tts", default_value="true"), DeclareLaunchArgument("enable_tts_audio", default_value="true"), DeclareLaunchArgument("tts_speech_rate", default_value="1.25"), - DeclareLaunchArgument("tts_startup_prompt", default_value="주문하시겠어요?"), + DeclareLaunchArgument( + "tts_startup_prompt", + default_value="원하는 맛을 말씀해주시면 추천해드릴게요. 주문하시겠어요?", + ), DeclareLaunchArgument("enable_llm", default_value="false"), DeclareLaunchArgument("llm_model", default_value="gpt-4o-mini"), DeclareLaunchArgument("llm_base_url", default_value="https://api.openai.com/v1"), DeclareLaunchArgument("llm_api_key_env", default_value="OPENAI_API_KEY"), + DeclareLaunchArgument("llm_request_timeout_sec", default_value="20.0"), DeclareLaunchArgument("stt_topic", default_value="/stt_result"), Node( package="azas_voice", @@ -55,6 +59,9 @@ def generate_launch_description(): "model": LaunchConfiguration("llm_model"), "base_url": LaunchConfiguration("llm_base_url"), "api_key_env": LaunchConfiguration("llm_api_key_env"), + "request_timeout_sec": ParameterValue( + LaunchConfiguration("llm_request_timeout_sec"), value_type=float + ), "publish_confirmation": False, } ], diff --git a/src/azas_voice/test/test_command_parser.py b/src/azas_voice/test/test_command_parser.py index 6e538c4..56cc534 100644 --- a/src/azas_voice/test/test_command_parser.py +++ b/src/azas_voice/test/test_command_parser.py @@ -49,16 +49,98 @@ def test_four_menu_recipes_map_to_one_dispenser_each(): assert decision.dispenser_ids == dispenser_ids -def test_mood_request_randomly_recommends_executable_recipe(): +def test_mood_request_maps_to_custom_recommendation(): decision = parse_recipe_command("오늘 기분이 우울한데 칵테일 추천해줘") assert decision.valid assert decision.intent == "make_cocktail" - assert decision.recipe_id is not None - assert decision.recipe_id.startswith("recipe_") + assert decision.recipe_id == "custom_preference_mix" assert_dispenser_colors(decision.dispenser_ids) + assert decision.dispenser_amounts == { + "blue": 1, + "yellow": 3, + "green": 1, + "red": 3, + } + assert "추천" in decision.confirmation + assert "진행할까요" in decision.confirmation + + +def test_reroll_recommendation_words_take_priority_over_cancel(): + for utterance in ("아니 다른거", "다른거", "말고 다른 메뉴 추천해줘"): + decision = parse_recipe_command(utterance) + assert decision.valid + assert decision.intent == "make_cocktail" + assert decision.recipe_id is not None + assert decision.recipe_id.startswith("recipe_") + assert_dispenser_colors(decision.dispenser_ids) + assert "추천" in decision.confirmation + + +def test_preference_recommendation_maps_to_custom_mix(): + decision = parse_recipe_command("너무 세지 않고 향 좋은 걸로 추천해줘") + assert decision.valid + assert decision.intent == "make_cocktail" + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts + assert decision.dispenser_amounts["blue"] == 1 + assert decision.dispenser_amounts["green"] >= 2 assert "추천" in decision.confirmation +def test_mood_recommendations_map_to_custom_mix(): + sad_decision = parse_recipe_command("기분 안좋은데 메뉴 추천해줘") + assert sad_decision.valid + assert sad_decision.recipe_id == "custom_preference_mix" + assert sad_decision.dispenser_amounts + assert sad_decision.dispenser_amounts["blue"] == 1 + assert sad_decision.dispenser_amounts["yellow"] == 3 + assert sad_decision.dispenser_amounts["red"] == 3 + + happy_decision = parse_recipe_command("기분 좋은데 메뉴 추천해줘") + assert happy_decision.valid + assert happy_decision.recipe_id == "custom_preference_mix" + assert happy_decision.dispenser_amounts + assert happy_decision.dispenser_amounts["green"] == 3 + assert happy_decision.dispenser_amounts["red"] == 3 + + +def test_bitter_alcohol_dislike_maps_to_sweeter_custom_mix(): + decision = parse_recipe_command("쓴맛 나는 술은 싫은데 메뉴 추천해줘") + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts + assert decision.dispenser_amounts["blue"] == 1 + assert decision.dispenser_amounts["yellow"] == 3 + assert decision.dispenser_amounts["red"] == 3 + + +def test_preference_request_maps_to_ingredient_amounts(): + decision = parse_recipe_command("술 약하게 하고 덜 달고 상큼하게 과일맛 진하게 만들어줘") + assert decision.valid + assert decision.intent == "make_cocktail" + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts + assert decision.dispenser_amounts["red"] == 3 + assert decision.dispenser_amounts["yellow"] == 1 + assert decision.dispenser_amounts["blue"] == 1 + assert decision.profile == { + "rum": "약하게", + "syrup": "적게", + "liqueur": "적게", + "juice": "많게", + } + assert decision.dispenser_ids == ("red", "yellow", "green", "blue") + + +def test_non_alcohol_preference_omits_blue_dispenser(): + decision = parse_recipe_command("무알콜로 달달하고 과일맛 나게") + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts + assert decision.dispenser_amounts["blue"] == 0 + assert "blue" not in decision.dispenser_ids + + def test_unknown_text_is_invalid(): decision = parse_recipe_command("무슨 말인지 모르겠어") assert not decision.valid @@ -75,3 +157,27 @@ def test_proceed_phrase_maps_to_confirm_intent(): decision = parse_recipe_command("진행해줘") assert decision.valid assert decision.intent == "confirm" + + +def test_common_acknowledgements_map_to_confirm_intent(): + for utterance in ( + "알겠어", + "알겠습니다", + "오케이", + "그래 그렇게 해줘", + "좋아요", + "괜찮아", + "콜", + "가자", + "계속해줘", + ): + decision = parse_recipe_command(utterance) + assert decision.valid + assert decision.intent == "confirm" + + +def test_order_phrase_with_make_word_still_maps_to_preference_order(): + decision = parse_recipe_command("술 약하게 해서 만들어줘") + assert decision.valid + assert decision.intent == "make_cocktail" + assert decision.recipe_id == "custom_preference_mix" diff --git a/src/azas_voice/test/test_llm_recipe_mapper.py b/src/azas_voice/test/test_llm_recipe_mapper.py index 68dc3ac..3888169 100644 --- a/src/azas_voice/test/test_llm_recipe_mapper.py +++ b/src/azas_voice/test/test_llm_recipe_mapper.py @@ -1,4 +1,5 @@ from azas_voice.llm_recipe_mapper_node import _sanitize_llm_decision +from azas_voice.recipe_catalog import RECIPE_DISPENSERS def test_sanitize_llm_decision_converts_dispenser_numbers_to_colors(): @@ -50,7 +51,7 @@ def test_sanitize_llm_decision_rejects_coordinate_like_output(): def test_sanitize_llm_decision_fills_recipe_dispenser_ids(): decision = _sanitize_llm_decision( - "기분에 맞는 칵테일 추천해줘", + "3번 메뉴 만들어줘", { "intent": "make_cocktail", "recipe_id": "recipe_03", @@ -63,3 +64,205 @@ def test_sanitize_llm_decision_fills_recipe_dispenser_ids(): assert decision.recipe_id == "recipe_03" assert decision.dispenser_ids assert "진행할까요" in decision.confirmation + + +def test_sanitize_llm_decision_preserves_recommendation_wording(): + decision = _sanitize_llm_decision( + "추천해줘", + { + "intent": "make_cocktail", + "recipe_id": "recipe_01", + "dispenser_ids": ["red", "yellow"], + "profile": {"preference_order": "['not too strong', 'light']"}, + "confirmation": "", + }, + ) + + assert decision.valid + assert decision.recipe_id in RECIPE_DISPENSERS + assert decision.dispenser_ids == RECIPE_DISPENSERS[decision.recipe_id] + assert "추천" in decision.confirmation + assert "진행할까요" in decision.confirmation + assert decision.profile is None + assert decision.dispenser_amounts is None + + +def test_sanitize_llm_decision_prefers_local_preference_recommendation(): + decision = _sanitize_llm_decision( + "너무 세지 않고 향 좋은 걸로 추천해줘", + { + "intent": "make_cocktail", + "recipe_id": "recipe_01", + "dispenser_ids": ["red"], + "confirmation": "", + }, + ) + + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts + assert decision.dispenser_amounts["blue"] == 1 + assert decision.dispenser_amounts["green"] >= 2 + + +def test_sanitize_llm_decision_prefers_local_mood_recommendation(): + decision = _sanitize_llm_decision( + "기분 안좋은데 메뉴 추천해줘", + { + "intent": "make_cocktail", + "recipe_id": "recipe_02", + "dispenser_ids": ["yellow"], + "confirmation": "", + }, + ) + + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts == { + "blue": 1, + "yellow": 3, + "green": 1, + "red": 3, + } + + +def test_sanitize_llm_decision_maps_traits_to_amounts(): + decision = _sanitize_llm_decision( + "쓴맛 나는 술은 싫고 달달한 걸로 추천해줘", + { + "intent": "make_cocktail", + "wanted_traits": ["sweetness"], + "avoided_traits": ["bitterness", "alcohol"], + "confirmation": "", + }, + ) + + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts == { + "red": 3, + "yellow": 3, + "green": 1, + "blue": 1, + } + assert decision.profile + assert decision.profile["rum"] == "약하게" + assert decision.profile["syrup"] == "많게" + + +def test_sanitize_llm_decision_prefers_local_confirm_intent(): + decision = _sanitize_llm_decision( + "진행해줘", + { + "intent": "make_cocktail", + "recipe_id": "custom_color_selection", + "dispenser_ids": ["red", "yellow"], + "confirmation": "custom_color_selection 요청을 인식했습니다. 진행할까요?", + }, + ) + + assert decision.valid + assert decision.intent == "confirm" + assert decision.recipe_id is None + assert decision.dispenser_ids == () + assert decision.confirmation == "선택한 칵테일 제조를 확인했습니다." + + +def test_sanitize_llm_decision_prefers_local_reroll_recommendation(): + decision = _sanitize_llm_decision( + "아니 다른거", + { + "intent": "cancel", + "recipe_id": None, + "dispenser_ids": [], + "confirmation": "칵테일 제조 요청을 취소합니다.", + }, + ) + + assert decision.valid + assert decision.intent == "make_cocktail" + assert decision.recipe_id in RECIPE_DISPENSERS + assert decision.dispenser_ids == RECIPE_DISPENSERS[decision.recipe_id] + assert "추천" in decision.confirmation + + +def test_sanitize_llm_decision_accepts_preference_amounts(): + decision = _sanitize_llm_decision( + "술 약하게 덜 달고 상큼하게", + { + "intent": "make_cocktail", + "recipe_id": "custom_preference_mix", + "dispenser_amounts": { + "red": 1, + "yellow": 1, + "green": 3, + "blue": 1, + }, + "profile": { + "rum": "약하게", + "syrup": "적게", + "liqueur": "많게", + "juice": "적게", + }, + "confirmation": "취향에 맞춰 제조할까요?", + }, + ) + + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_ids == ("red", "yellow", "green", "blue") + assert decision.dispenser_amounts + assert decision.dispenser_amounts["green"] == 3 + assert decision.profile + assert decision.profile["syrup"] == "적게" + + +def test_sanitize_llm_decision_repairs_incomplete_preference_mix(): + decision = _sanitize_llm_decision( + "시럽 적게 리큐르 많이 럼 약하게 주스 많이 넣어줘", + { + "intent": "make_cocktail", + "recipe_id": "custom_preference_mix", + "dispenser_ids": ["yellow", "green", "blue"], + "confirmation": "False", + }, + ) + + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_ids == ("red", "yellow", "green", "blue") + assert decision.dispenser_amounts + assert decision.dispenser_amounts["blue"] == 1 + assert decision.dispenser_amounts["yellow"] == 1 + assert decision.dispenser_amounts["green"] == 3 + assert decision.dispenser_amounts["red"] >= 2 + assert decision.profile + assert decision.profile["rum"] == "약하게" + assert "진행할까요" in decision.confirmation + + +def test_sanitize_llm_decision_repairs_nonstandard_preference_profile(): + decision = _sanitize_llm_decision( + "오늘은 너무 세지 않고 향은 좀 풍부한 느낌으로 만들어줘", + { + "intent": "make_cocktail", + "recipe_id": "custom_preference_mix", + "dispenser_amounts": { + "blue": 1, + "yellow": 2, + "green": 3, + "red": 1, + }, + "profile": {"preference_order": "['not too strong', 'rich aroma']"}, + "confirmation": "취향대로 맞출게요. 진행할까요?", + }, + ) + + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.profile == { + "rum": "약하게", + "syrup": "보통", + "liqueur": "많게", + "juice": "적게", + } From 17b9c527fe56a74ccb1b3b52edd5da462f3cfc31 Mon Sep 17 00:00:00 2001 From: suuu0719 Date: Fri, 5 Jun 2026 19:05:38 +0900 Subject: [PATCH 24/88] fix: handle stronger voice follow-up requests --- .../azas_voice/llm_recipe_mapper_node.py | 22 ++++++++++++- src/azas_voice/azas_voice/recipe_catalog.py | 31 ++++++++++++++++++- src/azas_voice/test/test_command_parser.py | 10 ++++++ src/azas_voice/test/test_llm_recipe_mapper.py | 19 ++++++++++++ 4 files changed, 80 insertions(+), 2 deletions(-) diff --git a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py index 00516f9..b4905ca 100644 --- a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py +++ b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py @@ -72,6 +72,23 @@ def _fallback_decision(text: str, reason: str = "") -> RecipeDecision: ) +def _has_explicit_local_preference(normalized: str) -> bool: + explicit_markers = ( + "더쎈", + "더센", + "더쌘", + "쎈거", + "센거", + "쌘거", + "도수쎈", + "도수센", + "도수쌘", + "강한거", + "더강한", + ) + return any(marker in normalized for marker in explicit_markers) + + def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: intent = str(payload.get("intent", "unknown")).strip() if intent not in ALLOWED_INTENTS: @@ -125,7 +142,10 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: if intent == "make_cocktail" and fallback.recipe_id == "custom_preference_mix": recipe_id = "custom_preference_mix" - if not any(dispenser_amounts.values()) and fallback.dispenser_amounts: + if ( + fallback.dispenser_amounts + and (not any(dispenser_amounts.values()) or _has_explicit_local_preference(fallback.normalized)) + ): dispenser_amounts = dict(fallback.dispenser_amounts) if any(dispenser_amounts.values()): dispenser_ids = tuple( diff --git a/src/azas_voice/azas_voice/recipe_catalog.py b/src/azas_voice/azas_voice/recipe_catalog.py index 688ae8b..d5ff31f 100644 --- a/src/azas_voice/azas_voice/recipe_catalog.py +++ b/src/azas_voice/azas_voice/recipe_catalog.py @@ -87,7 +87,27 @@ ), "freshness": ("상큼", "새콤", "산미", "산뜻", "상쾌"), "aroma": ("향", "향좋", "향진", "향강", "풍부", "리큐르", "기분좋", "행복", "신나"), - "alcohol": ("도수쎈", "도수센", "도수쌘", "도수높", "쎈술", "센술", "쌘술", "강한술", "술강", "럼강", "강하게"), + "alcohol": ( + "도수쎈", + "도수센", + "도수쌘", + "도수높", + "쎈술", + "센술", + "쌘술", + "쎈거", + "센거", + "쌘거", + "더쎈", + "더센", + "더쌘", + "강한술", + "강한거", + "더강한", + "술강", + "럼강", + "강하게", + ), "bitterness": ("쓴맛", "쓴술", "쌉싸름", "허브", "드라이"), "softness": ("부드럽", "순하게", "편한"), "light": ("가볍", "부담", "편한", "세지않", "안세"), @@ -218,6 +238,15 @@ "술강", "강하게", "도수", + "도수쎈", + "도수센", + "쎈거", + "센거", + "쌘거", + "더쎈", + "더센", + "더쌘", + "강한거", "럼", "무알콜", "논알콜", diff --git a/src/azas_voice/test/test_command_parser.py b/src/azas_voice/test/test_command_parser.py index 56cc534..c734d0e 100644 --- a/src/azas_voice/test/test_command_parser.py +++ b/src/azas_voice/test/test_command_parser.py @@ -114,6 +114,16 @@ def test_bitter_alcohol_dislike_maps_to_sweeter_custom_mix(): assert decision.dispenser_amounts["red"] == 3 +def test_stronger_followup_maps_to_high_alcohol_custom_mix(): + decision = parse_recipe_command("더 쎈거는 없어?") + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts + assert decision.dispenser_amounts["blue"] == 3 + assert decision.profile + assert decision.profile["rum"] == "강하게" + + def test_preference_request_maps_to_ingredient_amounts(): decision = parse_recipe_command("술 약하게 하고 덜 달고 상큼하게 과일맛 진하게 만들어줘") assert decision.valid diff --git a/src/azas_voice/test/test_llm_recipe_mapper.py b/src/azas_voice/test/test_llm_recipe_mapper.py index 3888169..9063b7a 100644 --- a/src/azas_voice/test/test_llm_recipe_mapper.py +++ b/src/azas_voice/test/test_llm_recipe_mapper.py @@ -150,6 +150,25 @@ def test_sanitize_llm_decision_maps_traits_to_amounts(): assert decision.profile["syrup"] == "많게" +def test_sanitize_llm_decision_prefers_explicit_stronger_followup(): + decision = _sanitize_llm_decision( + "더 쎈거는 없어?", + { + "intent": "make_cocktail", + "wanted_traits": ["sweetness", "fruitiness"], + "avoided_traits": ["bitterness"], + "confirmation": "", + }, + ) + + assert decision.valid + assert decision.recipe_id == "custom_preference_mix" + assert decision.dispenser_amounts + assert decision.dispenser_amounts["blue"] == 3 + assert decision.profile + assert decision.profile["rum"] == "강하게" + + def test_sanitize_llm_decision_prefers_local_confirm_intent(): decision = _sanitize_llm_decision( "진행해줘", From 6689dfeb6ff36c3ee73b64468b58ce352e116515 Mon Sep 17 00:00:00 2001 From: suuu0719 Date: Fri, 5 Jun 2026 19:39:23 +0900 Subject: [PATCH 25/88] =?UTF-8?q?feat:=201=EC=B0=A8=20=ED=82=A4=EC=98=A4?= =?UTF-8?q?=EC=8A=A4=ED=81=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/cocktail_dryrun.launch.py | 17 ++ src/azas_bringup/package.xml | 1 + src/azas_kiosk/azas_kiosk/__init__.py | 1 + src/azas_kiosk/azas_kiosk/kiosk_node.py | 221 ++++++++++++++++ src/azas_kiosk/azas_kiosk/menu_catalog.py | 53 ++++ src/azas_kiosk/launch/azas_kiosk.launch.py | 28 ++ src/azas_kiosk/package.xml | 17 ++ src/azas_kiosk/resource/azas_kiosk | 1 + src/azas_kiosk/setup.cfg | 4 + src/azas_kiosk/setup.py | 30 +++ src/azas_kiosk/test/test_menu_catalog.py | 13 + src/azas_kiosk/web/app.js | 122 +++++++++ src/azas_kiosk/web/index.html | 49 ++++ src/azas_kiosk/web/styles.css | 243 ++++++++++++++++++ 14 files changed, 800 insertions(+) create mode 100644 src/azas_kiosk/azas_kiosk/__init__.py create mode 100644 src/azas_kiosk/azas_kiosk/kiosk_node.py create mode 100644 src/azas_kiosk/azas_kiosk/menu_catalog.py create mode 100644 src/azas_kiosk/launch/azas_kiosk.launch.py create mode 100644 src/azas_kiosk/package.xml create mode 100644 src/azas_kiosk/resource/azas_kiosk create mode 100644 src/azas_kiosk/setup.cfg create mode 100644 src/azas_kiosk/setup.py create mode 100644 src/azas_kiosk/test/test_menu_catalog.py create mode 100644 src/azas_kiosk/web/app.js create mode 100644 src/azas_kiosk/web/index.html create mode 100644 src/azas_kiosk/web/styles.css diff --git a/src/azas_bringup/launch/cocktail_dryrun.launch.py b/src/azas_bringup/launch/cocktail_dryrun.launch.py index 0fc4a81..db60f76 100644 --- a/src/azas_bringup/launch/cocktail_dryrun.launch.py +++ b/src/azas_bringup/launch/cocktail_dryrun.launch.py @@ -11,6 +11,7 @@ def generate_launch_description(): run_voice = LaunchConfiguration("run_voice") run_yolo = LaunchConfiguration("run_yolo") + run_kiosk = LaunchConfiguration("run_kiosk") voice_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -43,9 +44,24 @@ def generate_launch_description(): condition=IfCondition(run_yolo), ) + kiosk_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare("azas_kiosk"), "launch", "azas_kiosk.launch.py"]) + ), + launch_arguments={ + "host": LaunchConfiguration("kiosk_host"), + "port": LaunchConfiguration("kiosk_port"), + "stt_topic": LaunchConfiguration("stt_topic"), + }.items(), + condition=IfCondition(run_kiosk), + ) + return LaunchDescription( [ DeclareLaunchArgument("run_voice", default_value="true"), + DeclareLaunchArgument("run_kiosk", default_value="true"), + DeclareLaunchArgument("kiosk_host", default_value="0.0.0.0"), + DeclareLaunchArgument("kiosk_port", default_value="8080"), DeclareLaunchArgument("use_live_stt", default_value="false"), DeclareLaunchArgument("use_llm", default_value="false"), DeclareLaunchArgument("enable_llm", default_value="false"), @@ -65,6 +81,7 @@ def generate_launch_description(): DeclareLaunchArgument("require_lid", default_value="true"), voice_launch, yolo_launch, + kiosk_launch, Node( package="azas_task_manager", executable="cocktail_dryrun_sequence_node", diff --git a/src/azas_bringup/package.xml b/src/azas_bringup/package.xml index a2a65f7..a22910d 100644 --- a/src/azas_bringup/package.xml +++ b/src/azas_bringup/package.xml @@ -10,6 +10,7 @@ azas_calibration azas_task_manager azas_gripper + azas_kiosk azas_motion azas_perception sensor_msgs diff --git a/src/azas_kiosk/azas_kiosk/__init__.py b/src/azas_kiosk/azas_kiosk/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/azas_kiosk/azas_kiosk/__init__.py @@ -0,0 +1 @@ + diff --git a/src/azas_kiosk/azas_kiosk/kiosk_node.py b/src/azas_kiosk/azas_kiosk/kiosk_node.py new file mode 100644 index 0000000..d1ce187 --- /dev/null +++ b/src/azas_kiosk/azas_kiosk/kiosk_node.py @@ -0,0 +1,221 @@ +from __future__ import annotations + +import json +import mimetypes +from http import HTTPStatus +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +import threading +import time +from typing import Any + +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.node import Node +from std_msgs.msg import String + +from azas_kiosk.menu_catalog import MENU_ITEMS, build_menu_payload + + +class KioskBridgeNode(Node): + """Expose a local kiosk UI without creating robot coordinates or motion plans.""" + + def __init__(self): + super().__init__("azas_kiosk_node") + self.declare_parameter("host", "0.0.0.0") + self.declare_parameter("port", 8080) + self.declare_parameter("stt_topic", "/stt_result") + self.declare_parameter("confirmation_topic", "/azas/voice/confirmation") + self.declare_parameter("ui_state_topic", "/azas/voice/ui_state") + self.declare_parameter("cocktail_status_topic", "/azas/cocktail/status") + + self._lock = threading.Lock() + self._state: dict[str, Any] = { + "started_at": time.time(), + "last_command": "", + "last_confirmation": "", + "ui_state": {"state": "unknown", "emotion": "neutral", "text": ""}, + "cocktail_status": {}, + } + + self._stt_pub = self.create_publisher( + String, + str(self.get_parameter("stt_topic").value), + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("confirmation_topic").value), + self._on_confirmation, + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("ui_state_topic").value), + self._on_ui_state, + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("cocktail_status_topic").value), + self._on_cocktail_status, + 10, + ) + + self._web_root = Path(get_package_share_directory("azas_kiosk")) / "web" + host = str(self.get_parameter("host").value) + port = int(self.get_parameter("port").value) + handler = self._build_handler() + self._server = ThreadingHTTPServer((host, port), handler) + self._thread = threading.Thread(target=self._server.serve_forever, daemon=True) + self._thread.start() + + self.get_logger().info( + f"Azas kiosk ready at http://{host}:{port} publishing to " + f"{self.get_parameter('stt_topic').value}" + ) + + def publish_command(self, text: str) -> None: + command = text.strip() + if not command: + raise ValueError("empty command") + msg = String() + msg.data = command + self._stt_pub.publish(msg) + with self._lock: + self._state["last_command"] = command + self._state["last_command_at"] = time.time() + self.get_logger().info(f"kiosk command -> {command}") + + def snapshot(self) -> dict[str, Any]: + with self._lock: + payload = dict(self._state) + payload["menus"] = build_menu_payload() + return payload + + def _on_confirmation(self, msg: String) -> None: + with self._lock: + self._state["last_confirmation"] = msg.data + self._state["last_confirmation_at"] = time.time() + + def _on_ui_state(self, msg: String) -> None: + with self._lock: + self._state["ui_state"] = _json_or_text(msg.data) + self._state["ui_state_at"] = time.time() + + def _on_cocktail_status(self, msg: String) -> None: + with self._lock: + self._state["cocktail_status"] = _json_or_text(msg.data) + self._state["cocktail_status_at"] = time.time() + + def _build_handler(self): + node = self + + class KioskRequestHandler(BaseHTTPRequestHandler): + def do_GET(self) -> None: + if self.path in {"/", "/index.html"}: + self._send_file(node._web_root / "index.html") + return + if self.path == "/styles.css": + self._send_file(node._web_root / "styles.css") + return + if self.path == "/app.js": + self._send_file(node._web_root / "app.js") + return + if self.path == "/api/state": + self._send_json(node.snapshot()) + return + self.send_error(HTTPStatus.NOT_FOUND) + + def do_POST(self) -> None: + try: + payload = self._read_json() + command = _command_from_request(self.path, payload) + node.publish_command(command) + except ValueError as exc: + self._send_json({"ok": False, "error": str(exc)}, HTTPStatus.BAD_REQUEST) + return + self._send_json({"ok": True, "command": command}) + + def log_message(self, format: str, *args: object) -> None: + node.get_logger().debug(format % args) + + def _read_json(self) -> dict[str, Any]: + length = int(self.headers.get("Content-Length", "0")) + if length <= 0: + return {} + body = self.rfile.read(length).decode("utf-8") + try: + payload = json.loads(body) + except json.JSONDecodeError as exc: + raise ValueError(f"invalid json: {exc}") from exc + if not isinstance(payload, dict): + raise ValueError("json body must be an object") + return payload + + def _send_json( + self, + payload: dict[str, Any], + status: HTTPStatus = HTTPStatus.OK, + ) -> None: + data = json.dumps(payload, ensure_ascii=False).encode("utf-8") + self.send_response(status) + self.send_header("Content-Type", "application/json; charset=utf-8") + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + + def _send_file(self, path: Path) -> None: + if not path.is_file(): + self.send_error(HTTPStatus.NOT_FOUND) + return + data = path.read_bytes() + content_type = mimetypes.guess_type(path.name)[0] or "application/octet-stream" + self.send_response(HTTPStatus.OK) + self.send_header("Content-Type", f"{content_type}; charset=utf-8") + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + + return KioskRequestHandler + + def destroy_node(self): + self._server.shutdown() + self._server.server_close() + self._thread.join(timeout=1.0) + super().destroy_node() + + +def _json_or_text(text: str) -> Any: + try: + return json.loads(text) + except json.JSONDecodeError: + return {"text": text} + + +def _command_from_request(path: str, payload: dict[str, Any]) -> str: + if path == "/api/order": + recipe_id = str(payload.get("recipe_id", "")) + for item in MENU_ITEMS: + if item.recipe_id == recipe_id: + return item.order_text + raise ValueError(f"unknown recipe_id: {recipe_id}") + if path == "/api/recommend": + return "메뉴 추천해줘" + if path == "/api/confirm": + return "응" + if path == "/api/cancel": + return "취소" + if path == "/api/command": + return str(payload.get("text", "")).strip() + raise ValueError(f"unknown endpoint: {path}") + + +def main(args=None): + rclpy.init(args=args) + node = KioskBridgeNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/src/azas_kiosk/azas_kiosk/menu_catalog.py b/src/azas_kiosk/azas_kiosk/menu_catalog.py new file mode 100644 index 0000000..b7fea4a --- /dev/null +++ b/src/azas_kiosk/azas_kiosk/menu_catalog.py @@ -0,0 +1,53 @@ +from __future__ import annotations + +from dataclasses import asdict, dataclass + + +@dataclass(frozen=True) +class KioskMenuItem: + recipe_id: str + name: str + color: str + role: str + description: str + order_text: str + + +MENU_ITEMS: tuple[KioskMenuItem, ...] = ( + KioskMenuItem( + recipe_id="recipe_01", + name="레드 메뉴", + color="red", + role="주스", + description="과일감이 선명하고 가볍게 마시기 좋은 메뉴", + order_text="레드 메뉴 만들어줘", + ), + KioskMenuItem( + recipe_id="recipe_02", + name="옐로우 메뉴", + color="yellow", + role="시럽", + description="달콤하고 부드러운 느낌이 강한 메뉴", + order_text="옐로우 메뉴 만들어줘", + ), + KioskMenuItem( + recipe_id="recipe_03", + name="그린 메뉴", + color="green", + role="리큐르", + description="향이 선명하고 깔끔한 여운이 있는 메뉴", + order_text="그린 메뉴 만들어줘", + ), + KioskMenuItem( + recipe_id="recipe_04", + name="블루 메뉴", + color="blue", + role="럼", + description="칵테일다운 존재감과 깊이가 있는 메뉴", + order_text="블루 메뉴 만들어줘", + ), +) + + +def build_menu_payload() -> list[dict[str, str]]: + return [asdict(item) for item in MENU_ITEMS] diff --git a/src/azas_kiosk/launch/azas_kiosk.launch.py b/src/azas_kiosk/launch/azas_kiosk.launch.py new file mode 100644 index 0000000..2f7915d --- /dev/null +++ b/src/azas_kiosk/launch/azas_kiosk.launch.py @@ -0,0 +1,28 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument("host", default_value="0.0.0.0"), + DeclareLaunchArgument("port", default_value="8080"), + DeclareLaunchArgument("stt_topic", default_value="/stt_result"), + Node( + package="azas_kiosk", + executable="kiosk_node", + name="azas_kiosk_node", + output="screen", + parameters=[ + { + "host": LaunchConfiguration("host"), + "port": ParameterValue(LaunchConfiguration("port"), value_type=int), + "stt_topic": LaunchConfiguration("stt_topic"), + } + ], + ), + ] + ) diff --git a/src/azas_kiosk/package.xml b/src/azas_kiosk/package.xml new file mode 100644 index 0000000..7d057ee --- /dev/null +++ b/src/azas_kiosk/package.xml @@ -0,0 +1,17 @@ + + + azas_kiosk + 0.0.1 + Local kiosk UI bridge for symbolic Azas cocktail ordering. + Azas Team + MIT + + ament_index_python + rclpy + std_msgs + python3-pytest + + + ament_python + + diff --git a/src/azas_kiosk/resource/azas_kiosk b/src/azas_kiosk/resource/azas_kiosk new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/azas_kiosk/resource/azas_kiosk @@ -0,0 +1 @@ + diff --git a/src/azas_kiosk/setup.cfg b/src/azas_kiosk/setup.cfg new file mode 100644 index 0000000..0d5ee15 --- /dev/null +++ b/src/azas_kiosk/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/azas_kiosk +[install] +install_scripts=$base/lib/azas_kiosk diff --git a/src/azas_kiosk/setup.py b/src/azas_kiosk/setup.py new file mode 100644 index 0000000..aca176b --- /dev/null +++ b/src/azas_kiosk/setup.py @@ -0,0 +1,30 @@ +from glob import glob + +from setuptools import find_packages, setup + + +package_name = "azas_kiosk" + +setup( + name=package_name, + version="0.0.1", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (f"share/{package_name}/launch", glob("launch/*.launch.py")), + (f"share/{package_name}/web", glob("web/*")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Azas Team", + maintainer_email="team@example.com", + description="Local kiosk UI bridge for symbolic Azas cocktail ordering.", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "kiosk_node = azas_kiosk.kiosk_node:main", + ], + }, +) diff --git a/src/azas_kiosk/test/test_menu_catalog.py b/src/azas_kiosk/test/test_menu_catalog.py new file mode 100644 index 0000000..ea11312 --- /dev/null +++ b/src/azas_kiosk/test/test_menu_catalog.py @@ -0,0 +1,13 @@ +from azas_kiosk.menu_catalog import build_menu_payload + + +def test_menu_payload_has_four_symbolic_recipes(): + menus = build_menu_payload() + + assert [menu["recipe_id"] for menu in menus] == [ + "recipe_01", + "recipe_02", + "recipe_03", + "recipe_04", + ] + assert all("order_text" in menu for menu in menus) diff --git a/src/azas_kiosk/web/app.js b/src/azas_kiosk/web/app.js new file mode 100644 index 0000000..c4e4455 --- /dev/null +++ b/src/azas_kiosk/web/app.js @@ -0,0 +1,122 @@ +const state = { + menus: [], + selectedRecipeId: null, +}; + +const menuGrid = document.querySelector("#menu-grid"); +const confirmationText = document.querySelector("#confirmation-text"); +const lastCommand = document.querySelector("#last-command"); +const cocktailStatus = document.querySelector("#cocktail-status"); +const voiceState = document.querySelector("#voice-state"); + +const colorLabels = { + red: "Red", + yellow: "Yellow", + green: "Green", + blue: "Blue", +}; + +async function postJson(path, payload = {}) { + const response = await fetch(path, { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify(payload), + }); + const data = await response.json(); + if (!response.ok || !data.ok) { + throw new Error(data.error || "요청을 처리하지 못했습니다."); + } + return data; +} + +function renderMenus(menus) { + if (!menus.length) return; + menuGrid.innerHTML = ""; + for (const menu of menus) { + const button = document.createElement("button"); + button.type = "button"; + button.className = `menu-card tone-${menu.color}`; + button.dataset.recipeId = menu.recipe_id; + button.innerHTML = ` + ${colorLabels[menu.color] || menu.color} + ${menu.name} + ${menu.role} + ${menu.description} + `; + button.addEventListener("click", async () => { + await selectMenu(menu.recipe_id); + }); + menuGrid.appendChild(button); + } +} + +async function selectMenu(recipeId) { + state.selectedRecipeId = recipeId; + setSelectedCard(recipeId); + await postJson("/api/order", { recipe_id: recipeId }); + await refreshState(); +} + +function setSelectedCard(recipeId) { + for (const card of menuGrid.querySelectorAll(".menu-card")) { + card.classList.toggle("selected", card.dataset.recipeId === recipeId); + } +} + +async function refreshState() { + const response = await fetch("/api/state"); + const payload = await response.json(); + if (Array.isArray(payload.menus) && payload.menus.length && !state.menus.length) { + state.menus = payload.menus; + renderMenus(state.menus); + } + + const ui = payload.ui_state || {}; + const status = payload.cocktail_status || {}; + const prompt = payload.last_confirmation || ui.text || "주문을 기다리고 있습니다."; + + confirmationText.textContent = prompt; + lastCommand.textContent = payload.last_command || "-"; + voiceState.textContent = ui.state === "speaking" ? "안내 중" : "대기 중"; + cocktailStatus.textContent = status.status || "대기"; +} + +function showError(error) { + confirmationText.textContent = error.message || String(error); +} + +document.querySelector("#recommend-button").addEventListener("click", async () => { + try { + state.selectedRecipeId = null; + setSelectedCard(null); + await postJson("/api/recommend"); + await refreshState(); + } catch (error) { + showError(error); + } +}); + +document.querySelector("#cancel-button").addEventListener("click", async () => { + try { + state.selectedRecipeId = null; + setSelectedCard(null); + await postJson("/api/cancel"); + await refreshState(); + } catch (error) { + showError(error); + } +}); + +document.querySelector("#confirm-button").addEventListener("click", async () => { + try { + await postJson("/api/confirm"); + await refreshState(); + } catch (error) { + showError(error); + } +}); + +refreshState().catch(showError); +setInterval(() => { + refreshState().catch(showError); +}, 1000); diff --git a/src/azas_kiosk/web/index.html b/src/azas_kiosk/web/index.html new file mode 100644 index 0000000..d1b0934 --- /dev/null +++ b/src/azas_kiosk/web/index.html @@ -0,0 +1,49 @@ + + + + + + Azas Kiosk + + + +
+
+
+
+

Azas Cocktail Robot

+

원하는 메뉴를 선택하세요

+
+
대기 중
+
+ + + +
+ + + +
+
+ + +
+ + + + diff --git a/src/azas_kiosk/web/styles.css b/src/azas_kiosk/web/styles.css new file mode 100644 index 0000000..08ada33 --- /dev/null +++ b/src/azas_kiosk/web/styles.css @@ -0,0 +1,243 @@ +:root { + color-scheme: light; + font-family: + Inter, Pretendard, "Noto Sans KR", system-ui, -apple-system, BlinkMacSystemFont, + "Segoe UI", sans-serif; + background: #f3f5f1; + color: #17201d; +} + +* { + box-sizing: border-box; +} + +body { + margin: 0; + min-height: 100vh; + background: + linear-gradient(135deg, rgba(255, 255, 255, 0.88), rgba(238, 243, 234, 0.94)), + url("data:image/svg+xml,%3Csvg width='160' height='160' viewBox='0 0 160 160' xmlns='http://www.w3.org/2000/svg'%3E%3Cg fill='none' stroke='%23d7ded2' stroke-width='1'%3E%3Cpath d='M0 80h160M80 0v160'/%3E%3Ccircle cx='80' cy='80' r='52'/%3E%3C/g%3E%3C/svg%3E"); +} + +button { + border: 0; + font: inherit; +} + +.shell { + display: grid; + grid-template-columns: minmax(0, 1fr) 360px; + gap: 24px; + min-height: 100vh; + padding: 28px; +} + +.order-surface, +.status-panel { + background: rgba(255, 255, 255, 0.84); + border: 1px solid rgba(23, 32, 29, 0.12); + border-radius: 8px; + box-shadow: 0 18px 44px rgba(31, 42, 36, 0.11); +} + +.order-surface { + display: flex; + flex-direction: column; + gap: 24px; + padding: 28px; +} + +.topbar { + display: flex; + align-items: flex-start; + justify-content: space-between; + gap: 18px; +} + +.eyebrow, +.panel-label { + margin: 0 0 8px; + color: #617067; + font-size: 13px; + font-weight: 700; + letter-spacing: 0; + text-transform: uppercase; +} + +h1 { + margin: 0; + font-size: clamp(32px, 5vw, 58px); + line-height: 1.04; + letter-spacing: 0; +} + +.status-pill { + flex: 0 0 auto; + min-width: 92px; + padding: 10px 14px; + border-radius: 999px; + background: #17201d; + color: white; + font-weight: 800; + text-align: center; +} + +.menu-grid { + display: grid; + grid-template-columns: repeat(2, minmax(0, 1fr)); + gap: 18px; + flex: 1; +} + +.menu-card { + display: grid; + grid-template-rows: auto auto auto 1fr; + gap: 12px; + min-height: 230px; + padding: 24px; + border: 2px solid transparent; + border-radius: 8px; + color: #15201c; + text-align: left; + cursor: pointer; + transition: + border-color 160ms ease, + transform 160ms ease, + box-shadow 160ms ease; +} + +.menu-card:hover, +.menu-card:focus-visible, +.menu-card.selected { + border-color: #17201d; + box-shadow: 0 14px 28px rgba(23, 32, 29, 0.16); + transform: translateY(-2px); + outline: none; +} + +.menu-card strong { + font-size: clamp(26px, 3vw, 38px); + line-height: 1.08; + letter-spacing: 0; +} + +.menu-tone, +.menu-role { + width: fit-content; + padding: 6px 10px; + border-radius: 999px; + background: rgba(255, 255, 255, 0.66); + color: rgba(23, 32, 29, 0.74); + font-size: 14px; + font-weight: 800; +} + +.menu-description { + align-self: end; + color: rgba(23, 32, 29, 0.72); + font-size: 17px; + line-height: 1.45; +} + +.tone-red { + background: linear-gradient(135deg, #ffebe6, #ffd1c9); +} + +.tone-yellow { + background: linear-gradient(135deg, #fff5d8, #f8dc82); +} + +.tone-green { + background: linear-gradient(135deg, #def6e7, #9ed8b6); +} + +.tone-blue { + background: linear-gradient(135deg, #dcecff, #9fc7ef); +} + +.actions { + display: grid; + grid-template-columns: 150px 150px minmax(180px, 1fr); + gap: 14px; +} + +.primary-button, +.secondary-button { + min-height: 72px; + border-radius: 8px; + font-size: 22px; + font-weight: 900; + cursor: pointer; +} + +.primary-button { + background: #17201d; + color: white; +} + +.secondary-button { + background: #e6ece3; + color: #17201d; +} + +.secondary-button.danger { + background: #ffe1dc; +} + +.status-panel { + display: flex; + flex-direction: column; + justify-content: space-between; + gap: 28px; + padding: 24px; +} + +.large-status { + margin: 0; + font-size: 30px; + font-weight: 900; + line-height: 1.24; + letter-spacing: 0; +} + +.status-list { + display: grid; + gap: 14px; +} + +.status-list div { + display: grid; + gap: 6px; + padding: 16px; + border-radius: 8px; + background: #f1f4ef; +} + +.status-list span { + color: #617067; + font-size: 14px; + font-weight: 800; +} + +.status-list strong { + min-width: 0; + overflow-wrap: anywhere; + font-size: 18px; + letter-spacing: 0; +} + +@media (max-width: 980px) { + .shell { + grid-template-columns: 1fr; + padding: 18px; + } + + .menu-grid, + .actions { + grid-template-columns: 1fr; + } + + .topbar { + flex-direction: column; + } +} From fae41d3ce2a7acb5041fc82340d31862cfbd5f90 Mon Sep 17 00:00:00 2001 From: suuu0719 Date: Mon, 8 Jun 2026 15:22:42 +0900 Subject: [PATCH 26/88] Add kiosk and voice UI integration --- src/azas_kiosk/azas_kiosk/kiosk_node.py | 100 +++++- src/azas_kiosk/azas_kiosk/menu_catalog.py | 24 +- src/azas_kiosk/test/test_menu_catalog.py | 22 ++ src/azas_kiosk/web/app.js | 35 +- src/azas_kiosk/web/index.html | 2 +- src/azas_kiosk/web/styles.css | 269 +++++++++++++--- .../azas_voice/voice_screen_node.py | 262 +++++++++++++++ src/azas_voice/launch/azas_voice.launch.py | 20 ++ src/azas_voice/setup.py | 2 + src/azas_voice/test/test_voice_screen_node.py | 22 ++ src/azas_voice/web/voice.css | 300 ++++++++++++++++++ src/azas_voice/web/voice.html | 62 ++++ src/azas_voice/web/voice.js | 223 +++++++++++++ tools/checks/check_kiosk_voice_flow.sh | 234 ++++++++++++++ tools/run/run_kiosk_voice_demo.sh | 101 ++++++ 15 files changed, 1604 insertions(+), 74 deletions(-) create mode 100644 src/azas_voice/azas_voice/voice_screen_node.py create mode 100644 src/azas_voice/test/test_voice_screen_node.py create mode 100644 src/azas_voice/web/voice.css create mode 100644 src/azas_voice/web/voice.html create mode 100644 src/azas_voice/web/voice.js create mode 100755 tools/checks/check_kiosk_voice_flow.sh create mode 100755 tools/run/run_kiosk_voice_demo.sh diff --git a/src/azas_kiosk/azas_kiosk/kiosk_node.py b/src/azas_kiosk/azas_kiosk/kiosk_node.py index d1ce187..15405d8 100644 --- a/src/azas_kiosk/azas_kiosk/kiosk_node.py +++ b/src/azas_kiosk/azas_kiosk/kiosk_node.py @@ -2,6 +2,7 @@ import json import mimetypes +import random from http import HTTPStatus from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer from pathlib import Path @@ -9,10 +10,19 @@ import time from typing import Any -import rclpy -from ament_index_python.packages import get_package_share_directory -from rclpy.node import Node -from std_msgs.msg import String +try: + from ament_index_python.packages import get_package_share_directory +except ImportError: # pragma: no cover - allows helper tests without sourced ROS + get_package_share_directory = None + +try: + import rclpy + from rclpy.node import Node + from std_msgs.msg import String +except ImportError: # pragma: no cover - allows helper tests without sourced ROS + rclpy = None + Node = object + String = None from azas_kiosk.menu_catalog import MENU_ITEMS, build_menu_payload @@ -21,10 +31,13 @@ class KioskBridgeNode(Node): """Expose a local kiosk UI without creating robot coordinates or motion plans.""" def __init__(self): + if rclpy is None or String is None or get_package_share_directory is None: + raise RuntimeError("ROS 2 Python packages are not available. Source the ROS environment first.") super().__init__("azas_kiosk_node") self.declare_parameter("host", "0.0.0.0") self.declare_parameter("port", 8080) self.declare_parameter("stt_topic", "/stt_result") + self.declare_parameter("decision_topic", "/azas/voice/recipe_decision") self.declare_parameter("confirmation_topic", "/azas/voice/confirmation") self.declare_parameter("ui_state_topic", "/azas/voice/ui_state") self.declare_parameter("cocktail_status_topic", "/azas/cocktail/status") @@ -34,6 +47,7 @@ def __init__(self): "started_at": time.time(), "last_command": "", "last_confirmation": "", + "selected_recipe_id": "", "ui_state": {"state": "unknown", "emotion": "neutral", "text": ""}, "cocktail_status": {}, } @@ -43,6 +57,12 @@ def __init__(self): str(self.get_parameter("stt_topic").value), 10, ) + self.create_subscription( + String, + str(self.get_parameter("decision_topic").value), + self._on_decision, + 10, + ) self.create_subscription( String, str(self.get_parameter("confirmation_topic").value), @@ -75,7 +95,14 @@ def __init__(self): f"{self.get_parameter('stt_topic').value}" ) - def publish_command(self, text: str) -> None: + def publish_command( + self, + text: str, + *, + selected_recipe_id: str = "", + local_confirmation: str = "", + clear_selection: bool = False, + ) -> None: command = text.strip() if not command: raise ValueError("empty command") @@ -85,8 +112,24 @@ def publish_command(self, text: str) -> None: with self._lock: self._state["last_command"] = command self._state["last_command_at"] = time.time() + if clear_selection: + self._state["selected_recipe_id"] = "" + if selected_recipe_id: + self._state["selected_recipe_id"] = selected_recipe_id + if local_confirmation: + self._state["last_confirmation"] = local_confirmation + self._state["last_confirmation_at"] = time.time() self.get_logger().info(f"kiosk command -> {command}") + def recommend_menu(self) -> dict[str, str]: + item = random.choice(MENU_ITEMS) + self.publish_command( + item.order_text, + selected_recipe_id=item.recipe_id, + local_confirmation=f"{item.name}을 추천드릴게요. 시작을 누르면 진행합니다.", + ) + return {"recipe_id": item.recipe_id, "name": item.name, "command": item.order_text} + def snapshot(self) -> dict[str, Any]: with self._lock: payload = dict(self._state) @@ -98,6 +141,19 @@ def _on_confirmation(self, msg: String) -> None: self._state["last_confirmation"] = msg.data self._state["last_confirmation_at"] = time.time() + def _on_decision(self, msg: String) -> None: + decision = _json_or_text(msg.data) + if not isinstance(decision, dict): + return + recipe_id = str(decision.get("recipe_id") or "") + confirmation = str(decision.get("confirmation") or "") + with self._lock: + if recipe_id: + self._state["selected_recipe_id"] = recipe_id + if confirmation and not self._state.get("last_confirmation"): + self._state["last_confirmation"] = confirmation + self._state["last_confirmation_at"] = time.time() + def _on_ui_state(self, msg: String) -> None: with self._lock: self._state["ui_state"] = _json_or_text(msg.data) @@ -130,12 +186,16 @@ def do_GET(self) -> None: def do_POST(self) -> None: try: payload = self._read_json() - command = _command_from_request(self.path, payload) - node.publish_command(command) + if self.path == "/api/recommend": + result = node.recommend_menu() + self._send_json({"ok": True, **result}) + return + result = _command_from_request(self.path, payload) + node.publish_command(**result) except ValueError as exc: self._send_json({"ok": False, "error": str(exc)}, HTTPStatus.BAD_REQUEST) return - self._send_json({"ok": True, "command": command}) + self._send_json({"ok": True, "command": result["text"]}) def log_message(self, format: str, *args: object) -> None: node.get_logger().debug(format % args) @@ -193,25 +253,35 @@ def _json_or_text(text: str) -> Any: return {"text": text} -def _command_from_request(path: str, payload: dict[str, Any]) -> str: +def _command_from_request(path: str, payload: dict[str, Any]) -> dict[str, Any]: if path == "/api/order": recipe_id = str(payload.get("recipe_id", "")) for item in MENU_ITEMS: if item.recipe_id == recipe_id: - return item.order_text + return { + "text": item.order_text, + "selected_recipe_id": item.recipe_id, + "local_confirmation": ( + f"{item.name}을 선택했습니다. 시작을 누르면 진행합니다." + ), + } raise ValueError(f"unknown recipe_id: {recipe_id}") - if path == "/api/recommend": - return "메뉴 추천해줘" if path == "/api/confirm": - return "응" + return {"text": "응", "local_confirmation": "제조 시작 요청을 보냈습니다."} if path == "/api/cancel": - return "취소" + return { + "text": "취소", + "local_confirmation": "주문을 취소했습니다.", + "clear_selection": True, + } if path == "/api/command": - return str(payload.get("text", "")).strip() + return {"text": str(payload.get("text", "")).strip()} raise ValueError(f"unknown endpoint: {path}") def main(args=None): + if rclpy is None: + raise RuntimeError("ROS 2 Python packages are not available. Source the ROS environment first.") rclpy.init(args=args) node = KioskBridgeNode() try: diff --git a/src/azas_kiosk/azas_kiosk/menu_catalog.py b/src/azas_kiosk/azas_kiosk/menu_catalog.py index b7fea4a..0626b3b 100644 --- a/src/azas_kiosk/azas_kiosk/menu_catalog.py +++ b/src/azas_kiosk/azas_kiosk/menu_catalog.py @@ -16,34 +16,34 @@ class KioskMenuItem: MENU_ITEMS: tuple[KioskMenuItem, ...] = ( KioskMenuItem( recipe_id="recipe_01", - name="레드 메뉴", + name="베리 선셋", color="red", - role="주스", - description="과일감이 선명하고 가볍게 마시기 좋은 메뉴", + role="달콤한 과일감", + description="붉은 베리 톤의 산뜻하고 가벼운 시그니처 칵테일", order_text="레드 메뉴 만들어줘", ), KioskMenuItem( recipe_id="recipe_02", - name="옐로우 메뉴", + name="시트러스 글로우", color="yellow", - role="시럽", - description="달콤하고 부드러운 느낌이 강한 메뉴", + role="밝은 달콤함", + description="시트러스처럼 밝고 부드럽게 마무리되는 칵테일", order_text="옐로우 메뉴 만들어줘", ), KioskMenuItem( recipe_id="recipe_03", - name="그린 메뉴", + name="허브 가든", color="green", - role="리큐르", - description="향이 선명하고 깔끔한 여운이 있는 메뉴", + role="허브 아로마", + description="은은한 향과 깔끔한 여운을 살린 그린 칵테일", order_text="그린 메뉴 만들어줘", ), KioskMenuItem( recipe_id="recipe_04", - name="블루 메뉴", + name="오션 브리즈", color="blue", - role="럼", - description="칵테일다운 존재감과 깊이가 있는 메뉴", + role="시원한 깊이감", + description="차분한 블루 톤에 깊이감을 더한 시원한 칵테일", order_text="블루 메뉴 만들어줘", ), ) diff --git a/src/azas_kiosk/test/test_menu_catalog.py b/src/azas_kiosk/test/test_menu_catalog.py index ea11312..de0dc7d 100644 --- a/src/azas_kiosk/test/test_menu_catalog.py +++ b/src/azas_kiosk/test/test_menu_catalog.py @@ -1,4 +1,5 @@ from azas_kiosk.menu_catalog import build_menu_payload +from azas_kiosk.kiosk_node import _command_from_request def test_menu_payload_has_four_symbolic_recipes(): @@ -10,4 +11,25 @@ def test_menu_payload_has_four_symbolic_recipes(): "recipe_03", "recipe_04", ] + assert [menu["name"] for menu in menus] == [ + "베리 선셋", + "시트러스 글로우", + "허브 가든", + "오션 브리즈", + ] assert all("order_text" in menu for menu in menus) + + +def test_order_request_sets_local_kiosk_feedback(): + result = _command_from_request("/api/order", {"recipe_id": "recipe_01"}) + + assert result["text"] == "레드 메뉴 만들어줘" + assert result["selected_recipe_id"] == "recipe_01" + assert "베리 선셋" in result["local_confirmation"] + + +def test_cancel_request_clears_selected_menu(): + result = _command_from_request("/api/cancel", {}) + + assert result["text"] == "취소" + assert result["clear_selection"] diff --git a/src/azas_kiosk/web/app.js b/src/azas_kiosk/web/app.js index c4e4455..f75dcf0 100644 --- a/src/azas_kiosk/web/app.js +++ b/src/azas_kiosk/web/app.js @@ -10,10 +10,10 @@ const cocktailStatus = document.querySelector("#cocktail-status"); const voiceState = document.querySelector("#voice-state"); const colorLabels = { - red: "Red", - yellow: "Yellow", - green: "Green", - blue: "Blue", + red: "Berry", + yellow: "Citrus", + green: "Herb", + blue: "Ocean", }; async function postJson(path, payload = {}) { @@ -39,9 +39,21 @@ function renderMenus(menus) { button.dataset.recipeId = menu.recipe_id; button.innerHTML = ` ${colorLabels[menu.color] || menu.color} - ${menu.name} - ${menu.role} - ${menu.description} + + + ${menu.name} + ${menu.role} + ${menu.description} + `; button.addEventListener("click", async () => { await selectMenu(menu.recipe_id); @@ -58,6 +70,7 @@ async function selectMenu(recipeId) { } function setSelectedCard(recipeId) { + state.selectedRecipeId = recipeId || null; for (const card of menuGrid.querySelectorAll(".menu-card")) { card.classList.toggle("selected", card.dataset.recipeId === recipeId); } @@ -75,6 +88,9 @@ async function refreshState() { const status = payload.cocktail_status || {}; const prompt = payload.last_confirmation || ui.text || "주문을 기다리고 있습니다."; + if (payload.selected_recipe_id) { + setSelectedCard(payload.selected_recipe_id); + } confirmationText.textContent = prompt; lastCommand.textContent = payload.last_command || "-"; voiceState.textContent = ui.state === "speaking" ? "안내 중" : "대기 중"; @@ -89,7 +105,10 @@ document.querySelector("#recommend-button").addEventListener("click", async () = try { state.selectedRecipeId = null; setSelectedCard(null); - await postJson("/api/recommend"); + const result = await postJson("/api/recommend"); + if (result.recipe_id) { + setSelectedCard(result.recipe_id); + } await refreshState(); } catch (error) { showError(error); diff --git a/src/azas_kiosk/web/index.html b/src/azas_kiosk/web/index.html index d1b0934..3e7a82e 100644 --- a/src/azas_kiosk/web/index.html +++ b/src/azas_kiosk/web/index.html @@ -22,7 +22,7 @@

원하는 메뉴를 선택하세요

- +
diff --git a/src/azas_kiosk/web/styles.css b/src/azas_kiosk/web/styles.css index 08ada33..3741a1a 100644 --- a/src/azas_kiosk/web/styles.css +++ b/src/azas_kiosk/web/styles.css @@ -19,6 +19,11 @@ body { url("data:image/svg+xml,%3Csvg width='160' height='160' viewBox='0 0 160 160' xmlns='http://www.w3.org/2000/svg'%3E%3Cg fill='none' stroke='%23d7ded2' stroke-width='1'%3E%3Cpath d='M0 80h160M80 0v160'/%3E%3Ccircle cx='80' cy='80' r='52'/%3E%3C/g%3E%3C/svg%3E"); } +body, +button { + -webkit-tap-highlight-color: transparent; +} + button { border: 0; font: inherit; @@ -26,10 +31,12 @@ button { .shell { display: grid; - grid-template-columns: minmax(0, 1fr) 360px; - gap: 24px; + grid-template-rows: auto auto; + gap: 14px; + width: min(100%, 540px); min-height: 100vh; - padding: 28px; + margin: 0 auto; + padding: 14px; } .order-surface, @@ -43,8 +50,8 @@ button { .order-surface { display: flex; flex-direction: column; - gap: 24px; - padding: 28px; + gap: 14px; + padding: 18px; } .topbar { @@ -56,9 +63,9 @@ button { .eyebrow, .panel-label { - margin: 0 0 8px; + margin: 0 0 6px; color: #617067; - font-size: 13px; + font-size: 12px; font-weight: 700; letter-spacing: 0; text-transform: uppercase; @@ -66,8 +73,8 @@ button { h1 { margin: 0; - font-size: clamp(32px, 5vw, 58px); - line-height: 1.04; + font-size: 36px; + line-height: 1.08; letter-spacing: 0; } @@ -84,17 +91,18 @@ h1 { .menu-grid { display: grid; - grid-template-columns: repeat(2, minmax(0, 1fr)); - gap: 18px; + grid-template-columns: 1fr; + gap: 12px; flex: 1; } .menu-card { display: grid; - grid-template-rows: auto auto auto 1fr; + grid-template-columns: minmax(0, 1fr) 108px; + grid-template-rows: auto minmax(0, 1fr); gap: 12px; - min-height: 230px; - padding: 24px; + min-height: 176px; + padding: 18px; border: 2px solid transparent; border-radius: 8px; color: #15201c; @@ -116,13 +124,14 @@ h1 { } .menu-card strong { - font-size: clamp(26px, 3vw, 38px); + font-size: 30px; line-height: 1.08; letter-spacing: 0; + overflow-wrap: anywhere; } -.menu-tone, -.menu-role { +.menu-tone { + grid-column: 1 / 2; width: fit-content; padding: 6px 10px; border-radius: 999px; @@ -132,40 +141,162 @@ h1 { font-weight: 800; } -.menu-description { +.menu-copy { + display: flex; + grid-column: 1 / 2; + flex-direction: column; + gap: 8px; + min-width: 0; align-self: end; +} + +.menu-role { + color: rgba(23, 32, 29, 0.76); + font-size: 16px; + font-weight: 900; +} + +.menu-description { color: rgba(23, 32, 29, 0.72); - font-size: 17px; - line-height: 1.45; + font-size: 15px; + line-height: 1.38; +} + +.cocktail-art { + position: relative; + grid-column: 2 / 3; + grid-row: 1 / 3; + width: 104px; + height: 146px; + align-self: center; + justify-self: end; +} + +.glass { + position: absolute; + left: 20px; + top: 6px; + width: 68px; + height: 94px; + overflow: hidden; + border: 4px solid rgba(23, 32, 29, 0.78); + border-top-width: 6px; + border-radius: 8px 8px 28px 28px; + background: rgba(255, 255, 255, 0.44); + box-shadow: inset 0 10px 18px rgba(255, 255, 255, 0.44); +} + +.liquid { + position: absolute; + left: 0; + right: 0; + bottom: 0; + height: 72%; + background: var(--drink); +} + +.liquid::before { + position: absolute; + top: -10px; + left: -8px; + width: 82px; + height: 20px; + border-radius: 50%; + background: rgba(255, 255, 255, 0.34); + content: ""; +} + +.ice { + position: absolute; + width: 17px; + height: 17px; + border: 2px solid rgba(255, 255, 255, 0.72); + border-radius: 5px; + background: rgba(255, 255, 255, 0.28); + transform: rotate(14deg); +} + +.ice-one { + left: 15px; + top: 44px; +} + +.ice-two { + right: 15px; + top: 62px; + transform: rotate(-18deg); +} + +.garnish { + position: absolute; + right: -13px; + top: 12px; + width: 28px; + height: 28px; + border: 5px solid var(--garnish); + border-radius: 50%; + background: rgba(255, 255, 255, 0.78); +} + +.stem { + position: absolute; + left: 50px; + top: 100px; + width: 7px; + height: 34px; + border-radius: 999px; + background: rgba(23, 32, 29, 0.78); +} + +.base { + position: absolute; + left: 30px; + bottom: 5px; + width: 50px; + height: 9px; + border-radius: 999px; + background: rgba(23, 32, 29, 0.78); } .tone-red { + --drink: linear-gradient(180deg, #ff8378, #d33148); + --garnish: #e82f4e; background: linear-gradient(135deg, #ffebe6, #ffd1c9); } .tone-yellow { + --drink: linear-gradient(180deg, #ffe98d, #f4b72d); + --garnish: #f0bf24; background: linear-gradient(135deg, #fff5d8, #f8dc82); } .tone-green { + --drink: linear-gradient(180deg, #9be4a8, #35a96b); + --garnish: #45b66d; background: linear-gradient(135deg, #def6e7, #9ed8b6); } .tone-blue { + --drink: linear-gradient(180deg, #86c8ff, #2e75c8); + --garnish: #4d9ee4; background: linear-gradient(135deg, #dcecff, #9fc7ef); } .actions { display: grid; - grid-template-columns: 150px 150px minmax(180px, 1fr); - gap: 14px; + grid-template-columns: 1fr 1fr 1.35fr; + gap: 10px; + position: sticky; + bottom: 12px; + z-index: 2; + padding-top: 4px; } .primary-button, .secondary-button { - min-height: 72px; + min-height: 64px; border-radius: 8px; - font-size: 22px; + font-size: 20px; font-weight: 900; cursor: pointer; } @@ -187,14 +318,13 @@ h1 { .status-panel { display: flex; flex-direction: column; - justify-content: space-between; - gap: 28px; - padding: 24px; + gap: 14px; + padding: 16px; } .large-status { margin: 0; - font-size: 30px; + font-size: 22px; font-weight: 900; line-height: 1.24; letter-spacing: 0; @@ -202,13 +332,14 @@ h1 { .status-list { display: grid; - gap: 14px; + grid-template-columns: 1fr 1fr; + gap: 10px; } .status-list div { display: grid; gap: 6px; - padding: 16px; + padding: 12px; border-radius: 8px; background: #f1f4ef; } @@ -222,22 +353,84 @@ h1 { .status-list strong { min-width: 0; overflow-wrap: anywhere; - font-size: 18px; + font-size: 15px; letter-spacing: 0; } -@media (max-width: 980px) { +@media (min-width: 760px) { .shell { - grid-template-columns: 1fr; - padding: 18px; + width: min(100%, 680px); + padding: 24px; } - .menu-grid, - .actions { - grid-template-columns: 1fr; + h1 { + font-size: 46px; + } + + .menu-card { + grid-template-columns: minmax(0, 1fr) 132px; + min-height: 210px; + } + + .cocktail-art { + transform: scale(1.12); + } + + .large-status { + font-size: 26px; + } +} + +@media (max-width: 420px) { + .shell { + padding: 10px; + } + + .order-surface, + .status-panel { + border-radius: 8px; } .topbar { - flex-direction: column; + gap: 10px; + } + + h1 { + font-size: 30px; + } + + .status-pill { + min-width: 80px; + padding: 8px 10px; + font-size: 14px; + } + + .menu-card { + grid-template-columns: minmax(0, 1fr) 90px; + min-height: 162px; + padding: 15px; + } + + .menu-card strong { + font-size: 25px; + } + + .cocktail-art { + transform: scale(0.82); + transform-origin: center right; + } + + .actions { + grid-template-columns: 1fr 1fr 1.2fr; + } + + .primary-button, + .secondary-button { + min-height: 58px; + font-size: 18px; + } + + .status-list { + grid-template-columns: 1fr; } } diff --git a/src/azas_voice/azas_voice/voice_screen_node.py b/src/azas_voice/azas_voice/voice_screen_node.py new file mode 100644 index 0000000..fafd9f3 --- /dev/null +++ b/src/azas_voice/azas_voice/voice_screen_node.py @@ -0,0 +1,262 @@ +from __future__ import annotations + +from collections import deque +import json +import mimetypes +from http import HTTPStatus +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path +import threading +import time +from typing import Any + +try: + from ament_index_python.packages import get_package_share_directory +except ImportError: # pragma: no cover - allows helper tests without sourced ROS + get_package_share_directory = None + +try: + import rclpy + from rclpy.node import Node + from std_msgs.msg import String +except ImportError: # pragma: no cover - allows helper tests without sourced ROS + rclpy = None + Node = object + String = None + + +def build_initial_state() -> dict[str, Any]: + return { + "started_at": time.time(), + "last_stt": "", + "last_confirmation": "", + "ui_state": {"state": "idle", "emotion": "neutral", "text": ""}, + "decision": {}, + "confirmed_decision": {}, + "events": [], + } + + +class VoiceScreenNode(Node): + """Serve a local voice screen and aggregate Azas voice topic state.""" + + def __init__(self): + if rclpy is None or String is None or get_package_share_directory is None: + raise RuntimeError("ROS 2 Python packages are not available. Source the ROS environment first.") + super().__init__("azas_voice_screen_node") + + self.declare_parameter("host", "0.0.0.0") + self.declare_parameter("port", 8090) + self.declare_parameter("stt_topic", "/stt_result") + self.declare_parameter("decision_topic", "/azas/voice/recipe_decision") + self.declare_parameter("confirmation_topic", "/azas/voice/confirmation") + self.declare_parameter("ui_state_topic", "/azas/voice/ui_state") + self.declare_parameter("confirmed_decision_topic", "/azas/voice/confirmed_recipe_decision") + + self._lock = threading.Lock() + self._events: deque[dict[str, Any]] = deque(maxlen=12) + self._state = build_initial_state() + + self._stt_pub = self.create_publisher( + String, + str(self.get_parameter("stt_topic").value), + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("stt_topic").value), + self._on_stt, + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("decision_topic").value), + self._on_decision, + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("confirmation_topic").value), + self._on_confirmation, + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("ui_state_topic").value), + self._on_ui_state, + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("confirmed_decision_topic").value), + self._on_confirmed_decision, + 10, + ) + + self._web_root = Path(get_package_share_directory("azas_voice")) / "web" + host = str(self.get_parameter("host").value) + port = int(self.get_parameter("port").value) + self._server = ThreadingHTTPServer((host, port), self._build_handler()) + self._thread = threading.Thread(target=self._server.serve_forever, daemon=True) + self._thread.start() + self.get_logger().info(f"Azas voice screen ready at http://{host}:{port}") + + def snapshot(self) -> dict[str, Any]: + with self._lock: + payload = dict(self._state) + payload["events"] = list(self._events) + return payload + + def publish_test_utterance(self, text: str) -> None: + utterance = text.strip() + if not utterance: + raise ValueError("empty utterance") + msg = String() + msg.data = utterance + self._stt_pub.publish(msg) + with self._lock: + self._state["last_stt"] = utterance + self._state["last_stt_at"] = time.time() + + def _on_stt(self, msg: String) -> None: + text = msg.data.strip() + if not text: + return + with self._lock: + self._state["last_stt"] = text + self._state["last_stt_at"] = time.time() + self._remember("user", text) + + def _on_decision(self, msg: String) -> None: + decision = _json_or_text(msg.data) + with self._lock: + self._state["decision"] = decision + self._state["decision_at"] = time.time() + + def _on_confirmation(self, msg: String) -> None: + text = msg.data.strip() + if not text: + return + with self._lock: + self._state["last_confirmation"] = text + self._state["last_confirmation_at"] = time.time() + self._remember("azas", text) + + def _on_ui_state(self, msg: String) -> None: + with self._lock: + self._state["ui_state"] = _json_or_text(msg.data) + self._state["ui_state_at"] = time.time() + + def _on_confirmed_decision(self, msg: String) -> None: + confirmed = _json_or_text(msg.data) + with self._lock: + self._state["confirmed_decision"] = confirmed + self._state["confirmed_decision_at"] = time.time() + + def _remember(self, speaker: str, text: str) -> None: + with self._lock: + self._events.appendleft( + { + "speaker": speaker, + "text": text, + "at": time.time(), + } + ) + + def _build_handler(self): + node = self + + class VoiceScreenRequestHandler(BaseHTTPRequestHandler): + def do_GET(self) -> None: + if self.path in {"/", "/voice.html"}: + self._send_file(node._web_root / "voice.html") + return + if self.path == "/voice.css": + self._send_file(node._web_root / "voice.css") + return + if self.path == "/voice.js": + self._send_file(node._web_root / "voice.js") + return + if self.path == "/api/state": + self._send_json(node.snapshot()) + return + self.send_error(HTTPStatus.NOT_FOUND) + + def do_POST(self) -> None: + try: + payload = self._read_json() + if self.path != "/api/utterance": + raise ValueError(f"unknown endpoint: {self.path}") + text = str(payload.get("text", "")) + node.publish_test_utterance(text) + except ValueError as exc: + self._send_json({"ok": False, "error": str(exc)}, HTTPStatus.BAD_REQUEST) + return + self._send_json({"ok": True, "text": text}) + + def log_message(self, format: str, *args: object) -> None: + node.get_logger().debug(format % args) + + def _read_json(self) -> dict[str, Any]: + length = int(self.headers.get("Content-Length", "0")) + if length <= 0: + return {} + body = self.rfile.read(length).decode("utf-8") + try: + payload = json.loads(body) + except json.JSONDecodeError as exc: + raise ValueError(f"invalid json: {exc}") from exc + if not isinstance(payload, dict): + raise ValueError("json body must be an object") + return payload + + def _send_json( + self, + payload: dict[str, Any], + status: HTTPStatus = HTTPStatus.OK, + ) -> None: + data = json.dumps(payload, ensure_ascii=False).encode("utf-8") + self.send_response(status) + self.send_header("Content-Type", "application/json; charset=utf-8") + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + + def _send_file(self, path: Path) -> None: + if not path.is_file(): + self.send_error(HTTPStatus.NOT_FOUND) + return + data = path.read_bytes() + content_type = mimetypes.guess_type(path.name)[0] or "application/octet-stream" + self.send_response(HTTPStatus.OK) + self.send_header("Content-Type", f"{content_type}; charset=utf-8") + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + + return VoiceScreenRequestHandler + + def destroy_node(self): + self._server.shutdown() + self._server.server_close() + self._thread.join(timeout=1.0) + super().destroy_node() + + +def _json_or_text(text: str) -> Any: + try: + return json.loads(text) + except json.JSONDecodeError: + return {"text": text} + + +def main(args=None): + if rclpy is None: + raise RuntimeError("ROS 2 Python packages are not available. Source the ROS environment first.") + rclpy.init(args=args) + node = VoiceScreenNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/src/azas_voice/launch/azas_voice.launch.py b/src/azas_voice/launch/azas_voice.launch.py index 7f16e23..4c1ca1a 100644 --- a/src/azas_voice/launch/azas_voice.launch.py +++ b/src/azas_voice/launch/azas_voice.launch.py @@ -10,6 +10,7 @@ def generate_launch_description(): use_live_stt = LaunchConfiguration("use_live_stt") use_llm = LaunchConfiguration("use_llm") use_conversation_manager = LaunchConfiguration("use_conversation_manager") + run_voice_screen = LaunchConfiguration("run_voice_screen") use_tts = LaunchConfiguration("use_tts") enable_tts_audio = LaunchConfiguration("enable_tts_audio") tts_speech_rate = LaunchConfiguration("tts_speech_rate") @@ -21,6 +22,9 @@ def generate_launch_description(): DeclareLaunchArgument("use_live_stt", default_value="false"), DeclareLaunchArgument("use_llm", default_value="false"), DeclareLaunchArgument("use_conversation_manager", default_value="true"), + DeclareLaunchArgument("run_voice_screen", default_value="true"), + DeclareLaunchArgument("voice_screen_host", default_value="0.0.0.0"), + DeclareLaunchArgument("voice_screen_port", default_value="8090"), DeclareLaunchArgument("use_tts", default_value="true"), DeclareLaunchArgument("enable_tts_audio", default_value="true"), DeclareLaunchArgument("tts_speech_rate", default_value="1.25"), @@ -96,5 +100,21 @@ def generate_launch_description(): ], condition=IfCondition(use_tts), ), + Node( + package="azas_voice", + executable="voice_screen_node", + name="azas_voice_screen_node", + output="screen", + parameters=[ + { + "host": LaunchConfiguration("voice_screen_host"), + "port": ParameterValue( + LaunchConfiguration("voice_screen_port"), value_type=int + ), + "stt_topic": stt_topic, + } + ], + condition=IfCondition(run_voice_screen), + ), ] ) diff --git a/src/azas_voice/setup.py b/src/azas_voice/setup.py index fb87c04..f6aa164 100644 --- a/src/azas_voice/setup.py +++ b/src/azas_voice/setup.py @@ -12,6 +12,7 @@ (f"share/{package_name}", ["package.xml"]), (f"share/{package_name}/launch", glob("launch/*.launch.py")), (f"share/{package_name}/config", glob("config/*.yaml")), + (f"share/{package_name}/web", glob("web/*")), ], install_requires=["setuptools"], zip_safe=True, @@ -27,6 +28,7 @@ "recipe_mapper_node = azas_voice.recipe_mapper_node:main", "stt_node = azas_voice.stt_node:main", "tts_node = azas_voice.tts_node:main", + "voice_screen_node = azas_voice.voice_screen_node:main", "stt_pick_and_place_legacy = azas_voice.stt_pick_and_place_legacy:main", "stt_robot_control_legacy = azas_voice.stt_robot_control_legacy:main", ], diff --git a/src/azas_voice/test/test_voice_screen_node.py b/src/azas_voice/test/test_voice_screen_node.py new file mode 100644 index 0000000..4fe02b5 --- /dev/null +++ b/src/azas_voice/test/test_voice_screen_node.py @@ -0,0 +1,22 @@ +from azas_voice.voice_screen_node import _json_or_text, build_initial_state + + +def test_voice_screen_initial_state_has_dialogue_fields(): + state = build_initial_state() + + assert state["last_stt"] == "" + assert state["last_confirmation"] == "" + assert state["ui_state"]["state"] == "idle" + assert state["events"] == [] + + +def test_json_or_text_parses_decision_payload(): + payload = _json_or_text('{"intent": "make_cocktail", "recipe_id": "recipe_01"}') + + assert payload == {"intent": "make_cocktail", "recipe_id": "recipe_01"} + + +def test_json_or_text_wraps_plain_text(): + payload = _json_or_text("진행할까요?") + + assert payload == {"text": "진행할까요?"} diff --git a/src/azas_voice/web/voice.css b/src/azas_voice/web/voice.css new file mode 100644 index 0000000..aecb3fe --- /dev/null +++ b/src/azas_voice/web/voice.css @@ -0,0 +1,300 @@ +:root { + --ink: #263631; + --muted: #6e8179; + --surface: rgba(255, 255, 255, 0.78); + --line: rgba(38, 54, 49, 0.12); + --mint: #5fd8ad; + --mint-deep: #1fa77e; + --citrus: #ffd464; + --berry: #ff7e96; + --coral: #ff9a76; + --wave-listen: #ffbe4f; + --wave-speak: #39cfa2; + color-scheme: light; + font-family: + Inter, Pretendard, "Noto Sans KR", system-ui, -apple-system, BlinkMacSystemFont, + "Segoe UI", sans-serif; + background: #f8fbf4; + color: var(--ink); +} + +* { + box-sizing: border-box; +} + +body { + min-height: 100vh; + margin: 0; + background: + radial-gradient(circle at 18% 12%, rgba(255, 126, 150, 0.24), transparent 28%), + radial-gradient(circle at 82% 18%, rgba(255, 212, 100, 0.36), transparent 30%), + radial-gradient(circle at 50% 60%, rgba(95, 216, 173, 0.3), transparent 42%), + linear-gradient(180deg, #fffdf3 0%, #effbf1 54%, #eaf7ff 100%); +} + +body, +button, +input { + -webkit-tap-highlight-color: transparent; + font: inherit; +} + +button, +input { + border: 0; +} + +.voice-shell { + display: grid; + gap: 14px; + width: min(100%, 540px); + min-height: 100vh; + margin: 0 auto; + padding: 18px; +} + +.voice-header { + display: flex; + align-items: flex-start; + justify-content: space-between; + gap: 14px; +} + +.eyebrow { + margin: 0 0 6px; + color: var(--muted); + font-size: 12px; + font-weight: 800; + letter-spacing: 0; + text-transform: uppercase; +} + +h1 { + margin: 0; + font-size: 40px; + line-height: 1.05; + letter-spacing: 0; +} + +.state-pill { + flex: 0 0 auto; + padding: 10px 13px; + border-radius: 999px; + background: rgba(255, 255, 255, 0.8); + color: #10745b; + box-shadow: 0 8px 22px rgba(42, 129, 101, 0.13); + font-size: 14px; + font-weight: 900; +} + +.visual-stage { + display: grid; + justify-items: center; + gap: 18px; + padding: 26px 16px 18px; + border: 1px solid var(--line); + border-radius: 8px; + background: + linear-gradient(145deg, rgba(255, 255, 255, 0.86), rgba(255, 247, 225, 0.68)), + radial-gradient(circle at 70% 18%, rgba(255, 126, 150, 0.12), transparent 34%); + box-shadow: 0 22px 50px rgba(67, 119, 96, 0.16); +} + +.voice-orb { + --level: 0; + --speak: 0; + position: relative; + width: 230px; + height: 230px; + display: grid; + place-items: center; + transform: scale(calc(1 + var(--level) * 0.08)); + transition: transform 80ms linear; +} + +.orb-core, +.orb-ring { + position: absolute; + border-radius: 50%; +} + +.orb-core { + width: 128px; + height: 128px; + background: + radial-gradient(circle at 34% 24%, #ffffff, transparent 14%), + radial-gradient(circle at 64% 22%, rgba(255, 231, 143, 0.9), transparent 24%), + linear-gradient(145deg, #ff8aa0 0%, #ffd464 42%, #5fd8ad 100%); + box-shadow: + 0 18px 38px rgba(255, 126, 150, 0.2), + 0 0 34px rgba(95, 216, 173, 0.32), + inset 0 -16px 30px rgba(39, 112, 91, 0.18); +} + +.orb-ring { + inset: 28px; + border: 2px solid rgba(31, 167, 126, 0.22); + opacity: calc(0.42 + var(--level) * 0.58 + var(--speak) * 0.28); +} + +.ring-one { + animation: breathe 2.8s ease-in-out infinite; +} + +.ring-two { + inset: 8px; + animation: breathe 2.8s ease-in-out 560ms infinite; +} + +.voice-orb.speaking .orb-core { + animation: speakPulse 720ms ease-in-out infinite; +} + +.voice-orb.listening .orb-ring { + border-color: rgba(255, 190, 79, 0.58); +} + +#waveform { + width: 100%; + height: 96px; +} + +.mic-button, +.test-form button { + min-height: 52px; + border-radius: 8px; + background: linear-gradient(135deg, #52d7a8, #ffd464); + color: #17362d; + font-weight: 900; + cursor: pointer; + box-shadow: 0 12px 24px rgba(42, 129, 101, 0.16); +} + +.dialogue { + display: grid; + gap: 12px; +} + +.bubble { + padding: 16px; + border: 1px solid var(--line); + border-radius: 8px; + background: var(--surface); + box-shadow: 0 14px 34px rgba(67, 119, 96, 0.1); +} + +.bubble span, +.status-grid span { + display: block; + margin-bottom: 6px; + color: var(--muted); + font-size: 13px; + font-weight: 900; +} + +.bubble p { + min-height: 34px; + margin: 0; + overflow-wrap: anywhere; + font-size: 22px; + font-weight: 900; + line-height: 1.32; +} + +.bubble.user { + background: linear-gradient(145deg, rgba(255, 246, 211, 0.92), rgba(255, 255, 255, 0.74)); +} + +.bubble.azas { + background: linear-gradient(145deg, rgba(225, 255, 242, 0.96), rgba(255, 255, 255, 0.76)); +} + +.status-grid { + display: grid; + grid-template-columns: repeat(3, minmax(0, 1fr)); + gap: 10px; +} + +.status-grid div { + min-width: 0; + padding: 13px; + border-radius: 8px; + background: rgba(255, 255, 255, 0.72); + border: 1px solid var(--line); +} + +.status-grid strong { + display: block; + min-width: 0; + overflow-wrap: anywhere; + font-size: 16px; + letter-spacing: 0; +} + +.test-form { + display: grid; + grid-template-columns: minmax(0, 1fr) 88px; + gap: 10px; +} + +.test-form input { + min-width: 0; + min-height: 52px; + padding: 0 14px; + border-radius: 8px; + background: rgba(255, 255, 255, 0.76); + color: var(--ink); + outline: 1px solid var(--line); +} + +.test-form input::placeholder { + color: var(--muted); +} + +@keyframes breathe { + 0%, + 100% { + transform: scale(0.92); + } + 50% { + transform: scale(1.08); + } +} + +@keyframes speakPulse { + 0%, + 100% { + transform: scale(0.97); + } + 50% { + transform: scale(1.08); + } +} + +@media (max-width: 420px) { + .voice-shell { + padding: 12px; + } + + h1 { + font-size: 32px; + } + + .voice-orb { + width: 198px; + height: 198px; + } + + .orb-core { + width: 112px; + height: 112px; + } + + .bubble p { + font-size: 19px; + } + + .status-grid { + grid-template-columns: 1fr; + } +} diff --git a/src/azas_voice/web/voice.html b/src/azas_voice/web/voice.html new file mode 100644 index 0000000..94b8526 --- /dev/null +++ b/src/azas_voice/web/voice.html @@ -0,0 +1,62 @@ + + + + + + Azas Voice + + + +
+
+
+

Azas Voice

+

대화 상태

+
+ 대기 중 +
+ +
+
+ + + +
+ + +
+ +
+
+ 사용자 +

아직 인식된 발화가 없습니다.

+
+
+ Azas +

말씀해주시면 주문을 도와드릴게요.

+
+
+ +
+
+ 선택 메뉴 + - +
+
+ 의도 + 대기 +
+
+ 확정 + 대기 +
+
+ +
+ + +
+
+ + + diff --git a/src/azas_voice/web/voice.js b/src/azas_voice/web/voice.js new file mode 100644 index 0000000..9aca0ab --- /dev/null +++ b/src/azas_voice/web/voice.js @@ -0,0 +1,223 @@ +const orb = document.querySelector("#voice-orb"); +const waveform = document.querySelector("#waveform"); +const ctx = waveform.getContext("2d"); +const micButton = document.querySelector("#mic-button"); +const statePill = document.querySelector("#state-pill"); +const userText = document.querySelector("#user-text"); +const azasText = document.querySelector("#azas-text"); +const recipeId = document.querySelector("#recipe-id"); +const intent = document.querySelector("#intent"); +const confirmed = document.querySelector("#confirmed"); +const testForm = document.querySelector("#test-form"); +const testUtterance = document.querySelector("#test-utterance"); + +let analyser = null; +let timeData = null; +let micLevel = 0; +let micReady = false; +let currentUiState = "idle"; +let recognition = null; +let recognitionActive = false; +let browserTranscript = ""; +let browserTranscriptAt = 0; + +function labelForState(state) { + if (state === "speaking") return "Azas 응답 중"; + if (micLevel > 0.08) return "듣는 중"; + if (state === "error") return "오류"; + return "대기 중"; +} + +async function enableMicVisualizer() { + const stream = await navigator.mediaDevices.getUserMedia({ audio: true }); + const audioContext = new AudioContext(); + const source = audioContext.createMediaStreamSource(stream); + analyser = audioContext.createAnalyser(); + analyser.fftSize = 256; + analyser.smoothingTimeConstant = 0.74; + timeData = new Uint8Array(analyser.fftSize); + source.connect(analyser); + micReady = true; +} + +function enableBrowserSpeechRecognition() { + const SpeechRecognition = window.SpeechRecognition || window.webkitSpeechRecognition; + if (!SpeechRecognition) { + micButton.textContent = "마이크 시각화 켜짐"; + azasText.textContent = "이 브라우저는 음성 인식을 지원하지 않습니다. 테스트 발화 입력창을 사용해주세요."; + return; + } + + recognition = new SpeechRecognition(); + recognition.lang = "ko-KR"; + recognition.continuous = true; + recognition.interimResults = true; + + recognition.addEventListener("result", async (event) => { + let interim = ""; + for (let index = event.resultIndex; index < event.results.length; index += 1) { + const transcript = event.results[index][0].transcript.trim(); + if (!transcript) continue; + if (event.results[index].isFinal) { + browserTranscript = transcript; + browserTranscriptAt = Date.now(); + userText.textContent = transcript; + try { + await postUtterance(transcript); + await refreshState(); + } catch (error) { + azasText.textContent = error.message || String(error); + } + } else { + interim = transcript; + } + } + + if (interim) { + browserTranscript = interim; + browserTranscriptAt = Date.now(); + userText.textContent = `${interim} ...`; + } + }); + + recognition.addEventListener("end", () => { + if (recognitionActive) { + try { + recognition.start(); + } catch (error) { + recognitionActive = false; + micButton.textContent = "음성 인식 다시 켜기"; + micButton.disabled = false; + } + } + }); + + recognition.addEventListener("error", (event) => { + if (event.error === "no-speech") return; + azasText.textContent = `브라우저 음성 인식 오류: ${event.error}`; + }); + + recognition.start(); + recognitionActive = true; + micButton.textContent = "음성 인식 중"; + micButton.disabled = true; +} + +function drawWaveform() { + const width = waveform.width; + const height = waveform.height; + ctx.clearRect(0, 0, width, height); + ctx.fillStyle = "rgba(255, 255, 255, 0.34)"; + ctx.fillRect(0, 0, width, height); + + if (analyser && timeData) { + analyser.getByteTimeDomainData(timeData); + let sum = 0; + for (const value of timeData) { + const normalized = (value - 128) / 128; + sum += normalized * normalized; + } + micLevel = Math.min(1, Math.sqrt(sum / timeData.length) * 3.4); + } else { + micLevel = Math.max(0, micLevel * 0.94); + } + + const bars = 36; + const gap = 5; + const barWidth = (width - gap * (bars - 1)) / bars; + const styles = getComputedStyle(document.documentElement); + ctx.fillStyle = + currentUiState === "speaking" + ? styles.getPropertyValue("--wave-speak").trim() + : styles.getPropertyValue("--wave-listen").trim(); + + for (let index = 0; index < bars; index += 1) { + const phase = performance.now() / 180 + index * 0.52; + const idle = (Math.sin(phase) + 1) * 0.18; + const level = micReady ? micLevel : idle; + const heightScale = Math.max(0.08, idle + level * (0.55 + (index % 5) * 0.06)); + const barHeight = height * Math.min(0.9, heightScale); + const x = index * (barWidth + gap); + const y = (height - barHeight) / 2; + roundRect(ctx, x, y, barWidth, barHeight, 999); + ctx.fill(); + } + + const speakLevel = currentUiState === "speaking" ? 1 : 0; + orb.style.setProperty("--level", micLevel.toFixed(3)); + orb.style.setProperty("--speak", speakLevel); + orb.classList.toggle("listening", micLevel > 0.08 && currentUiState !== "speaking"); + orb.classList.toggle("speaking", currentUiState === "speaking"); + statePill.textContent = labelForState(currentUiState); + requestAnimationFrame(drawWaveform); +} + +function roundRect(context, x, y, width, height, radius) { + const r = Math.min(radius, width / 2, height / 2); + context.beginPath(); + context.moveTo(x + r, y); + context.arcTo(x + width, y, x + width, y + height, r); + context.arcTo(x + width, y + height, x, y + height, r); + context.arcTo(x, y + height, x, y, r); + context.arcTo(x, y, x + width, y, r); + context.closePath(); +} + +async function refreshState() { + const response = await fetch("/api/state"); + const state = await response.json(); + const ui = state.ui_state || {}; + const decision = state.decision || {}; + const confirmedDecision = state.confirmed_decision || {}; + const recentBrowserSpeech = Date.now() - browserTranscriptAt < 1800; + + currentUiState = ui.state || "idle"; + userText.textContent = recentBrowserSpeech + ? browserTranscript + : state.last_stt || "아직 인식된 발화가 없습니다."; + azasText.textContent = state.last_confirmation || ui.text || "말씀해주시면 주문을 도와드릴게요."; + recipeId.textContent = decision.recipe_id || confirmedDecision.recipe_id || "-"; + intent.textContent = decision.intent || "대기"; + confirmed.textContent = confirmedDecision.confirmed ? "확정됨" : "대기"; +} + +async function postUtterance(text) { + const response = await fetch("/api/utterance", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ text }), + }); + const result = await response.json(); + if (!response.ok || !result.ok) { + throw new Error(result.error || "발화를 전송하지 못했습니다."); + } +} + +micButton.addEventListener("click", async () => { + try { + await enableMicVisualizer(); + enableBrowserSpeechRecognition(); + } catch (error) { + micButton.textContent = "마이크 권한 필요"; + azasText.textContent = error.message || String(error); + } +}); + +testForm.addEventListener("submit", async (event) => { + event.preventDefault(); + const text = testUtterance.value.trim(); + if (!text) return; + try { + await postUtterance(text); + testUtterance.value = ""; + await refreshState(); + } catch (error) { + azasText.textContent = error.message || String(error); + } +}); + +drawWaveform(); +refreshState().catch(() => {}); +setInterval(() => { + refreshState().catch(() => {}); +}, 500); diff --git a/tools/checks/check_kiosk_voice_flow.sh b/tools/checks/check_kiosk_voice_flow.sh new file mode 100755 index 0000000..13e2c47 --- /dev/null +++ b/tools/checks/check_kiosk_voice_flow.sh @@ -0,0 +1,234 @@ +#!/usr/bin/env bash +set -euo pipefail + +# End-to-end no-hardware check: +# kiosk HTTP order button -> /stt_result -> recipe mapper -> conversation manager +# kiosk HTTP confirm button -> /stt_result -> confirmed recipe decision +# +# No robot motion, gripper command, dispenser command, coordinates, or calibration +# values are generated or used. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +LOG_DIR="${LOG_DIR:-/tmp/azas_kiosk_voice_flow_check}" +KIOSK_PORT="${KIOSK_PORT:-18080}" +VOICE_SCREEN_PORT="${VOICE_SCREEN_PORT:-18090}" +KIOSK_URL="${KIOSK_URL:-http://127.0.0.1:${KIOSK_PORT}}" +ORDER_RECIPE_ID="${ORDER_RECIPE_ID:-recipe_01}" +TIMEOUT_SEC="${TIMEOUT_SEC:-15.0}" +START_STACK="${START_STACK:-true}" + +mkdir -p "${LOG_DIR}" +export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +mkdir -p "${ROS_LOG_DIR}" + +set +u +source /opt/ros/humble/setup.bash +source "${ROOT_DIR}/install/setup.bash" +set -u + +voice_pid="" +kiosk_pid="" + +terminate_tree() { + local pid="$1" + if [[ -z "${pid}" ]]; then + return + fi + pkill -TERM -P "${pid}" 2>/dev/null || true + if kill -0 "${pid}" 2>/dev/null; then + kill "${pid}" 2>/dev/null || true + fi + sleep 1 + pkill -KILL -P "${pid}" 2>/dev/null || true + if kill -0 "${pid}" 2>/dev/null; then + kill -KILL "${pid}" 2>/dev/null || true + fi +} + +cleanup() { + if [[ "${START_STACK}" != "true" ]]; then + return + fi + terminate_tree "${kiosk_pid}" + terminate_tree "${voice_pid}" + wait "${kiosk_pid}" "${voice_pid}" 2>/dev/null || true +} +trap cleanup EXIT INT TERM + +if [[ "${START_STACK}" == "true" ]]; then + rm -f "${LOG_DIR}/voice.log" "${LOG_DIR}/kiosk.log" + + echo "[Azas] Starting temporary voice stack for flow check" + ros2 launch azas_voice azas_voice.launch.py \ + use_live_stt:=false \ + use_tts:=false \ + enable_tts_audio:=false \ + use_llm:=false \ + run_voice_screen:=true \ + voice_screen_host:=127.0.0.1 \ + voice_screen_port:="${VOICE_SCREEN_PORT}" \ + >"${LOG_DIR}/voice.log" 2>&1 & + voice_pid="$!" + + echo "[Azas] Starting temporary kiosk on ${KIOSK_URL}" + ros2 launch azas_kiosk azas_kiosk.launch.py \ + host:=127.0.0.1 \ + port:="${KIOSK_PORT}" \ + >"${LOG_DIR}/kiosk.log" 2>&1 & + kiosk_pid="$!" +else + echo "[Azas] START_STACK=false; using existing kiosk at ${KIOSK_URL}" +fi + +python3 - "${KIOSK_URL}" "${ORDER_RECIPE_ID}" "${TIMEOUT_SEC}" <<'PY' +import json +import sys +import time +import urllib.error +import urllib.request + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + + +class KioskVoiceFlowCheck(Node): + def __init__(self): + super().__init__("kiosk_voice_flow_check") + self.stt_messages = [] + self.decisions = [] + self.confirmed = [] + self.confirmations = [] + self.create_subscription(String, "/stt_result", self._on_stt, 10) + self.create_subscription(String, "/azas/voice/recipe_decision", self._on_decision, 10) + self.create_subscription( + String, + "/azas/voice/confirmed_recipe_decision", + self._on_confirmed, + 10, + ) + self.create_subscription(String, "/azas/voice/confirmation", self._on_confirmation, 10) + + def _on_stt(self, msg): + self.stt_messages.append(msg.data) + + def _append_json(self, target, msg): + try: + target.append(json.loads(msg.data)) + except json.JSONDecodeError: + target.append({"invalid_json": msg.data}) + + def _on_decision(self, msg): + self._append_json(self.decisions, msg) + + def _on_confirmed(self, msg): + self._append_json(self.confirmed, msg) + + def _on_confirmation(self, msg): + self.confirmations.append(msg.data) + + +def http_json(method, url, payload=None): + data = None + headers = {} + if payload is not None: + data = json.dumps(payload, ensure_ascii=False).encode("utf-8") + headers["Content-Type"] = "application/json" + request = urllib.request.Request(url, data=data, headers=headers, method=method) + with urllib.request.urlopen(request, timeout=2.0) as response: + return json.loads(response.read().decode("utf-8")) + + +def wait_for_http(url, deadline): + last_error = None + while time.monotonic() < deadline: + try: + http_json("GET", url + "/api/state") + return True + except (urllib.error.URLError, TimeoutError, json.JSONDecodeError) as exc: + last_error = exc + time.sleep(0.2) + print(f"[FAIL] kiosk HTTP endpoint did not become ready: {last_error}") + return False + + +def wait_until(node, deadline, predicate, label): + while time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if predicate(): + return True + print(f"[FAIL] timed out waiting for {label}") + return False + + +def main(): + kiosk_url = sys.argv[1].rstrip("/") + recipe_id = sys.argv[2] + timeout_sec = float(sys.argv[3]) + deadline = time.monotonic() + timeout_sec + + if not wait_for_http(kiosk_url, deadline): + return 1 + + rclpy.init() + node = KioskVoiceFlowCheck() + + if not wait_until( + node, + deadline, + lambda: node.count_subscribers("/stt_result") > 0 + and node.count_publishers("/azas/voice/recipe_decision") > 0, + "voice subscriptions/publishers", + ): + node.destroy_node() + rclpy.shutdown() + return 1 + + print(f"[Azas] POST /api/order recipe_id={recipe_id}") + order_result = http_json("POST", kiosk_url + "/api/order", {"recipe_id": recipe_id}) + print(json.dumps(order_result, ensure_ascii=False)) + + if not wait_until( + node, + deadline, + lambda: any(item.get("recipe_id") == recipe_id for item in node.decisions), + "/azas/voice/recipe_decision", + ): + print("[DEBUG] stt_messages=", node.stt_messages) + print("[DEBUG] decisions=", json.dumps(node.decisions, ensure_ascii=False)) + node.destroy_node() + rclpy.shutdown() + return 1 + + print("[Azas] POST /api/confirm") + confirm_result = http_json("POST", kiosk_url + "/api/confirm", {}) + print(json.dumps(confirm_result, ensure_ascii=False)) + + if not wait_until( + node, + deadline, + lambda: any(item.get("confirmed") and item.get("recipe_id") == recipe_id for item in node.confirmed), + "/azas/voice/confirmed_recipe_decision", + ): + print("[DEBUG] stt_messages=", node.stt_messages) + print("[DEBUG] confirmations=", node.confirmations) + print("[DEBUG] confirmed=", json.dumps(node.confirmed, ensure_ascii=False)) + node.destroy_node() + rclpy.shutdown() + return 1 + + print("[PASS] kiosk HTTP order and confirm reached azas_voice confirmed decision") + print("[INFO] stt_messages=", node.stt_messages) + print("[INFO] latest_decision=", json.dumps(node.decisions[-1], ensure_ascii=False)) + print("[INFO] latest_confirmed=", json.dumps(node.confirmed[-1], ensure_ascii=False)) + node.destroy_node() + rclpy.shutdown() + return 0 + + +raise SystemExit(main()) +PY + +echo "[Azas] Flow check logs:" +echo " ${LOG_DIR}/voice.log" +echo " ${LOG_DIR}/kiosk.log" diff --git a/tools/run/run_kiosk_voice_demo.sh b/tools/run/run_kiosk_voice_demo.sh new file mode 100755 index 0000000..4023697 --- /dev/null +++ b/tools/run/run_kiosk_voice_demo.sh @@ -0,0 +1,101 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Start the Azas voice stack and kiosk UI together for a no-hardware ordering demo. +# This does not send robot motion, gripper, dispenser, coordinate, or calibration commands. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +LOG_DIR="${LOG_DIR:-/tmp/azas_kiosk_voice_demo}" +VOICE_PORT="${VOICE_PORT:-8090}" +KIOSK_PORT="${KIOSK_PORT:-8080}" +HOST="${HOST:-0.0.0.0}" +USE_LIVE_STT="${USE_LIVE_STT:-false}" +USE_TTS="${USE_TTS:-true}" +ENABLE_TTS_AUDIO="${ENABLE_TTS_AUDIO:-true}" +USE_LLM="${USE_LLM:-false}" +ENABLE_LLM="${ENABLE_LLM:-false}" + +mkdir -p "${LOG_DIR}" +export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +mkdir -p "${ROS_LOG_DIR}" + +set +u +source /opt/ros/humble/setup.bash +source "${ROOT_DIR}/install/setup.bash" +set -u + +voice_pid="" +kiosk_pid="" + +terminate_tree() { + local pid="$1" + if [[ -z "${pid}" ]]; then + return + fi + pkill -TERM -P "${pid}" 2>/dev/null || true + if kill -0 "${pid}" 2>/dev/null; then + kill "${pid}" 2>/dev/null || true + fi + sleep 1 + pkill -KILL -P "${pid}" 2>/dev/null || true + if kill -0 "${pid}" 2>/dev/null; then + kill -KILL "${pid}" 2>/dev/null || true + fi +} + +cleanup() { + terminate_tree "${kiosk_pid}" + terminate_tree "${voice_pid}" + wait "${kiosk_pid}" "${voice_pid}" 2>/dev/null || true +} +trap cleanup EXIT INT TERM + +echo "[Azas] Starting voice stack" +ros2 launch azas_voice azas_voice.launch.py \ + use_live_stt:="${USE_LIVE_STT}" \ + use_tts:="${USE_TTS}" \ + enable_tts_audio:="${ENABLE_TTS_AUDIO}" \ + use_llm:="${USE_LLM}" \ + enable_llm:="${ENABLE_LLM}" \ + run_voice_screen:=true \ + voice_screen_host:="${HOST}" \ + voice_screen_port:="${VOICE_PORT}" \ + >"${LOG_DIR}/voice.log" 2>&1 & +voice_pid="$!" + +echo "[Azas] Starting kiosk" +ros2 launch azas_kiosk azas_kiosk.launch.py \ + host:="${HOST}" \ + port:="${KIOSK_PORT}" \ + >"${LOG_DIR}/kiosk.log" 2>&1 & +kiosk_pid="$!" + +sleep 3 + +if ! kill -0 "${voice_pid}" 2>/dev/null; then + echo "[FAIL] azas_voice launch exited early. Last log lines:" + tail -n 120 "${LOG_DIR}/voice.log" || true + exit 1 +fi + +if ! kill -0 "${kiosk_pid}" 2>/dev/null; then + echo "[FAIL] azas_kiosk launch exited early. Last log lines:" + tail -n 120 "${LOG_DIR}/kiosk.log" || true + exit 1 +fi + +cat < Date: Mon, 8 Jun 2026 15:59:01 +0900 Subject: [PATCH 27/88] Make cocktail panel execute the real integrated flow Unify the operator-facing panel around the actual hardware sequence: connect robot/gripper/camera, acquire the cup through side-grip, scan dispenser colors, run the measured dispenser recipe cycle, and place the cup holder result. Keep RViz preview and one-click scripts available while guarding real-motion setup against stale virtual Doosan sessions.\n\nConstraint: Cup poses remain vision-supplied; no generated or hardcoded cup coordinates were introduced.\nConstraint: New behaviors include developer-provided lid-pick and lying-cup pick integration surfaces plus dispenser/cocktail panel controls.\nRejected: Leaving the full-cocktail button to skip side-grip | the dispenser recipe assumes a cup is already grasped.\nConfidence: medium\nScope-risk: moderate\nDirective: Restart azas-panel after this commit so the updated server prerequisite ordering and HTML buttons are loaded.\nTested: node --check extracted robot_pipeline_control.html script; python3 -m py_compile changed Python launch/nodes/server; verified full-flow queue expansion order.\nNot-tested: Real hardware motion and camera/gripper physical execution. --- COMMANDS.md | 157 +++ docs/real_robot_full_command_runbook.md | 561 ++++++++++ docs/robot_pipeline_control.html | 960 +++++++++++------- omx_wiki/index.md | 3 +- omx_wiki/log.md | 4 + omx_wiki/session-log-2026-06-07-9-5l0d8p.md | 18 + .../launch/color_scan_pose_rviz.launch.py | 104 ++ .../dispenser_press_cycle_moveit.launch.py | 39 +- .../rviz/azas_cocktail_collision_preview.rviz | 87 ++ src/azas_bringup/rviz/color_scan_pose.rviz | 89 ++ .../dispenser_press_cycle_moveit_node.py | 89 +- .../m0609_shake_joint_state_node.py | 25 +- ...measured_dispenser_collision_scene_node.py | 48 +- .../check_panel_service_discovery_race.py | 20 +- tools/run/check_one_click_cocktail_config.sh | 107 ++ tools/run/check_one_click_cocktail_ready.sh | 153 +++ tools/run/check_one_click_cocktail_result.sh | 92 ++ tools/run/remove_moveit_collision_objects.py | 73 ++ tools/run/report_cocktail_now_status.sh | 70 ++ tools/run/robot_pipeline_control_server.py | 501 +++++++-- .../run_cocktail_collision_rviz_preview.sh | 110 ++ tools/run/run_cocktail_now_real.sh | 81 ++ .../run_course_dispenser_press_cycle_rviz.sh | 186 +++- tools/run/run_doosan_real_m0609.sh | 2 +- .../run_measured_dispenser_recipe_sequence.py | 30 +- tools/run/run_one_click_cocktail_real.sh | 379 +++++++ tools/run/show_cocktail_motion_preview.sh | 53 + tools/run/show_color_scan_pose_rviz.sh | 35 + tools/run/stop_cocktail_motion_preview.sh | 96 ++ .../smoke_one_click_cocktail_no_motion.sh | 161 +++ 30 files changed, 3841 insertions(+), 492 deletions(-) create mode 100644 docs/real_robot_full_command_runbook.md create mode 100644 omx_wiki/session-log-2026-06-07-9-5l0d8p.md create mode 100644 src/azas_bringup/launch/color_scan_pose_rviz.launch.py create mode 100644 src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz create mode 100644 src/azas_bringup/rviz/color_scan_pose.rviz create mode 100755 tools/run/check_one_click_cocktail_config.sh create mode 100755 tools/run/check_one_click_cocktail_ready.sh create mode 100755 tools/run/check_one_click_cocktail_result.sh create mode 100755 tools/run/remove_moveit_collision_objects.py create mode 100755 tools/run/report_cocktail_now_status.sh create mode 100755 tools/run/run_cocktail_collision_rviz_preview.sh create mode 100755 tools/run/run_cocktail_now_real.sh create mode 100755 tools/run/run_one_click_cocktail_real.sh create mode 100755 tools/run/show_cocktail_motion_preview.sh create mode 100755 tools/run/show_color_scan_pose_rviz.sh create mode 100755 tools/run/stop_cocktail_motion_preview.sh create mode 100755 tools/smoke/smoke_one_click_cocktail_no_motion.sh diff --git a/COMMANDS.md b/COMMANDS.md index fe6eca2..cc56223 100644 --- a/COMMANDS.md +++ b/COMMANDS.md @@ -117,6 +117,78 @@ ros2 launch dsr_bringup2 dsr_bringup2_moveit.launch.py \ model:=m0609 mode:=virtual host:=127.0.0.1 port:=12345 ``` +### 칵테일 디스펜서 전체 사이클 RViz preview + +컵을 디스펜서 앞에 놓고 → 그리퍼를 완전히 열고 → 안전하게 위로 올라간 뒤 +→ 빈 그리퍼를 닫고 → 측정된 프레스 조인트에서 펌프질 → 컵을 다시 잡는 +통합 사이클을 RViz에서 먼저 봅니다. + +```bash +cd /home/ssu/Azas +bash tools/run/stop_cocktail_motion_preview.sh +bash tools/run/show_cocktail_motion_preview.sh 1x1 +``` + +`show_cocktail_motion_preview.sh`의 기본 RViz는 교안의 MoveIt RViz +(`RVIZ_MODE=bringup`)라서 주황색 로봇/Trajectory/PlanningScene 화면으로 보입니다. +하얀 RobotModel 중심의 디버그 화면이 필요할 때만 `RVIZ_MODE=clean`을 붙입니다. + +동일한 동작을 환경변수로 직접 실행하려면: + +```bash +cd /home/ssu/Azas +RECIPE_DISPENSER_IDS=1x1 \ +DISPENSER_COLLISION_OBJECTS=1 \ +KEEP_ALIVE_AFTER_DONE=1 \ +RESET_EXISTING_VIRTUAL_PREVIEW=1 \ +REPLACE_EXISTING_RVIZ=1 \ +bash tools/run/run_cocktail_collision_rviz_preview.sh +``` + +예: 1번 디스펜서 2회 프레스 + +```bash +RECIPE_DISPENSER_IDS=1x2 \ +DISPENSER_COLLISION_OBJECTS=1 \ +KEEP_ALIVE_AFTER_DONE=1 \ +bash tools/run/run_cocktail_collision_rviz_preview.sh +``` + +RViz preview가 떠 있는 상태에서 실제 로봇 one-click 스크립트를 실행하면 +virtual/emulator 세션과 실제 세션이 섞이지 않도록 거부합니다. + +preview를 닫고 실제 로봇 실행으로 전환하려면: + +```bash +bash tools/run/stop_cocktail_motion_preview.sh +``` + +이 정리 스크립트는 preview shell뿐 아니라 `dsr_bringup2_moveit.launch.py mode:=virtual`, +`run_emulator`, `DRCF M0609`, 관련 RViz까지 확인합니다. 남은 virtual/emulator가 +있으면 실제 one-click은 계속 거부됩니다. + +### 색상 스캔 자세 RViz preview + +디스펜서 색상 구분 전에 쓰는 카메라 보기 관절 자세 +`[0, 10, 32, 0, 100, 90]°`를 실제 로봇 명령 없이 RViz에서 표시합니다. + +```bash +cd /home/ssu/Azas +bash tools/run/show_color_scan_pose_rviz.sh +``` + +검증용으로 RViz 창 없이 `/joint_states`만 확인하려면: + +```bash +USE_RVIZ=false bash tools/run/show_color_scan_pose_rviz.sh +``` + +현재 상태가 실제 실행 가능한지 확인하려면: + +```bash +bash tools/run/check_one_click_cocktail_ready.sh +``` + --- ## 4. 비-하드웨어 점검 @@ -308,6 +380,88 @@ bash tools/run/run_connected_robot_control.sh `run_robot_real.sh`는 strict gate stamp와 측정 config를 다시 확인한 뒤에도, operator 확인 전 `detected:upright` cup pose와 실제 camera-derived tumbler pose를 요구합니다. +### 7-6. 실제 로봇 디스펜서 통합 사이클 one-click + +RViz preview로 동작을 확인한 뒤, 실제 로봇 연결부터 통합 디스펜서 사이클까지 +한 번에 실행합니다. 실행 전 virtual/RViz preview 세션은 종료되어 있어야 합니다. +패널에서는 `실제 실행 준비확인` 버튼으로 상태를 보고, `실제 one-click` 버튼으로 +preview 정리 후 동일한 통합 실행을 시작할 수 있습니다. + +```bash +cd /home/ssu/Azas + +# preview/emulator가 남아 있으면 먼저 정리 +bash tools/run/stop_cocktail_motion_preview.sh + +# 현재 real/RG2 서비스 상태 확인 +TCP_CHECK_SEC=1 TCP_HARD_BLOCK=1 RECIPE_DISPENSER_IDS=1x1 \ +bash tools/run/check_one_click_cocktail_ready.sh || true + +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +RECIPE_DISPENSER_IDS=1x1 \ +ROBOT_HOST=192.168.1.100 \ +bash tools/run/run_cocktail_now_real.sh +``` + +`run_cocktail_now_real.sh`는 내부에서도 preview 정리를 한 번 더 수행합니다. 따라서 +운영 명령은 위 한 줄로 충분하지만, RViz preview에서 바로 넘어오는 경우에는 정리 로그가 +`Preview stop complete`인지 확인하고 진행합니다. + +예: 1번 디스펜서 2회 프레스 + +```bash +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +ROBOT_HOST=192.168.1.100 \ +bash tools/run/run_cocktail_now_real.sh 1x2 +``` + +이 스크립트는 `/dsr01` 아래 실제 Doosan motion 서비스, `/jarvis/rg2/set_width` +그리퍼 서비스, 측정 디스펜서 collision publisher를 준비한 뒤 +`run_measured_dispenser_recipe_sequence.py --execute --confirm`으로 +컵놓기→프레스→다시잡기 사이클을 실행합니다. 컵 좌표는 직접 입력하지 않고 +기존 비전/pose 파이프라인과 측정 디스펜서 pose만 사용합니다. +`calibration.yaml`에 `press_contact_joints_deg`가 있는 디스펜서는 +해당 측정 조인트를 프레스 접촉 자세의 기준으로 사용하고, 설정된 Cartesian +pre-pose를 먼저 강제로 타지 않습니다. 접촉 조인트 도달 후 live TCP를 읽어 +그 위치의 Z만 올리고/내리며 `RECIPE_DISPENSER_IDS=1x2` 같은 반복 프레스를 수행합니다. +실제 Doosan bringup 전에 `check_one_click_cocktail_config.sh`가 먼저 실행되어 +해당 레시피의 front-hold pose와 press contact joint가 모두 있는지 확인합니다. +motion service가 아직 없으면 `check_one_click_cocktail_ready.sh`와 +`run_one_click_cocktail_real.sh`가 `ROBOT_HOST:12345` TCP 연결을 먼저 확인합니다. +`[WARN] Doosan TCP not reachable now` 또는 연결 timeout 진단이 나오면 프레스 로직으로 +진입하지 못한 상태이므로 로봇 컨트롤러 IP/네트워크/펜던트 상태를 먼저 복구해야 합니다. +`run_cocktail_now_real.sh`는 실제 실행 모드에서 이 TCP 불가 상태를 hard-block으로 +처리하고, `DRY_RUN=1`일 때만 명령 경로 확인을 위해 계속 진행합니다. + +정상 종료 시 콘솔과 `log/manual/one_click_real_integrated_recipe.log`에 +`[PASS] measured dispenser recipe sequence completed`가 남고, +마지막에 `get_current_posj`/`get_current_posx` 샘플을 출력합니다. +실패 시에는 실패 stage와 통합 로그 tail을 바로 출력합니다. +실행 후 로그만 다시 판정하려면: + +```bash +bash tools/run/check_one_click_cocktail_result.sh +``` + +로그 tail, 관련 프로세스, 결과 판정을 한 번에 모으려면: + +```bash +bash tools/run/report_cocktail_now_status.sh +``` + +프레스 전 안전 상승 높이, 누르는 깊이, RG2 대기시간은 환경변수로 조절할 수 있습니다. + +```bash +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +RECIPE_DISPENSER_IDS=1x2 \ +PRESS_PRE_LIFT_M=0.35 \ +PRESS_TRANSIT_HEIGHT_M=0.30 \ +PRESS_DEPTH_M=0.07 \ +RG2_OPEN_SETTLE_SECONDS=6.0 \ +ROBOT_HOST=192.168.1.100 \ +bash tools/run/run_one_click_cocktail_real.sh +``` + ## 8. 스모크 테스트 하드웨어 없이 실행 가능한 자동화 테스트입니다. @@ -319,6 +473,9 @@ bash tools/smoke/smoke_pick_and_align_no_motion.sh # 제어 경로 엔드투엔드 스모크 bash tools/smoke/smoke_control_path.sh +# 실제 모션 없이 one-click 칵테일 경로/패널 명령 생성 검증 +bash tools/smoke/smoke_one_click_cocktail_no_motion.sh + # 가짜 하드웨어 서비스 스모크 bash tools/smoke/smoke_fake_hardware_path.sh diff --git a/docs/real_robot_full_command_runbook.md b/docs/real_robot_full_command_runbook.md new file mode 100644 index 0000000..9eb8765 --- /dev/null +++ b/docs/real_robot_full_command_runbook.md @@ -0,0 +1,561 @@ +# 실제 로봇 통합 명령어 총정리 + +이 문서는 실제 Doosan M0609, RG2, RealSense, 색상 구분, 디스펜서 투입, 컵홀더 재픽업, 쉐이킹까지 현장에서 쓰는 명령을 한 곳에 모은 런북입니다. + +중요: + +- 컵 좌표는 사람이 직접 넣지 않습니다. +- 컵 위치는 비전 파이프라인의 `/jarvis/tumbler_dispenser/tumbler_pose` 또는 측정된 `calibration.yaml` 값을 사용합니다. +- RViz 명령은 미리보기입니다. 실제 로봇 연결/모션 명령과 섞어 쓰지 마세요. +- 실제 모션 전에 비상정지, 주변 장애물, 컵 뚜껑, 디스펜서 위치, 그리퍼 상태를 확인하세요. + +--- + +## 0. 기본 터미널 준비 + +새 터미널마다 기본으로 실행합니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash +``` + +기본 환경값입니다. 현장 IP가 다르면 값만 바꿉니다. + +```bash +export ROBOT_HOST=192.168.1.100 +export RT_HOST=192.168.1.101 +export ROBOT_NAME=dsr01 +export SERVICE_PREFIX=dsr01 +export RG2_IP=192.168.1.1 +``` + +--- + +## 1. 제어 패널 실행 + +브라우저 패널에서 단계별 실행/명령 편집을 하려면 이것을 먼저 켭니다. + +```bash +cd /home/ssu/Azas +bash tools/run/run_robot_pipeline_control_panel.sh +``` + +패널이 보여주는 명령은 `tools/run/robot_pipeline_control_server.py`의 단계 정의와 저장된 명령 override를 기준으로 합니다. + +--- + +## 2. 실제 로봇 연결 + +로봇 bringup 터미널입니다. 이 터미널은 계속 켜둡니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +ROBOT_HOST=192.168.1.100 \ +ROBOT_NAME=dsr01 \ +RT_HOST=192.168.1.101 \ +DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP \ +bash tools/run/run_doosan_real_m0609.sh +``` + +연결 확인: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +ros2 service list | grep /dsr01/motion +ros2 service type /dsr01/motion/move_line +ros2 service type /dsr01/motion/move_joint +python3 tools/run/ros_call_empty_service.py /dsr01/system/get_robot_state dsr_msgs2/srv/GetRobotState --timeout 8.0 +``` + +--- + +## 3. RG2 그리퍼 연결 + +그리퍼 노드 터미널입니다. 이 터미널도 계속 켜둡니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +ros2 launch azas_gripper rg2_trigger.launch.py \ + ip:=192.168.1.1 \ + port:=502 \ + connect:=true \ + open_width:=1100 \ + close_width:=0 \ + force:=300 \ + settle_seconds:=0.6 +``` + +확인: + +```bash +ros2 service list | grep /jarvis/rg2 +timeout 12s ros2 service call /jarvis/rg2/set_width azas_interfaces/srv/SetGripper "{command: 'set_width', width_m: 0.075, force_n: 25.0}" +``` + +--- + +## 4. 카메라 연결과 컵 인식 + +RealSense 카메라 실행: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +ros2 launch realsense2_camera rs_launch.py \ + camera_name:=camera \ + enable_color:=true \ + enable_depth:=true \ + align_depth.enable:=true +``` + +중요: 아래 YOLO launch는 화면을 띄우는 명령이 아닙니다. `/camera/camera/color/image_raw`를 구독해서 `/azas/cup_detection` 같은 인식 토픽을 내보내는 명령입니다. + +카메라 화면 확인: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +ros2 run rqt_image_view rqt_image_view /camera/camera/color/image_raw +``` + +패널 화면에서 보려면 패널을 켠 뒤 `카메라 갱신`을 누릅니다. + +```bash +cd /home/ssu/Azas +bash tools/run/run_robot_pipeline_control_panel.sh +``` + +브라우저: + +```text +http://127.0.0.1:8765/ +``` + +YOLO 컵/뚜껑 인식 실행: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +ros2 launch azas_bringup yolo_perception.launch.py +``` + +토픽 확인: + +```bash +ros2 topic echo /azas/cup_detection --once +ros2 topic echo /jarvis/tumbler_dispenser/tumbler_pose --once +``` + +정리: + +- `realsense2_camera`: 카메라 토픽 생성 +- `rqt_image_view`: 사람이 보는 화면 +- `yolo_perception.launch.py`: 컵/뚜껑 인식 토픽 생성 +- `dispenser_color_scan_ros.sh`: 디스펜서 색상 JSON 생성 + +--- + +## 5. 디스펜서 색깔 구분 + +먼저 로봇을 색상 스캔 자세로 보냅니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +python3 tools/run/direct_movej_joints.py \ + --service-prefix dsr01 \ + --j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90 \ + --velocity 30 \ + --acceleration 30 \ + --timeout-sec 60 \ + --execute \ + --confirm ENABLE_DIRECT_MOVEJ +``` + +색상 스캔 실행: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +bash tools/run/dispenser_color_scan_ros.sh +``` + +결과 확인: + +```bash +cat outputs/dispenser_color_map.json +``` + +실패 파일 확인: + +```bash +cat outputs/dispenser_color_map.json.failed +``` + +`outputs/dispenser_color_map.json.failed`만 있고 `outputs/dispenser_color_map.json`이 없으면 색상 스캔이 실패한 상태입니다. 보통 원인은 디스펜서가 카메라 프레임 밖에 있거나, 색상 스캔 자세/TF가 맞지 않거나, 조명 때문에 분류가 `unknown`으로 나온 경우입니다. + +수동으로 색상 맵을 확정해야 할 때는 패널 API로 저장합니다. 예시는 1번 red, 2번 blue, 3번 green, 4번 yellow입니다. + +```bash +curl -fsS \ + -X POST http://127.0.0.1:8765/api/dispenser_color_map \ + -H 'Content-Type: application/json' \ + -d '{"map":{"1":"red","2":"blue","3":"green","4":"yellow"}}' +``` + +저장 후 확인: + +```bash +cat outputs/dispenser_color_map.json +curl -fsS http://127.0.0.1:8765/api/dispenser_color_map +``` + +이 파일은 색상 레시피 실행에서 `빨강/파랑/초록...` 같은 색상 이름을 실제 디스펜서 번호로 매핑하는 데 사용됩니다. + +--- + +## 6. 음성 레시피 입력 + +마이크/STT 실행: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +ros2 launch azas_voice azas_voice.launch.py +``` + +STT 레시피 수신: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +python3 tools/run/listen_stt_recipe.py --timeout 60 +cat outputs/latest_recipe.json +``` + +--- + +## 7. 실행 전 준비도 점검 + +레시피를 디스펜서 번호로 직접 지정할 때는 `1x1,2x2,3x1` 형식을 씁니다. 예시는 1번 1회, 2번 2회, 3번 1회입니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +RECIPE_DISPENSER_IDS=1x1,2x2,3x1 \ +ROBOT_HOST=192.168.1.100 \ +ROBOT_NAME=dsr01 \ +SERVICE_PREFIX=dsr01 \ +bash tools/run/check_one_click_cocktail_ready.sh +``` + +설정만 점검: + +```bash +RECIPE_DISPENSER_IDS=1x1,2x2,3x1 \ +bash tools/run/check_one_click_cocktail_config.sh +``` + +--- + +## 8. 전체 디스펜서 통합 실행 + +실제 로봇으로 컵 픽업, 디스펜서 앞 배치, 그리퍼 열기, 디스펜서 프레스, 다시 잡기/리프트까지 실행하는 통합 명령입니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +ROBOT_HOST=192.168.1.100 \ +ROBOT_NAME=dsr01 \ +SERVICE_PREFIX=dsr01 \ +bash tools/run/run_cocktail_now_real.sh 1x1,2x2,3x1 +``` + +동일한 통합 실행을 환경변수로 지정할 수도 있습니다. + +```bash +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +RECIPE_DISPENSER_IDS=1x1,2x2,3x1 \ +ROBOT_HOST=192.168.1.100 \ +ROBOT_NAME=dsr01 \ +SERVICE_PREFIX=dsr01 \ +bash tools/run/run_cocktail_now_real.sh +``` + +--- + +## 9. 색상/음성 레시피 기반 디스펜서 실행 + +`outputs/latest_recipe.json`과 `outputs/dispenser_color_map.json`을 사용해서 색상 레시피를 디스펜서 번호로 바꿔 실행합니다. + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +python3 tools/run/run_color_recipe_sequence.py --execute --confirm +``` + +디스펜서 번호를 직접 지정해서 실행: + +```bash +python3 tools/run/run_color_recipe_sequence.py \ + --dispenser-ids 1x1,2x2,3x1 \ + --execute \ + --confirm +``` + +--- + +## 10. 디스펜서 개별 단계 명령 + +통합 스크립트가 내부에서 하는 핵심 순서입니다. + +1. 컵을 들고 선택 디스펜서 앞 측정 pose로 이동 +2. 컵을 디스펜서 앞에 놓기 +3. 그리퍼를 열고 컵 안쪽/전방에서 빠지기 +4. 디스펜서 버튼을 1회 이상 프레스 +5. 컵을 다시 side grip으로 잡기 +6. 컵을 들어 올리기 + +1번 디스펜서 앞 이동: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +python3 tools/run/move_to_measured_dispenser_front_hold.py \ + --service-prefix dsr01 \ + --dispenser-id 1 \ + --timeout-sec 180 \ + --verify-target \ + --verify-timeout-sec 70 \ + --ikin-timeout-sec 20 \ + --ikin-retries 2 \ + --target-tolerance-mm 15 \ + --no-set-current-tcp-before-move \ + --compensate-current-tcp \ + --direct-x-max 0.95 \ + --verify-link6-target \ + --no-moveit-planning-guard \ + --velocity 35 \ + --acceleration 45 \ + --target-offset-x-m 0.0 \ + --target-offset-y-m 0.0 \ + --target-offset-z-m 0.0 \ + --execute \ + --confirm ENABLE_MEASURED_DISPENSER_FRONT_HOLD +``` + +1번 디스펜서 1회 전체 사이클: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +python3 tools/run/run_measured_dispenser_recipe_sequence.py \ + --service-prefix dsr01 \ + --dispenser-ids 1 \ + --execute \ + --confirm ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE +``` + +이 러너가 `calibration.yaml`의 해당 디스펜서 측정 press pose를 읽어서 `dispenser_x/y/z`, `rx/ry/rz`를 자동으로 넣습니다. 프레스 pose 좌표를 문서에서 사람이 새로 만들거나 복사하지 않습니다. + +1번 디스펜서 앞 컵 다시 잡기: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +python3 tools/run/pick_from_measured_dispenser_front_hold.py \ + --service-prefix dsr01 \ + --dispenser-id 1 \ + --approach-velocity 20.0 \ + --approach-acceleration 25.0 \ + --pregrasp-staging \ + --pregrasp-offset-x-m 0.0 \ + --pregrasp-offset-y-m 0.0 \ + --pregrasp-offset-z-m 0.060 \ + --pregrasp-staging-velocity 12.0 \ + --pregrasp-staging-acceleration 20.0 \ + --joint1-clearance-deg 0.0 \ + --lift-m 0.100 \ + --lift-velocity 18.0 \ + --lift-acceleration 24.0 \ + --timeout-sec 120 \ + --wait-service-sec 8 \ + --verify-timeout-sec 45 \ + --target-tolerance-mm 15 \ + --gripper-grasp-width-m 0.075 \ + --gripper-force-n 25.0 \ + --x-min 0.10 \ + --x-max 0.95 \ + --execute \ + --confirm ENABLE_PICK_FROM_MEASURED_DISPENSER_FRONT_HOLD +``` + +--- + +## 11. 컵홀더에 놓고 다시 잡아서 쉐이킹 + +컵을 컵홀더 측정 pose에 놓기: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +python3 tools/run/place_side_grip_cup_in_holder.py \ + --service-prefix dsr01 \ + --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml \ + --approach-velocity 15.0 \ + --approach-acceleration 20.0 \ + --place-final-z-offset-m -0.020 \ + --place-velocity 6.0 \ + --place-acceleration 10.0 \ + --retreat-velocity 12.0 \ + --retreat-acceleration 16.0 \ + --timeout-sec 90.0 \ + --target-tolerance-mm 12.0 \ + --verify-timeout-sec 45.0 \ + --z-max 0.28 \ + --execute \ + --confirm ENABLE_CUP_HOLDER_PLACE +``` + +컵홀더에 놓인 닫힌 컵을 다시 잡고 실제 관절 쉐이킹: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +SERVICE_PREFIX=dsr01 \ +bash tools/run/run_rule_based_shake_real.sh +``` + +이 스크립트는 실제 이동 전에 터미널에서 `ENABLE_REAL_ROBOT_MOTION` 입력을 요구합니다. 내부 순서는 다음과 같습니다. + +1. 컵홀더 측정 pose 접근 +2. RG2로 컵 다시 잡기 +3. 컵홀더에서 리프트 +4. 실제 로봇 관절 쉐이킹 실행 + +패널의 `전체: 컵홀더 재픽업->쉐이킹`은 디스펜서 통합 실행 후 `place_cup_holder`, `shake_closed_cup`을 이어서 실행하는 용도입니다. + +--- + +## 12. RViz 미리보기 전용 + +아래 명령은 실제 로봇을 움직이지 않습니다. + +디스펜서/컵 collision 미리보기: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +RECIPE_DISPENSER_IDS=1x1,2x2,3x1 \ +DISPENSER_COLLISION_OBJECTS=1 \ +bash tools/run/run_cocktail_collision_rviz_preview.sh +``` + +미리보기 정리: + +```bash +cd /home/ssu/Azas +bash tools/run/stop_cocktail_motion_preview.sh +``` + +다음 명령은 쉐이킹 RViz 프리뷰입니다. 실제 로봇 연결 명령이 아닙니다. + +```bash +cd /home/ssu/Azas +ROS_DOMAIN_ID=79 \ +TARGET_X=0.430 TARGET_Y=0.080 TARGET_Z=0.135 \ +SHAKE_DELAY_SEC=4.0 \ +SHAKE_CENTER_X=0.430 SHAKE_CENTER_Y=0.080 SHAKE_CENTER_Z=0.620 \ +SHAKE_AMPLITUDE_X=0.100 SHAKE_AMPLITUDE_Y=0.040 SHAKE_AMPLITUDE_Z=0.055 \ +SHAKE_CYCLES=4 \ +SHAKE_TWIST_RX_DEG=6.0 SHAKE_TWIST_RY_DEG=3.0 SHAKE_TWIST_RZ_DEG=22.0 \ +APPROACH_LINE_TIME=3.5 \ +SHAKE_LINE_TIME=0.40 \ +MIN_SHAKE_Z=0.550 \ +bash tools/run/run_cup_target_then_shake_rviz.sh +``` + +--- + +## 13. 결과 확인과 로그 + +통합 실행 후 결과 확인: + +```bash +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +source /home/ssu/Azas/install/local_setup.bash + +SERVICE_PREFIX=dsr01 \ +bash tools/run/check_one_click_cocktail_result.sh +``` + +주요 로그: + +```bash +ls -lt log/manual | head +tail -n 120 log/manual/one_click_real_integrated_recipe.log +tail -n 120 log/manual/one_click_real_readiness.log +tail -n 120 log/manual/one_click_real_result.log +``` + +--- + +## 14. 추천 실제 운영 순서 + +터미널별로 나누면 다음 순서가 가장 덜 헷갈립니다. + +1. `bash tools/run/run_robot_pipeline_control_panel.sh` +2. 실제 로봇 연결: `bash tools/run/run_doosan_real_m0609.sh` +3. RG2 연결: `ros2 launch azas_gripper rg2_trigger.launch.py ...` +4. 카메라 연결: `ros2 launch realsense2_camera rs_launch.py ...` +5. YOLO 인식: `ros2 launch azas_bringup yolo_perception.launch.py` +6. 색상 스캔 자세 이동 후 `bash tools/run/dispenser_color_scan_ros.sh` +7. `bash tools/run/check_one_click_cocktail_ready.sh` +8. `REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE bash tools/run/run_cocktail_now_real.sh 1x1,2x2,3x1` +9. 컵홀더 놓기: `python3 tools/run/place_side_grip_cup_in_holder.py ... --execute --confirm ENABLE_CUP_HOLDER_PLACE` +10. 컵홀더 재픽업 후 쉐이킹: `SERVICE_PREFIX=dsr01 bash tools/run/run_rule_based_shake_real.sh` diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index 38d2a22..c008858 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -21,7 +21,7 @@ --yellow: #a16207; --purple: #7c3aed; --shadow: 0 14px 34px rgba(15, 23, 42, 0.10); - --log-width: 430px; + --sidebar-width: 420px; } * { box-sizing: border-box; } html { min-height: 100%; } @@ -34,10 +34,10 @@ } button, input, summary { font: inherit; } button { - min-height: 38px; + min-height: 34px; border: 0; - border-radius: 10px; - padding: 0 13px; + border-radius: 8px; + padding: 0 11px; font-weight: 850; cursor: pointer; color: #111827; @@ -100,31 +100,40 @@ .pill.kind { color: #475569; background: #f8fafc; border-color: #d7dee9; } .page { - width: min(1680px, calc(100vw - 28px)); - margin: 14px auto 34px; + width: min(1760px, calc(100vw - 24px)); + margin: 12px auto 24px; + display: grid; + grid-template-columns: var(--sidebar-width) minmax(0, 1fr); + grid-template-rows: auto minmax(0, 1fr) auto; + gap: 12px; + align-items: start; + min-height: calc(100vh - 88px); + height: auto; + overflow: visible; } .control-panel { - position: sticky; - top: 56px; + position: static; z-index: 30; + grid-column: 1; + grid-row: 1; display: grid; gap: 10px; - padding: 12px; + padding: 11px; background: rgba(255, 255, 255, 0.96); border: 1px solid var(--line); - border-radius: 16px; + border-radius: 12px; box-shadow: var(--shadow); backdrop-filter: blur(8px); } .run-row { display: grid; - grid-template-columns: minmax(210px, 1fr) auto auto auto auto; - gap: 9px; + grid-template-columns: 1fr 1fr; + gap: 8px; align-items: center; } - .search-wrap { min-width: 220px; } - .main-run { background: var(--blue); color: #fff; min-width: 138px; min-height: 44px; font-size: 15px; } + .search-wrap { grid-column: 1 / -1; min-width: 0; } + .main-run { background: var(--blue); color: #fff; min-height: 40px; font-size: 14px; } .danger { background: var(--red); color: #fff; } .dark-btn { background: #334155; color: #fff; } .plain-btn { background: #f1f5f9; border: 1px solid var(--line); color: #334155; } @@ -132,7 +141,7 @@ display: inline-flex; align-items: center; gap: 8px; - min-height: 44px; + min-height: 38px; padding: 0 12px; border: 1px solid #fecdd3; border-radius: 11px; @@ -140,52 +149,62 @@ color: var(--red); font-weight: 900; white-space: nowrap; + grid-column: 1 / -1; } .arm input { width: 19px; height: 19px; } - .quick-row { - display: flex; - flex-wrap: wrap; + display: grid; + grid-template-columns: 1fr 1fr; gap: 7px; align-items: center; } .quick-row button { - min-height: 32px; + min-height: 34px; padding: 0 11px; border: 1px solid var(--line); background: #fff; color: #475569; font-size: 13px; + min-width: 0; + white-space: normal; + line-height: 1.18; } - .quick-row .recipe-preset { - background: #ecfdf5; - border-color: #a7f3d0; - color: #047857; + .quick-note { + grid-column: 1 / -1; + padding: 8px 10px; + border: 1px solid #bbf7d0; + border-radius: 8px; + background: #f0fdf4; + color: #14532d; + font-size: 12px; + font-weight: 850; + line-height: 1.35; } .quick-start-btn { background: #1e40af !important; border-color: #1e40af !important; color: #fff !important; - font-weight: 600 !important; + font-weight: 850 !important; letter-spacing: 0.02em; } - .quick-start-btn:hover { background: #1d3faa !important; } .direct-dispenser-field { - display: inline-flex; + display: flex; align-items: center; gap: 7px; - min-height: 32px; + min-height: 34px; padding: 0 9px; border: 1px solid #99f6e4; border-radius: 8px; background: #f0fdfa; color: #115e59; font-size: 12px; - font-weight: 800; + font-weight: 900; + grid-column: 1 / -1; } .direct-dispenser-field input { width: 180px; - height: 24px; + flex: 1; + height: 26px; border: 1px solid #5eead4; border-radius: 6px; padding: 0 8px; @@ -194,54 +213,11 @@ color: #0f172a; background: #fff; } - .danger-light { color: var(--red) !important; background: #fff1f2 !important; border-color: #fecdd3 !important; } - #cocktailCyclePanel { - display: none; - margin-top: 8px; - padding: 10px 14px 12px; - background: #f3eaff; - border: 1px solid #c4b5fd; - border-radius: 10px; - font-size: 13px; - } - #cocktailCyclePanel .cycle-label { - font-weight: 700; - color: #5b21b6; - margin-bottom: 8px; - font-size: 12px; - text-transform: uppercase; - letter-spacing: 0.05em; - } - #cocktailCyclePanel .cycle-checks { - display: flex; - flex-wrap: wrap; - gap: 6px 16px; - margin-bottom: 10px; + .danger-light { + color: var(--red) !important; + background: #fff1f2 !important; + border-color: #fecdd3 !important; } - #cocktailCyclePanel label { - display: flex; - align-items: center; - gap: 5px; - cursor: pointer; - color: #3b0764; - user-select: none; - } - #cocktailCyclePanel input[type="checkbox"] { - accent-color: #7c3aed; - width: 15px; height: 15px; - cursor: pointer; - } - #cocktailCycleApplyBtn { - background: #7c3aed !important; - color: #fff !important; - border: none !important; - padding: 0 14px !important; - min-height: 30px !important; - font-size: 13px !important; - font-weight: 700 !important; - border-radius: 8px !important; - } - #cocktailCycleApplyBtn:hover { background: #6d28d9 !important; } details.settings { border-top: 1px dashed var(--line); @@ -272,23 +248,18 @@ letter-spacing: 0.02em; } - .content-grid { - display: grid; - grid-template-columns: minmax(680px, 1fr) var(--log-width); - gap: 14px; - align-items: start; - margin-top: 14px; - } - .flow-panel { - position: sticky; - top: 178px; + .content-grid { display: contents; } + .flow-panel { + position: static; z-index: 20; - margin-top: 14px; + margin-top: 0; border: 1px solid var(--line); - border-radius: 16px; + border-radius: 12px; background: #fff; box-shadow: var(--shadow); overflow: hidden; + grid-column: 2; + grid-row: 1; } .flow-panel.flash { animation: flowFlash 900ms ease-out; @@ -309,15 +280,16 @@ .flow-head h2 { margin: 0; font-size: 18px; letter-spacing: -0.01em; } .flow-help { margin-top: 3px; color: var(--muted); font-size: 13px; line-height: 1.4; } .flow-strip { - display: flex; - gap: 0; - padding: 16px; - overflow-x: auto; - background: linear-gradient(90deg, rgba(37,99,235,0.07), transparent 36%), #ffffff; + display: grid; + grid-template-columns: repeat(auto-fill, minmax(178px, 1fr)); + gap: 10px; + padding: 14px; + overflow: visible; + background: #ffffff; } .flow-empty { width: 100%; - min-height: 84px; + min-height: 66px; display: grid; place-items: center; color: var(--muted); @@ -332,6 +304,7 @@ color: var(--blue-dark); } .reference-panel { + display: none; margin-top: 14px; border: 1px solid #bfdbfe; border-radius: 16px; @@ -371,12 +344,15 @@ white-space: pre-wrap; } .camera-panel { - margin-top: 14px; + margin-top: 12px; border: 1px solid #bfdbfe; - border-radius: 16px; + border-radius: 12px; background: #fff; box-shadow: var(--shadow); overflow: hidden; + grid-column: 2; + grid-row: 3; + max-height: 240px; } .camera-head { display: flex; @@ -392,57 +368,49 @@ .camera-tools { display: flex; flex-wrap: wrap; align-items: center; justify-content: flex-end; gap: 8px; } .camera-status { color: var(--muted); font-size: 13px; font-weight: 850; } .camera-frame { - min-height: 280px; + min-height: 160px; display: grid; place-items: center; background: #020617; } .camera-frame img { width: 100%; - max-height: 520px; + max-height: 220px; object-fit: contain; display: block; color: #dbeafe; font-size: 13px; text-align: center; + } + .camera-result { + max-height: 150px; + overflow: auto; + margin: 0; + padding: 10px 12px; + border-top: 1px solid var(--line); + background: #0b1120; + color: #dbeafe; + font-family: ui-monospace, SFMono-Regular, Menlo, Consolas, monospace; + font-size: 12px; + line-height: 1.45; + white-space: pre-wrap; } .flow-item { - flex: 0 0 220px; display: grid; grid-template-columns: 34px minmax(0, 1fr) 28px; gap: 10px; align-items: start; - min-height: 104px; - padding: 12px; + min-height: 92px; + padding: 10px; border: 1px solid var(--line); - border-radius: 14px; + border-radius: 10px; background: #fff; position: relative; cursor: grab; } .flow-item.dragging { opacity: 0.55; cursor: grabbing; } - .flow-item.drop-before { box-shadow: -5px 0 0 var(--blue), 0 8px 20px rgba(15, 23, 42, 0.08); } - .flow-item.drop-after { box-shadow: 5px 0 0 var(--blue), 0 8px 20px rgba(15, 23, 42, 0.08); } - .flow-item + .flow-item { margin-left: 42px; } - .flow-item + .flow-item::before { - content: ""; - position: absolute; - left: -42px; - top: 50%; - width: 42px; - border-top: 2px solid #94a3b8; - } - .flow-item + .flow-item::after { - content: ""; - position: absolute; - left: -10px; - top: calc(50% - 5px); - width: 10px; - height: 10px; - border-top: 2px solid #94a3b8; - border-right: 2px solid #94a3b8; - transform: rotate(45deg); - } + .flow-item.drop-before { box-shadow: inset 5px 0 0 var(--blue), 0 8px 20px rgba(15, 23, 42, 0.08); } + .flow-item.drop-after { box-shadow: inset -5px 0 0 var(--blue), 0 8px 20px rgba(15, 23, 42, 0.08); } .flow-index { width: 32px; height: 32px; @@ -454,7 +422,7 @@ font-size: 13px; font-weight: 950; } - .flow-title { font-size: 14px; line-height: 1.28; font-weight: 950; } + .flow-title { font-size: 13px; line-height: 1.28; font-weight: 950; } .flow-meta { display: flex; flex-wrap: wrap; gap: 5px; margin-top: 8px; } .flow-remove { width: 28px; @@ -472,67 +440,134 @@ .flow-item.status-failed, .flow-item.status-timeout { border-color: #fca5a5; background: #fff1f2; } .flow-item.status-blocked { border-color: #fcd34d; background: #fffbeb; } - .stage { + .stage { min-width: 0; background: var(--card); border: 1px solid var(--line); - border-radius: 16px; + border-radius: 12px; box-shadow: var(--shadow); overflow: hidden; + grid-column: 1; + grid-row: 2 / span 2; + position: static; + height: 100%; + min-height: 0; + max-height: none; + display: grid; + grid-template-rows: auto 1fr; + margin-top: 12px; } .stage-head { display: flex; justify-content: space-between; gap: 12px; align-items: flex-start; - padding: 16px 18px; + padding: 12px 14px; border-bottom: 1px solid var(--line); background: linear-gradient(180deg, #ffffff, #f8fafc); } - .stage-head h2 { margin: 0; font-size: 20px; letter-spacing: -0.02em; } - .stage-help { margin-top: 4px; color: var(--muted); font-size: 13px; line-height: 1.4; } + .stage-head h2 { margin: 0; font-size: 16px; letter-spacing: -0.01em; } + .stage-help { display: none; } .board { display: grid; - grid-template-columns: repeat(auto-fill, minmax(430px, 1fr)); - gap: 12px; - padding: 14px; + grid-template-columns: 1fr; + gap: 0; + padding: 8px; align-items: start; + overflow-y: auto; + overflow-x: hidden; + min-height: 0; + overscroll-behavior: contain; } .group-title { grid-column: 1 / -1; display: flex; align-items: center; justify-content: space-between; + position: sticky; + top: 0; + z-index: 2; margin: 8px 0 0; - padding: 10px 12px; - border-radius: 12px; + padding: 6px 8px; + border-radius: 6px 6px 0 0; background: #eef2ff; color: #1e3a8a; - font-size: 15px; + font-size: 13px; font-weight: 950; letter-spacing: -0.01em; } - .step { + .group-title:first-child { margin-top: 0; } + .step { display: grid; - grid-template-columns: 34px minmax(0, 1fr); - gap: 12px; - min-height: 136px; - border: 1px solid var(--line); - border-left: 6px solid #cbd5e1; - border-radius: 15px; - padding: 13px; + grid-template-columns: 20px minmax(0, 1fr) 46px; + gap: 6px; + min-height: 34px; + border: 0; + border-bottom: 1px solid var(--line); + border-left: 3px solid #cbd5e1; + border-radius: 0; + padding: 5px 7px; background: #fff; cursor: pointer; } .step[draggable="true"] { cursor: grab; } .step.dragging { opacity: 0.55; } - .step:hover { border-color: var(--line-strong); box-shadow: 0 8px 20px rgba(15, 23, 42, 0.06); } - .step input { width: 22px; height: 22px; margin-top: 2px; } - .step-top { display: flex; justify-content: space-between; gap: 10px; align-items: flex-start; } - .step-title { font-size: 16px; font-weight: 950; line-height: 1.28; letter-spacing: -0.015em; } - .step-note { margin-top: 6px; font-size: 13px; color: var(--muted); line-height: 1.45; } - .step-meta { display: flex; flex-wrap: wrap; gap: 6px; margin-top: 10px; } - .step-actions { display: flex; flex-wrap: wrap; gap: 7px; margin-top: 10px; } + .step:hover { background: #f8fafc; box-shadow: none; } + .step input { width: 15px; height: 15px; margin-top: 1px; } + .step-top { display: block; } + .step-top .pill.kind { display: none; } + .step-title { + font-size: 12px; + font-weight: 950; + line-height: 1.25; + letter-spacing: 0; + padding-right: 2px; + white-space: nowrap; + overflow: hidden; + text-overflow: ellipsis; + } + .step-note { + display: none; + margin-top: 2px; + font-size: 11px; + color: var(--muted); + line-height: 1.35; + -webkit-line-clamp: 1; + -webkit-box-orient: vertical; + overflow: hidden; + } + .step-meta { display: flex; flex-wrap: wrap; gap: 5px; margin-top: 6px; } + .step-meta { display: none; } + .step.status-running .step-meta, + .step.status-starting .step-meta, + .step.status-started .step-meta, + .step.status-restarted .step-meta, + .step.status-passed .step-meta, + .step.status-failed .step-meta, + .step.status-timeout .step-meta, + .step.status-blocked .step-meta { + display: flex; + margin-top: 4px; + } + .step-meta .pill.kind, + .step-meta .pill.ok:not(.result-badge), + .step-meta .pill.blocked:not(.result-badge) { + display: none; + } + .step-actions { display: none; } + .step-command-toggle { + min-height: 24px; + width: 42px; + align-self: start; + padding: 0 6px; + border-radius: 999px; + border: 1px solid #cbd5e1; + background: #f8fafc; + color: #334155; + font-size: 11px; + font-weight: 900; + white-space: nowrap; + } .step-add { min-height: 32px; padding: 0 10px; @@ -552,36 +587,65 @@ font-size: 13px; font-weight: 850; } - details.command { margin-top: 10px; } - details.command summary { - cursor: pointer; - color: var(--blue-dark); - font-size: 13px; - font-weight: 900; - list-style: none; + .modal-backdrop { + position: fixed; + inset: 0; + z-index: 80; + display: none; + align-items: center; + justify-content: center; + padding: 24px; + background: rgba(15, 23, 42, 0.55); } - details.command summary::-webkit-details-marker { display: none; } - details.command summary::before { content: "▸ "; } - details.command[open] summary::before { content: "▾ "; } - .command-editor { - width: 100%; - min-height: 150px; - margin-top: 8px; - padding: 10px; + .modal-backdrop.open { display: flex; } + .command-modal { + width: min(980px, calc(100vw - 48px)); + max-height: calc(100vh - 80px); + display: grid; + grid-template-rows: auto 1fr auto; + border-radius: 12px; border: 1px solid var(--line); - border-radius: 10px; - background: #f8fafc; - color: #0f172a; - font-family: ui-monospace, SFMono-Regular, Menlo, Consolas, monospace; - font-size: 12px; - line-height: 1.45; - resize: vertical; + background: #fff; + box-shadow: 0 24px 60px rgba(15, 23, 42, 0.28); + overflow: hidden; } + .command-modal-head, .command-tools { display: flex; - flex-wrap: wrap; - gap: 8px; - margin-top: 8px; + align-items: center; + justify-content: space-between; + gap: 10px; + padding: 12px 14px; + border-bottom: 1px solid var(--line); + background: #f8fafc; + } + .command-tools { + border-top: 1px solid var(--line); + border-bottom: 0; + justify-content: flex-end; + margin-top: 0; + } + .command-modal-title { + min-width: 0; + font-size: 15px; + font-weight: 950; + white-space: nowrap; + overflow: hidden; + text-overflow: ellipsis; + } + #commandModalEditor { + width: 100%; + min-height: 420px; + height: 58vh; + padding: 14px; + border: 0; + outline: none; + resize: none; + background: #0b1120; + color: #dbeafe; + font-family: ui-monospace, SFMono-Regular, Menlo, Consolas, monospace; + font-size: 14px; + line-height: 1.55; } .command-tools button { min-height: 30px; @@ -639,17 +703,19 @@ } .log-panel { - position: sticky; - top: 178px; - height: calc(100vh - 196px); + position: static; + height: min(72vh, 760px); min-height: 520px; display: grid; grid-template-rows: 48px 1fr; border: 1px solid #1e293b; - border-radius: 16px; + border-radius: 12px; overflow: hidden; box-shadow: var(--shadow); background: #0b1120; + grid-column: 2; + grid-row: 2; + margin-top: 0; } .log-head { display: flex; @@ -670,21 +736,21 @@ background: #0b1120; color: #dbeafe; padding: 13px; - font-size: 12px; - line-height: 1.5; + font-size: 14px; + line-height: 1.55; white-space: pre-wrap; word-break: break-word; } @media (max-width: 1100px) { - .run-row { grid-template-columns: 1fr 1fr; } - .search-wrap { grid-column: 1 / -1; } + .page { grid-template-columns: 1fr; height: auto; overflow: visible; } .arm { justify-content: center; } .field-grid { grid-template-columns: repeat(2, minmax(130px, 1fr)); } - .content-grid { grid-template-columns: 1fr; } + .content-grid { display: contents; } .board { grid-template-columns: 1fr; } .stage-head { display: block; } - .flow-panel { position: static; } + .control-panel, .stage, .flow-panel, .camera-panel, .log-panel { grid-column: 1; grid-row: auto; position: static; } + .stage { max-height: none; height: auto; } .flow-head { display: block; } .log-panel { position: static; height: 420px; min-height: 420px; } } @@ -695,18 +761,61 @@ .run-row { grid-template-columns: 1fr; } .field-grid { grid-template-columns: 1fr; } .board { padding: 10px; } - .step { grid-template-columns: 30px 1fr; min-height: auto; } + .step { grid-template-columns: 30px 1fr 46px; min-height: auto; } .flow-strip { padding: 10px; } .flow-item { flex-basis: 190px; } .log-panel { height: 340px; min-height: 340px; } } + + /* Operator-first layout: no floating overlay, no covered content. */ + .quick-row.operator-actions { + grid-template-columns: 1fr; + gap: 10px; + padding: 10px; + border: 2px solid #bfdbfe; + border-radius: 12px; + background: #eff6ff; + } + .quick-row.operator-actions .quick-note { + background: #fff; + border-color: #bfdbfe; + color: #1e3a8a; + font-size: 13px; + } + .operator-button-grid { + display: grid; + grid-template-columns: 1fr; + gap: 9px; + } + .operator-button-grid.module-grid { grid-template-columns: 1fr 1fr; } + .operator-button-grid button { + min-height: 48px; + font-size: 15px; + font-weight: 950; + border-radius: 10px; + border: 1px solid var(--line); + background: #fff; + color: #334155; + } + .operator-button-grid small { font-size: 11px; font-weight: 800; opacity: 0.92; } + #startPrepBtn { background: #1d4ed8 !important; border-color: #1d4ed8 !important; color: #fff !important; } + #sideGripBtn { background: #ea580c !important; border-color: #ea580c !important; color: #fff !important; } + #colorScanJsonBtn { background: #7c3aed !important; border-color: #7c3aed !important; color: #fff !important; } + #recipeCycleBtn { background: #b91c1c !important; border-color: #b91c1c !important; color: #fff !important; } + #fullCocktailRealBtn { background: #047857 !important; border-color: #047857 !important; color: #fff !important; } + .secondary-actions { + display: grid; + grid-template-columns: 1fr 1fr; + gap: 8px; + } + .secondary-actions button { min-height: 36px; }

Azas Robot Pipeline Control

-
넓은 단계판 · 프레스 반복 시퀀스 · 오른쪽 상시 로그
+
실제 쉐이킹은 컵홀더 컵을 다시 side-grip으로 잡은 뒤 실행합니다 · RViz preview는 현장 실행 후보에서 숨김
단계 로드 전 @@ -722,54 +831,36 @@

Azas Robot Pipeline Control

- + +
-
- - -
-
-
-
-

RealSense 카메라 화면

-
패널 안에서 /camera/camera/color/image_raw를 직접 확인합니다. 카메라 시작 후 자동 갱신됩니다.
-
-
- 대기 - -
-
-
- RealSense color preview -
-
-
@@ -902,8 +977,8 @@

선택 실행 순서

-

파이프라인 단계

-
긴 명령어는 카드 안의 “명령 보기”에 숨겼습니다. 평소에는 단계 이름/상태만 보고 누르면 됩니다.
+

디버그 단계

+
평소에는 쓰지 마세요. 실제 칵테일은 왼쪽 빨간 “로봇연결→칵테일이동” 버튼 하나로 실행합니다.
@@ -920,11 +995,70 @@

파이프라인 단계

서버 연결 대기...
+ +
+
+
+

RealSense 카메라 화면

+
/camera/camera/color/image_raw 미리보기
+
+
+ 대기 + + + + +
+
+
+ RealSense color preview +
+
색상 스캔 결과 대기
+
+ + + diff --git a/omx_wiki/index.md b/omx_wiki/index.md index b1af80a..e313dd4 100644 --- a/omx_wiki/index.md +++ b/omx_wiki/index.md @@ -1,6 +1,6 @@ # Wiki Index -> 4 pages | Last updated: 2026-06-05T03:23:17.635Z +> 5 pages | Last updated: 2026-06-07T11:46:17.709Z ## session-log @@ -8,3 +8,4 @@ - [Session Log 2026-05-19](session-log-2026-05-19-0-4as20g.md) — # Session Log 2026-05-19 - [Session Log 2026-05-28](session-log-2026-05-28-3-adlm15.md) — # Session Log 2026-05-28 - [Session Log 2026-06-05](session-log-2026-06-05-8-3rrp4t.md) — # Session Log 2026-06-05 +- [Session Log 2026-06-07](session-log-2026-06-07-9-5l0d8p.md) — # Session Log 2026-06-07 diff --git a/omx_wiki/log.md b/omx_wiki/log.md index 1cce7af..bedcfd2 100644 --- a/omx_wiki/log.md +++ b/omx_wiki/log.md @@ -58,3 +58,7 @@ - **Pages:** session-log-2026-06-05-8-3rrp4t.md - **Summary:** Auto-captured session log for omx-1780629727098-3rrp4t +## [2026-06-07T11:46:17.707Z] session-end +- **Pages:** session-log-2026-06-07-9-5l0d8p.md +- **Summary:** Auto-captured session log for omx-1780832772689-5l0d8p + diff --git a/omx_wiki/session-log-2026-06-07-9-5l0d8p.md b/omx_wiki/session-log-2026-06-07-9-5l0d8p.md new file mode 100644 index 0000000..b096ef2 --- /dev/null +++ b/omx_wiki/session-log-2026-06-07-9-5l0d8p.md @@ -0,0 +1,18 @@ +--- +title: "Session Log 2026-06-07" +tags: ["session-log", "auto-captured"] +created: 2026-06-07T11:46:17.707Z +updated: 2026-06-07T11:46:17.707Z +sources: ["omx-1780832772689-5l0d8p"] +links: [] +category: session-log +confidence: medium +schemaVersion: 1 +--- + +# Session Log 2026-06-07 + +Auto-captured session metadata. +Session ID: omx-1780832772689-5l0d8p + +Review and promote significant findings to curated wiki pages via `wiki_ingest`. diff --git a/src/azas_bringup/launch/color_scan_pose_rviz.launch.py b/src/azas_bringup/launch/color_scan_pose_rviz.launch.py new file mode 100644 index 0000000..667fc65 --- /dev/null +++ b/src/azas_bringup/launch/color_scan_pose_rviz.launch.py @@ -0,0 +1,104 @@ +import math + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + + +COLOR_SCAN_JOINTS_RAD = [ + 0.0, + math.radians(10.0), + math.radians(32.0), + 0.0, + math.radians(100.0), + math.radians(90.0), +] + + +def generate_launch_description(): + use_rviz = LaunchConfiguration("use_rviz") + preview_mode = LaunchConfiguration("preview_mode") + rviz_config = LaunchConfiguration("rviz_config") + + collision_scene = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "launch", "workspace_collision_scene.launch.py"] + ) + ) + ) + gripper_tcp_tree = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "launch", "rg2_link6_tcp.launch.py"] + ) + ) + ) + robot_description = { + "robot_description": ParameterValue( + Command( + [ + FindExecutable(name="xacro"), + " ", + PathJoinSubstitution( + [FindPackageShare("dsr_description2"), "xacro", "m0609.urdf.xacro"] + ), + " color:=white simple:=true", + ] + ), + value_type=str, + ) + } + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="m0609_color_scan_pose_state_publisher", + output="screen", + parameters=[robot_description], + ) + + color_scan_joint_state = Node( + package="azas_motion", + executable="m0609_shake_joint_state_node", + name="m0609_color_scan_pose_joint_state_node", + output="screen", + parameters=[ + { + "preview_mode": preview_mode, + "loop_motion": False, + "home_joints_rad": COLOR_SCAN_JOINTS_RAD, + } + ], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + condition=IfCondition(use_rviz), + output="screen", + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("use_rviz", default_value="true"), + DeclareLaunchArgument("preview_mode", default_value="color_scan_pose_move"), + DeclareLaunchArgument( + "rviz_config", + default_value=PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "rviz", "color_scan_pose.rviz"] + ), + ), + collision_scene, + gripper_tcp_tree, + robot_state_publisher, + color_scan_joint_state, + rviz_node, + ] + ) diff --git a/src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py b/src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py index a2b7928..de82aa2 100644 --- a/src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py +++ b/src/azas_bringup/launch/dispenser_press_cycle_moveit.launch.py @@ -27,6 +27,7 @@ def generate_launch_description(): [ DeclareLaunchArgument("dispenser_id", default_value="1"), DeclareLaunchArgument("press_count", default_value="2"), + DeclareLaunchArgument("press_only", default_value="false"), DeclareLaunchArgument("start_delay_sec", default_value="4.0"), DeclareLaunchArgument("dispenser_x", default_value="0.50"), DeclareLaunchArgument("dispenser_y", default_value="0.00"), @@ -37,17 +38,34 @@ def generate_launch_description(): DeclareLaunchArgument("cup_release_retract_m", default_value="0.05"), DeclareLaunchArgument("press_ready_z", default_value="0.54"), DeclareLaunchArgument("press_down_m", default_value="0.08"), - DeclareLaunchArgument("press_up_m", default_value="0.02"), + DeclareLaunchArgument("press_up_m", default_value="0.05"), DeclareLaunchArgument("trajectory_time_scale", default_value="5.0"), DeclareLaunchArgument("planning_time_sec", default_value="5.0"), + DeclareLaunchArgument("joint_states_topic", default_value="/dsr01/joint_states"), + DeclareLaunchArgument( + "moveit_controller_action", + default_value="/dsr01/dsr_moveit_controller/follow_joint_trajectory", + ), Node( package="azas_motion", executable="dispenser_press_cycle_moveit_node", name="dispenser_press_cycle_moveit_node", output="screen", + remappings=[ + ("/joint_states", LaunchConfiguration("joint_states_topic")), + ( + "dsr_moveit_controller/follow_joint_trajectory", + LaunchConfiguration("moveit_controller_action"), + ), + ( + "/dsr_moveit_controller/follow_joint_trajectory", + LaunchConfiguration("moveit_controller_action"), + ), + ], additional_env={ "DISPENSER_ID": LaunchConfiguration("dispenser_id"), "PRESS_COUNT": LaunchConfiguration("press_count"), + "PRESS_ONLY": LaunchConfiguration("press_only"), "DISPENSER_X": LaunchConfiguration("dispenser_x"), "DISPENSER_Y": LaunchConfiguration("dispenser_y"), "CUP_PLACE_Z": LaunchConfiguration("cup_place_z"), @@ -65,6 +83,25 @@ def generate_launch_description(): moveit_config.to_dict(), moveit_py_params, { + "planning_scene_monitor_options": { + "joint_state_topic": LaunchConfiguration("joint_states_topic"), + }, + "moveit_simple_controller_manager": { + "controller_names": ["/dsr01/dsr_moveit_controller"], + "/dsr01/dsr_moveit_controller": { + "action_ns": "follow_joint_trajectory", + "type": "FollowJointTrajectory", + "default": True, + "joints": [ + "joint_1", + "joint_2", + "joint_3", + "joint_4", + "joint_5", + "joint_6", + ], + }, + }, "press_count": ParameterValue(LaunchConfiguration("press_count"), value_type=int), "start_delay_sec": ParameterValue(LaunchConfiguration("start_delay_sec"), value_type=float), "dispenser_x": ParameterValue(LaunchConfiguration("dispenser_x"), value_type=float), diff --git a/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz b/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz new file mode 100644 index 0000000..0391e93 --- /dev/null +++ b/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz @@ -0,0 +1,87 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.45 + Cell Size: 0.1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Name: Grid + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Class: rviz_default_plugins/RobotModel + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dsr01/robot_description + Enabled: true + Name: M0609 Robot + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/measured_dispenser_collision/markers + Name: Measured Dispenser Collision + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/link6_gripper/markers + Name: Link6 RG2 Gripper Markers + Value: true + Enabled: true + Global Options: + Background Color: 24; 24; 28 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.2 + Focal Point: + X: 0.40 + Y: 0.05 + Z: 0.35 + Name: Current View + Pitch: 0.58 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 2.55 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 900 + Hide Left Dock: false + Hide Right Dock: true + Width: 1280 + X: 40 + Y: 40 diff --git a/src/azas_bringup/rviz/color_scan_pose.rviz b/src/azas_bringup/rviz/color_scan_pose.rviz new file mode 100644 index 0000000..1952e61 --- /dev/null +++ b/src/azas_bringup/rviz/color_scan_pose.rviz @@ -0,0 +1,89 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.45 + Cell Size: 0.1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Name: Grid + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Class: rviz_default_plugins/RobotModel + Description Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Name: M0609 Color Scan Pose + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/measured_dispenser_collision/markers + Name: Measured Dispenser Collision + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/link6_gripper/markers + Name: Link6 RG2 Gripper Markers + Value: true + Enabled: true + Global Options: + Background Color: 24; 24; 28 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: 0.35 + Y: 0.05 + Z: 0.30 + Name: Current View + Pitch: 0.58 + Target Frame: base_link + Value: Orbit (rviz) + Yaw: 2.55 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 900 + Hide Left Dock: false + Hide Right Dock: true + Width: 1280 + X: 40 + Y: 40 diff --git a/src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py b/src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py index 961580b..0ed66d4 100644 --- a/src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py +++ b/src/azas_motion/azas_motion/dispenser_press_cycle_moveit_node.py @@ -51,6 +51,8 @@ class Config: cup_release_retract_m: float cup_place_z: float | None planning_time_sec: float + moveit_execution_settle_sec: float + press_only: bool @dataclass(frozen=True) @@ -76,6 +78,11 @@ def _env_int(name: str, default: int) -> int: return int(_env(name, str(default))) +def _env_bool(name: str, default: bool = False) -> bool: + raw = _env(name, "1" if default else "0").strip().lower() + return raw in {"1", "true", "yes", "on"} + + def _env_optional_float(name: str) -> float | None: raw = _env(name, "").strip() if not raw: @@ -93,11 +100,13 @@ def read_config() -> Config: ee_link=_env("EE_LINK", EE_LINK), calibration_path=Path(_env("CALIBRATION_PATH", str(DEFAULT_CALIBRATION))), cup_lift_m=_env_float("CUP_LIFT_M", 0.08), - press_up_m=_env_float("PRESS_UP_M", 0.02), + press_up_m=_env_float("PRESS_UP_M", 0.05), cup_pre_grasp_backoff_m=max(_env_float("CUP_PRE_GRASP_BACKOFF_M", 0.08), 0.0), cup_release_retract_m=max(_env_float("CUP_RELEASE_RETRACT_M", 0.05), 0.0), cup_place_z=_env_optional_float("CUP_PLACE_Z"), planning_time_sec=_env_float("PLANNING_TIME_SEC", 5.0), + moveit_execution_settle_sec=max(_env_float("MOVEIT_EXECUTION_SETTLE_SEC", 5.0), 0.0), + press_only=_env_bool("PRESS_ONLY", False), ) @@ -175,7 +184,9 @@ def plan_and_execute_pose_stamped(robot, arm, params, cfg: Config, label: str, g if not result: raise RuntimeError(f"planning failed at {label}") logger.info(f"Executing plan: {label}") - robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + ok = robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + if ok is False: + raise RuntimeError(f"MoveIt execution failed at {label}") logger.info(f"Execution finished: {label}") time.sleep(cfg.waypoint_hold_sec) @@ -190,7 +201,9 @@ def plan_and_execute_pose(robot, arm, params, cfg: Config, label: str, xyz_m: li if not result: raise RuntimeError(f"planning failed at {label}") logger.info(f"Executing plan: {label}") - robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + ok = robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + if ok is False: + raise RuntimeError(f"MoveIt execution failed at {label}") logger.info(f"Execution finished: {label}") time.sleep(cfg.waypoint_hold_sec) @@ -227,7 +240,9 @@ def plan_and_execute_joints(robot, arm, model, params, cfg: Config, label: str, if not result: raise RuntimeError(f"planning failed at {label}") logger.info(f"Executing plan: {label}") - robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + ok = robot.execute(group_name=cfg.planning_group, robot_trajectory=result.trajectory, blocking=True) + if ok is False: + raise RuntimeError(f"MoveIt execution failed at {label}") logger.info(f"Execution finished: {label}") time.sleep(cfg.waypoint_hold_sec) @@ -244,12 +259,18 @@ def main(args: list[str] | None = None) -> None: outlet = load_outlet(cfg) logger.info( f"Ready: measured-joint dispenser press cycle. dispenser={cfg.dispenser_id} " + f"press_count={cfg.press_count} press_only={cfg.press_only} " f"press_contact_joints_deg={outlet.press_contact_joints_deg}" ) robot = MoveItPy(node_name="dispenser_press_cycle_moveit_py") arm = robot.get_planning_component(cfg.planning_group) model = robot.get_robot_model() logger.info("MoveItPy instance created") + if cfg.moveit_execution_settle_sec > 0.0: + logger.info( + f"Waiting {cfg.moveit_execution_settle_sec:.1f}s for MoveIt trajectory execution action clients to connect" + ) + time.sleep(cfg.moveit_execution_settle_sec) plan_params = PlanRequestParameters(robot) plan_params.planning_pipeline = "ompl" plan_params.planner_id = "RRTConnectkConfigDefault" @@ -272,6 +293,66 @@ def main(args: list[str] | None = None) -> None: lin_params.planning_time = cfg.planning_time_sec logger.info("Press params: pipeline=pilz_industrial_motion_planner planner=LIN z-only Cartesian stroke") + # PRESS_ONLY는 RViz/프레스 검증용이다. 컵 배치/복귀 IK 경로를 모두 빼고, + # 사용자가 실측한 프레스 조인트 자세와 그 FK 기준 Z-only 펌프만 보여준다. + # 이 모드는 컵 좌표나 outlet IK가 섞여서 "프레스 움직임" 판단을 흐리는 것을 막는다. + if cfg.press_only: + logger.info( + "PRESS_ONLY_MODE: skipping cup placement, gripper-open release, and cup return. " + "Executing measured press joint pose followed by Z-only pump strokes." + ) + gripper_event(logger, "CLOSE", "PRESS_ONLY: 프레스 검증을 위해 빈 그리퍼를 닫은 상태로 가정") + plan_and_execute_joints( + robot, + arm, + model, + ptp_params, + cfg, + f"press_only_move_to_measured_press_contact_joints_{cfg.dispenser_id}", + outlet.press_contact_joints_deg, + logger, + ) + press_contact_pose = fk_pose_from_joints(model, outlet.press_contact_joints_deg, cfg.ee_link, logger) + press_ready_pose = clone_pose_with_z( + press_contact_pose, + press_contact_pose.position.z + max(cfg.press_up_m, 0.0), + ) + logger.info( + "PRESS_ONLY_Z_ONLY: repeating LIN strokes with fixed " + f"x={press_contact_pose.position.x:.3f}, y={press_contact_pose.position.y:.3f}, " + f"contact_z={press_contact_pose.position.z:.3f}, ready_z={press_ready_pose.position.z:.3f}" + ) + plan_and_execute_pose_stamped( + robot, + arm, + lin_params, + cfg, + "press_only_linear_lift_from_contact_to_press_ready_z_only", + pose_stamped_from_pose(cfg, press_ready_pose), + logger, + ) + for index in range(1, cfg.press_count + 1): + plan_and_execute_pose_stamped( + robot, + arm, + lin_params, + cfg, + f"press_only_press_{index}_down_z_only", + pose_stamped_from_pose(cfg, press_contact_pose), + logger, + ) + plan_and_execute_pose_stamped( + robot, + arm, + lin_params, + cfg, + f"press_only_press_{index}_up_z_only", + pose_stamped_from_pose(cfg, press_ready_pose), + logger, + ) + logger.info("DONE: measured dispenser press-only cycle completed by MoveItPy robot.execute().") + return + # 1. 컵을 디스펜서 앞에 갖다 놓기: measured outlet pose. cup_place = list(outlet.outlet_xyz_m) if cfg.cup_place_z is not None: diff --git a/src/azas_motion/azas_motion/m0609_shake_joint_state_node.py b/src/azas_motion/azas_motion/m0609_shake_joint_state_node.py index 02fc17d..4e8c245 100644 --- a/src/azas_motion/azas_motion/m0609_shake_joint_state_node.py +++ b/src/azas_motion/azas_motion/m0609_shake_joint_state_node.py @@ -38,7 +38,11 @@ def publish_joint_state(self) -> None: home.append(0.0) mode = str(self.get_parameter("preview_mode").value).strip().lower() - if mode in {"cup_target_move", "side_grasp_target_move", "target_move"}: + if mode in {"color_scan_pose_move", "color_scan_move", "camera_view_move"}: + positions = self.color_scan_pose_move_joints(elapsed, home) + elif mode in {"static_pose", "color_scan_pose", "color_scan", "camera_view_pose"}: + positions = home[:6] + elif mode in {"cup_target_move", "side_grasp_target_move", "target_move"}: positions = self.cup_target_move_joints(elapsed, home) elif mode in {"side_grasp_move_then_shake", "side_grasp_then_shake", "move_then_shake"}: positions = self.side_grasp_move_then_shake_joints(elapsed, home) @@ -72,6 +76,25 @@ def high_shake_joints(self, elapsed: float, home: list[float]) -> list[float]: home[5] + math.radians(12.0) * wrist_yaw, ] + def color_scan_pose_move_joints(self, elapsed: float, target: list[float]) -> list[float]: + start = [ + 0.0, + 0.0, + math.radians(90.0), + 0.0, + math.radians(90.0), + 0.0, + ] + cycle_seconds = 10.0 + t = elapsed % cycle_seconds + if t < 4.0: + ratio = self.minimum_jerk(t / 4.0) + return [a + (b - a) * ratio for a, b in zip(start, target[:6])] + if t < 7.0: + return target[:6] + ratio = self.minimum_jerk((t - 7.0) / 3.0) + return [a + (b - a) * ratio for a, b in zip(target[:6], start)] + def side_grasp_move_then_shake_joints(self, elapsed: float, home: list[float]) -> list[float]: # Joint-space storyboard for RViz visibility only. It follows the # dispenser task shape without publishing Path/markers as the primary diff --git a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py index 856d01d..87ad676 100644 --- a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py +++ b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py @@ -137,6 +137,7 @@ def __init__(self) -> None: self.declare_parameter("remove_legacy_collision_objects", True) self.declare_parameter("remove_course_workspace_collision_objects", False) self.declare_parameter("clear_markers_before_publish", True) + self.declare_parameter("collision_object_exclude_ids", "") config_path = Path( self.get_parameter("config_path").get_parameter_value().string_value @@ -185,6 +186,16 @@ def __init__(self) -> None: .get_parameter_value() .bool_value ) + exclude_raw = ( + self.get_parameter("collision_object_exclude_ids") + .get_parameter_value() + .string_value + ) + self.collision_object_exclude_ids = { + item.strip() + for item in exclude_raw.replace(";", ",").split(",") + if item.strip() + } self._warn_about_draft_status() self._warn_about_front_hold_overlaps() @@ -248,24 +259,31 @@ def _collision_objects(self) -> dict[str, dict[str, Any]]: def _publish_scene(self) -> None: collision_objects = self._collision_objects() + if ( + self.remove_legacy_collision_objects + and not self._legacy_collision_objects_removed + ): + remove_ids = list(LEGACY_DISPENSER_COLLISION_OBJECT_IDS) + if self.remove_course_workspace_collision_objects: + remove_ids.extend(COURSE_WORKSPACE_COLLISION_OBJECT_IDS) + for object_id in remove_ids: + self.collision_pub.publish(self._make_remove_collision_object(object_id)) + self._legacy_collision_objects_removed = True + if self.remove_course_workspace_collision_objects: + self.get_logger().info( + "Requested removal of course workspace wall collision objects: " + + ", ".join(COURSE_WORKSPACE_COLLISION_OBJECT_IDS) + ) if self.publish_collision_objects: - if ( - self.remove_legacy_collision_objects - and not self._legacy_collision_objects_removed - ): - remove_ids = list(LEGACY_DISPENSER_COLLISION_OBJECT_IDS) - if self.remove_course_workspace_collision_objects: - remove_ids.extend(COURSE_WORKSPACE_COLLISION_OBJECT_IDS) - for object_id in remove_ids: - self.collision_pub.publish(self._make_remove_collision_object(object_id)) - self._legacy_collision_objects_removed = True - if self.remove_course_workspace_collision_objects: - self.get_logger().info( - "Requested removal of course workspace wall collision objects: " - + ", ".join(COURSE_WORKSPACE_COLLISION_OBJECT_IDS) - ) published_ids = [] for object_id, object_config in collision_objects.items(): + if object_id in self.collision_object_exclude_ids: + if not self._published_ids_logged: + self.get_logger().info( + f"Skipping PlanningScene collision object {object_id}; " + "it remains visible as an RViz marker only." + ) + continue if not object_config.get("publish_to_planning_scene", True): continue self.collision_pub.publish( diff --git a/tools/checks/check_panel_service_discovery_race.py b/tools/checks/check_panel_service_discovery_race.py index 1a0d083..f038b13 100755 --- a/tools/checks/check_panel_service_discovery_race.py +++ b/tools/checks/check_panel_service_discovery_race.py @@ -44,12 +44,30 @@ def main() -> int: print(color_scan_required) return 1 color_scan_order = panel.with_collision_scene_prereq(["color_scan"]) - expected_color_scan_order = ["move_to_color_scan_pose", "start_camera", "color_scan"] + expected_color_scan_order = [ + "connect_robot", + "status_check", + "move_to_color_scan_pose", + "start_camera", + "color_scan", + ] if color_scan_order != expected_color_scan_order: print("[FAIL] color_scan does not auto-run camera pose and RealSense prerequisites") print(color_scan_order) return 1 + rviz_preview = next( + step for step in panel.STEPS if step.key == "rviz_color_scan_pose_preview" + ) + if rviz_preview.real_motion: + print("[FAIL] color scan pose RViz preview is marked as real motion") + return 1 + rviz_preview_command = panel.command_for(rviz_preview, {"service_prefix": "dsr01"}) + if "tools/run/show_color_scan_pose_rviz.sh" not in rviz_preview_command: + print("[FAIL] color scan pose RViz preview is not wired to its runner") + print(rviz_preview_command) + return 1 + custom_command = "echo custom panel command" panel.save_command_override("move_to_color_scan_pose", custom_command) if panel.command_for(color_scan_pose, {"service_prefix": "dsr01"}) != custom_command: diff --git a/tools/run/check_one_click_cocktail_config.sh b/tools/run/check_one_click_cocktail_config.sh new file mode 100755 index 0000000..34ac73a --- /dev/null +++ b/tools/run/check_one_click_cocktail_config.sh @@ -0,0 +1,107 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS:-${DISPENSER_IDS:-1x1}}" +MEASURED_CONFIG="${MEASURED_CONFIG:-${ROOT_DIR}/src/azas_bringup/config/measured_dispenser_collision.yaml}" +CALIBRATION_CONFIG="${CALIBRATION_CONFIG:-${ROOT_DIR}/src/azas_bringup/config/calibration.yaml}" + +python3 - "${RECIPE_DISPENSER_IDS}" "${MEASURED_CONFIG}" "${CALIBRATION_CONFIG}" <<'PY' +from __future__ import annotations + +import math +import sys +from pathlib import Path +from typing import Any + +import yaml + +raw_ids, measured_path_raw, calibration_path_raw = sys.argv[1:4] +measured_path = Path(measured_path_raw) +calibration_path = Path(calibration_path_raw) +allowed = {"1", "2", "3", "4"} + + +def parse_ids(raw: str) -> list[str]: + values: list[str] = [] + for part in raw.replace(";", ",").split(","): + item = part.strip().lower() + if not item: + continue + if "x" in item: + dispenser_id, count_raw = item.split("x", 1) + elif ":" in item: + dispenser_id, count_raw = item.split(":", 1) + else: + dispenser_id, count_raw = item, "1" + dispenser_id = dispenser_id.strip() + try: + count = int(count_raw.strip()) + except ValueError as exc: + raise ValueError(f"invalid count for dispenser {dispenser_id}: {count_raw!r}") from exc + if count < 1: + raise ValueError(f"count must be >= 1 for dispenser {dispenser_id}") + if dispenser_id not in allowed: + raise ValueError(f"unsupported dispenser id {dispenser_id}; allowed: 1,2,3,4") + values.extend([dispenser_id] * count) + if not values: + raise ValueError("at least one dispenser id is required") + return values + + +def require_list(block: dict[str, Any], key: str, count: int, label: str) -> list[float]: + value = block.get(key) + if not isinstance(value, list) or len(value) != count: + raise ValueError(f"{label}.{key} must be a {count}-number list") + try: + numbers = [float(item) for item in value] + except (TypeError, ValueError) as exc: + raise ValueError(f"{label}.{key} must contain only numbers") from exc + if not all(math.isfinite(item) for item in numbers): + raise ValueError(f"{label}.{key} contains non-finite values") + return numbers + +try: + dispenser_ids = parse_ids(raw_ids) + unique_ids = [] + for dispenser_id in dispenser_ids: + if dispenser_id not in unique_ids: + unique_ids.append(dispenser_id) + if not measured_path.is_file(): + raise FileNotFoundError(f"measured dispenser config not found: {measured_path}") + if not calibration_path.is_file(): + raise FileNotFoundError(f"calibration config not found: {calibration_path}") + measured = yaml.safe_load(measured_path.read_text(encoding="utf-8")) or {} + calibration = yaml.safe_load(calibration_path.read_text(encoding="utf-8")) or {} + front_hold_poses = measured.get("front_hold_poses") or {} + outlets = calibration.get("dispenser_outlets") or {} + + for dispenser_id in unique_ids: + front_key = f"dispenser_{dispenser_id}" + front = front_hold_poses.get(front_key) + if not isinstance(front, dict): + raise ValueError(f"front_hold_poses.{front_key} missing in {measured_path}") + require_list(front, "position_xyz_m", 3, f"front_hold_poses.{front_key}") + require_list(front, "quaternion_xyzw", 4, f"front_hold_poses.{front_key}") + + outlet = outlets.get(dispenser_id) + if not isinstance(outlet, dict): + raise ValueError(f"dispenser_outlets.{dispenser_id} missing in {calibration_path}") + require_list(outlet, "press_pose_xyz_m", 3, f"dispenser_outlets.{dispenser_id}") + require_list(outlet, "press_pose_rpy_deg", 3, f"dispenser_outlets.{dispenser_id}") + require_list(outlet, "press_contact_joints_deg", 6, f"dispenser_outlets.{dispenser_id}") + + grouped: list[tuple[str, int]] = [] + for dispenser_id in dispenser_ids: + if grouped and grouped[-1][0] == dispenser_id: + grouped[-1] = (grouped[-1][0], grouped[-1][1] + 1) + else: + grouped.append((dispenser_id, 1)) + print("[PASS] one-click cocktail config preflight OK") + print(f"[Azas] dispenser_ids={','.join(dispenser_ids)}") + print("[Azas] grouped_press_counts=" + ",".join(f"{dispenser_id}x{count}" for dispenser_id, count in grouped)) + print(f"[Azas] checked_front_hold_and_press_joints={','.join(unique_ids)}") +except Exception as exc: + print(f"[FAIL] one-click cocktail config preflight failed: {exc}", file=sys.stderr) + raise SystemExit(1) +PY diff --git a/tools/run/check_one_click_cocktail_ready.sh b/tools/run/check_one_click_cocktail_ready.sh new file mode 100755 index 0000000..ac6dd82 --- /dev/null +++ b/tools/run/check_one_click_cocktail_ready.sh @@ -0,0 +1,153 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +SERVICE_PREFIX="${SERVICE_PREFIX:-${ROBOT_NAME:-dsr01}}" +ROBOT_NAME="${ROBOT_NAME:-${SERVICE_PREFIX}}" +ROBOT_HOST="${ROBOT_HOST:-192.168.1.100}" +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS:-${DISPENSER_IDS:-1x1}}" +CHECK_TIMEOUT_SEC="${CHECK_TIMEOUT_SEC:-3}" +ROBOT_PORT="${ROBOT_PORT:-12345}" +TCP_CHECK_SEC="${TCP_CHECK_SEC:-2}" +TCP_HARD_BLOCK="${TCP_HARD_BLOCK:-0}" +STRICT_REAL="${STRICT_REAL:-0}" + +source_ros() { + set +u + source /opt/ros/humble/setup.bash + source /home/ssu/ws_moveit/install/setup.bash 2>/dev/null || true + source /home/ssu/ros2_ws/install/setup.bash 2>/dev/null || true + if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then + source "${ROOT_DIR}/install/setup.bash" 2>/dev/null || true + elif [[ -f "${ROOT_DIR}/install/local_setup.bash" ]]; then + source "${ROOT_DIR}/install/local_setup.bash" 2>/dev/null || true + fi + set -u +} + +has_service() { + ros2 service list 2>/dev/null | grep -qx "$1" +} + +show_service() { + local service="$1" label="$2" + if has_service "${service}"; then + echo "[OK] ${label}: ${service}" + return 0 + fi + echo "[MISSING] ${label}: ${service}" + return 1 +} + +tcp_check_robot_host() { + if [[ "${ROBOT_HOST}" == "127.0.0.1" || "${ROBOT_HOST}" == "localhost" ]]; then + echo "[BLOCKED] ROBOT_HOST=${ROBOT_HOST} is localhost; real one-click requires the real controller IP." + return 2 + fi + if command -v nc >/dev/null 2>&1; then + if timeout "${TCP_CHECK_SEC}s" nc -z "${ROBOT_HOST}" "${ROBOT_PORT}" >/dev/null 2>&1; then + echo "[OK] Doosan TCP reachable: ${ROBOT_HOST}:${ROBOT_PORT}" + return 0 + fi + echo "[WARN] Doosan TCP not reachable now: ${ROBOT_HOST}:${ROBOT_PORT}" + echo "[WARN] If real Doosan services are absent, one-click bringup will likely fail until network/controller is ready." + return 1 + fi + echo "[INFO] nc not installed; skipping Doosan TCP reachability check." + return 0 +} + +virtual_matches() { + pgrep -af 'dsr_bringup2_moveit|run_emulator|DRCF|ros2_control_node' \ + | grep -v "$$" \ + | grep -v 'check_one_click_cocktail_ready.sh' \ + | grep -v 'pgrep -af' \ + | grep -v 'grep -E' \ + | grep -E 'mode:=virtual|run_emulator|DRCF' || true +} + +real_matches() { + pgrep -af 'dsr_bringup2_moveit' \ + | grep -v "$$" \ + | grep -v 'check_one_click_cocktail_ready.sh' \ + | grep -v 'pgrep -af' \ + | grep -v 'grep -E' \ + | grep -E 'mode:=real' || true +} + +source_ros + +echo "[Azas] One-click cocktail readiness" +echo "[Azas] expected robot_name=${ROBOT_NAME} service_prefix=/${SERVICE_PREFIX} robot_host=${ROBOT_HOST}" +echo "[Azas] recipe_dispenser_ids=${RECIPE_DISPENSER_IDS}" + +rc=0 +if RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS}" "${ROOT_DIR}/tools/run/check_one_click_cocktail_config.sh"; then + : +else + rc=1 +fi +vm="$(virtual_matches)" +rm="$(real_matches)" +if [[ -n "${vm}" ]]; then + echo "[BLOCKED] Virtual/emulator Doosan session is active. Stop preview before real motion:" + echo " bash tools/run/stop_cocktail_motion_preview.sh" + echo "--- virtual matches ---" + echo "${vm}" + rc=2 +elif [[ -n "${rm}" ]]; then + echo "[OK] Real Doosan launch process detected." + echo "${rm}" +elif [[ "${STRICT_REAL}" == "1" || "${STRICT_REAL}" == "true" ]]; then + echo "[MISSING] No real Doosan launch process detected. one-click script can start it, but STRICT_REAL requested an existing real session." + rc=1 +else + echo "[INFO] No existing real Doosan launch process detected. one-click script will start it if services are absent." +fi + +if ! has_service "/${SERVICE_PREFIX}/motion/move_joint"; then + tcp_rc=0 + tcp_check_robot_host || tcp_rc=$? + if [[ "${tcp_rc}" -eq 2 ]]; then + rc=2 + elif [[ "${tcp_rc}" -ne 0 && "${rc}" -ne 2 ]]; then + if [[ "${TCP_HARD_BLOCK}" == "1" || "${TCP_HARD_BLOCK}" == "true" ]]; then + echo "[BLOCKED] Doosan TCP is required for real one-click startup but is not reachable." + rc=2 + else + rc=1 + fi + fi +fi + +show_service "/${SERVICE_PREFIX}/motion/move_joint" "Doosan move_joint" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/${SERVICE_PREFIX}/motion/move_line" "Doosan move_line" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/${SERVICE_PREFIX}/motion/move_wait" "Doosan move_wait" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/${SERVICE_PREFIX}/motion/fkin" "Doosan fkin" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/${SERVICE_PREFIX}/motion/ikin" "Doosan ikin" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/${SERVICE_PREFIX}/motion/check_motion" "Doosan check_motion" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/${SERVICE_PREFIX}/system/get_robot_state" "Doosan get_robot_state" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/${SERVICE_PREFIX}/aux_control/get_current_posj" "Doosan get_current_posj" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/${SERVICE_PREFIX}/aux_control/get_current_posx" "Doosan get_current_posx" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/jarvis/rg2/set_width" "RG2 set_width" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/jarvis/rg2/open" "RG2 open" || { [[ "${rc}" -eq 2 ]] || rc=1; } +show_service "/jarvis/rg2/close" "RG2 close" || { [[ "${rc}" -eq 2 ]] || rc=1; } + +if has_service "/${SERVICE_PREFIX}/aux_control/get_current_posj"; then + echo "[Azas] Sampling current joints..." + if timeout "${CHECK_TIMEOUT_SEC}s" ros2 service call "/${SERVICE_PREFIX}/aux_control/get_current_posj" dsr_msgs2/srv/GetCurrentPosj "{}" 2>&1 | sed -n '1,12p'; then + : + else + echo "[WARN] get_current_posj sample failed or timed out." + rc=1 + fi +fi + +if [[ "${rc}" -eq 0 ]]; then + echo "[PASS] one-click cocktail stack is ready to run now." +elif [[ "${rc}" -eq 2 ]]; then + echo "[FAIL] hard real-motion block is active; see [BLOCKED] lines above." +else + echo "[WARN] not fully ready yet; run_one_click_cocktail_real.sh can start missing robot/gripper nodes when confirmed." +fi +exit "${rc}" diff --git a/tools/run/check_one_click_cocktail_result.sh b/tools/run/check_one_click_cocktail_result.sh new file mode 100755 index 0000000..030d95c --- /dev/null +++ b/tools/run/check_one_click_cocktail_result.sh @@ -0,0 +1,92 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" +INTEGRATED_LOG="${INTEGRATED_LOG:-${LOG_DIR}/one_click_real_integrated_recipe.log}" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +SAMPLE_CURRENT_POSE="${SAMPLE_CURRENT_POSE:-1}" + +source_ros() { + set +u + source /opt/ros/humble/setup.bash 2>/dev/null || true + source /home/ssu/ws_moveit/install/setup.bash 2>/dev/null || true + source /home/ssu/ros2_ws/install/setup.bash 2>/dev/null || true + if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then + source "${ROOT_DIR}/install/setup.bash" 2>/dev/null || true + elif [[ -f "${ROOT_DIR}/install/local_setup.bash" ]]; then + source "${ROOT_DIR}/install/local_setup.bash" 2>/dev/null || true + fi + set -u +} + +if [[ "${1:-}" == "--help" || "${1:-}" == "-h" ]]; then + cat <&2 + exit 1 +fi + +rc=0 +if grep -q '\[PASS\] measured dispenser recipe sequence completed' "${INTEGRATED_LOG}"; then + echo "[PASS] integrated measured dispenser recipe sequence completed" +else + echo "[FAIL] PASS marker missing from integrated log" >&2 + rc=1 +fi + +for needle in \ + 'RG2 full-open release complete; continuing only after open settle wait' \ + 'RG2 close empty gripper for dispenser press' \ + 'move to measured press contact joints exactly' \ + 'press dispenser pump' \ + 'RG2 soft side-grasp' \ + 'post-grasp lift'; do + if grep -q "${needle}" "${INTEGRATED_LOG}"; then + echo "[OK] found: ${needle}" + else + echo "[WARN] not found: ${needle}" + rc=1 + fi +done + +if grep -q '\[FAIL\]\|\[BLOCKED\]\|target verification timeout\|joint target verification timeout\|response timeout' "${INTEGRATED_LOG}"; then + echo "[FAIL] failure/blocking marker detected in integrated log" >&2 + grep -n '\[FAIL\]\|\[BLOCKED\]\|target verification timeout\|joint target verification timeout\|response timeout' "${INTEGRATED_LOG}" | tail -20 >&2 || true + rc=1 +fi + +if [[ "${SAMPLE_CURRENT_POSE}" == "1" || "${SAMPLE_CURRENT_POSE}" == "true" ]]; then + source_ros + if ros2 service list 2>/dev/null | grep -qx "/${SERVICE_PREFIX}/aux_control/get_current_posj"; then + echo "--- current_posj sample ---" + python3 "${ROOT_DIR}/tools/run/ros_call_empty_service.py" "/${SERVICE_PREFIX}/aux_control/get_current_posj" dsr_msgs2/srv/GetCurrentPosj --timeout 5.0 || true + fi + if ros2 service list 2>/dev/null | grep -qx "/${SERVICE_PREFIX}/aux_control/get_current_posx"; then + echo "--- current_posx sample ---" + python3 "${ROOT_DIR}/tools/run/ros_call_empty_service.py" "/${SERVICE_PREFIX}/aux_control/get_current_posx" dsr_msgs2/srv/GetCurrentPosx --timeout 5.0 || true + fi +fi + +if [[ "${rc}" -eq 0 ]]; then + echo "[PASS] one-click cocktail result log satisfies the expected cup-place -> press -> re-grasp evidence." +else + echo "[WARN] one-click cocktail result is not fully proven by the log. See tail below." + echo "--- integrated tail ---" + tail -80 "${INTEGRATED_LOG}" || true +fi +exit "${rc}" diff --git a/tools/run/remove_moveit_collision_objects.py b/tools/run/remove_moveit_collision_objects.py new file mode 100755 index 0000000..921b7d7 --- /dev/null +++ b/tools/run/remove_moveit_collision_objects.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import sys + +import rclpy +from moveit_msgs.msg import CollisionObject, PlanningScene +from moveit_msgs.srv import ApplyPlanningScene + +DEFAULT_IDS = [ + "dispenser_body_box", + "dispenser_1_body_box_v2", + "dispenser_2_body_box_v2", + "dispenser_3_body_box_v2", + "dispenser_4_body_box_v2", + "dispenser_head_box", + "dispenser_head_nozzle_merged_vertical_box", + "dispenser_head_nozzle_merged_horizontal_spout_box", + "dispenser_1_head_nozzle_box", + "dispenser_2_head_nozzle_box", + "dispenser_3_head_nozzle_box", + "dispenser_4_head_nozzle_box", + "side_grip_workspace_x_min_wall", + "side_grip_workspace_x_max_wall", + "side_grip_workspace_y_min_wall", + "side_grip_workspace_y_max_wall", +] + + +def main() -> int: + parser = argparse.ArgumentParser(description="Remove stale collision objects from MoveIt planning scene.") + parser.add_argument("--service", default="/apply_planning_scene") + parser.add_argument("--frame-id", default="base_link") + parser.add_argument("--ids", default=",".join(DEFAULT_IDS)) + parser.add_argument("--timeout-sec", type=float, default=5.0) + args = parser.parse_args() + + object_ids = [item.strip() for item in args.ids.split(",") if item.strip()] + rclpy.init() + node = rclpy.create_node("azas_remove_moveit_collision_objects") + try: + client = node.create_client(ApplyPlanningScene, args.service) + if not client.wait_for_service(timeout_sec=args.timeout_sec): + node.get_logger().error(f"service not available: {args.service}") + return 1 + scene = PlanningScene() + scene.is_diff = True + for object_id in object_ids: + obj = CollisionObject() + obj.id = object_id + obj.header.frame_id = args.frame_id + obj.operation = CollisionObject.REMOVE + scene.world.collision_objects.append(obj) + request = ApplyPlanningScene.Request() + request.scene = scene + future = client.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=args.timeout_sec) + if not future.done() or future.result() is None: + node.get_logger().error("ApplyPlanningScene timed out") + return 2 + if not future.result().success: + node.get_logger().error("ApplyPlanningScene returned success=false") + return 3 + node.get_logger().info("Removed collision objects: " + ", ".join(object_ids)) + return 0 + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/tools/run/report_cocktail_now_status.sh b/tools/run/report_cocktail_now_status.sh new file mode 100755 index 0000000..caa68f8 --- /dev/null +++ b/tools/run/report_cocktail_now_status.sh @@ -0,0 +1,70 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +TAIL_LINES="${TAIL_LINES:-80}" +SAMPLE_CURRENT_POSE="${SAMPLE_CURRENT_POSE:-1}" + +print_file_tail() { + local label="$1" path="$2" + echo "--- ${label}: ${path} ---" + if [[ -f "${path}" ]]; then + tail -n "${TAIL_LINES}" "${path}" || true + else + echo "[missing] ${path}" + fi +} + +diagnose_doosan_log() { + local path="$1" + echo "--- diagnosis ---" + if [[ ! -f "${path}" ]]; then + echo "[INFO] Doosan log is missing; real one-click has not started Doosan in this log dir." + return 0 + fi + if grep -qE 'Timeout: connect timed out|Connect Failed Please check network state|DRCF connecting ERROR' "${path}"; then + echo "[FAIL] Doosan controller connection failed before motion services became usable." + echo "[CAUSE] ROS tried to connect to the configured ROBOT_HOST:12345 but timed out." + echo "[CHECK] Verify robot controller IP, Ethernet route, pendant state, and that no virtual preview is running." + echo "[NEXT] bash tools/run/stop_cocktail_motion_preview.sh" + echo "[NEXT] RECIPE_DISPENSER_IDS=1x1 bash tools/run/check_one_click_cocktail_ready.sh || true" + return 0 + fi + if grep -qE 'Wrong state or command interface configuration|missing state interfaces|missing command interfaces' "${path}"; then + echo "[FAIL] Doosan ros2_control failed to initialize hardware interfaces." + echo "[CAUSE] This usually follows a controller connection failure or an aborted/stale bringup." + echo "[NEXT] Stop stale Doosan processes, verify controller network, then rerun real NOW." + return 0 + fi + if grep -qE 'mode:=virtual|run_emulator|DRCF' "${path}"; then + echo "[WARN] Doosan log contains virtual/emulator markers. Real one-click must not use virtual motion services." + echo "[NEXT] bash tools/run/stop_cocktail_motion_preview.sh" + return 0 + fi + echo "[INFO] No common Doosan failure pattern detected in the displayed log." +} + +echo "[Azas] Cocktail NOW status report" +echo "[Azas] log_dir=${LOG_DIR} service_prefix=${SERVICE_PREFIX}" + +echo "--- process snapshot ---" +pgrep -af 'run_cocktail_now_real|run_one_click_cocktail_real|run_measured_dispenser_recipe_sequence|dsr_bringup2_moveit|run_emulator|DRCF|ros2_control_node|rg2_gripper_node|measured_dispenser_collision_scene_node|tumbler_collision_scene_node' \ + | grep -v "$$" \ + | grep -v 'pgrep -af' || true + +print_file_tail "integrated recipe" "${LOG_DIR}/one_click_real_integrated_recipe.log" +print_file_tail "doosan" "${LOG_DIR}/one_click_real_doosan.log" +diagnose_doosan_log "${LOG_DIR}/one_click_real_doosan.log" +print_file_tail "gripper" "${LOG_DIR}/one_click_real_gripper.log" +print_file_tail "collision" "${LOG_DIR}/one_click_real_collision_scene.log" + +if [[ -f "${LOG_DIR}/one_click_real_integrated_recipe.log" ]]; then + SAMPLE_CURRENT_POSE="${SAMPLE_CURRENT_POSE}" \ + SERVICE_PREFIX="${SERVICE_PREFIX}" \ + INTEGRATED_LOG="${LOG_DIR}/one_click_real_integrated_recipe.log" \ + bash "${ROOT_DIR}/tools/run/check_one_click_cocktail_result.sh" || true +else + echo "[Azas] Result checker skipped: integrated log missing." +fi diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 619e33a..9541f39 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -3,6 +3,7 @@ from __future__ import annotations +import errno import json import os import re @@ -46,6 +47,7 @@ f"export PYTHONPATH={shlex.quote(str(ROOT / 'tools' / 'run' / 'python_compat'))}:${{PYTHONPATH:-}}" ) DEFAULT_ROBOT_HOST = "192.168.1.100" +DEFAULT_RT_HOST = "0.0.0.0" DEFAULT_ROS_DOMAIN_ID = "9" DEFAULT_YOLO_MODEL_PATH = ROOT / "local_models" / "best.pt" PR20_YOLO_MODEL_PATH = DEFAULT_YOLO_MODEL_PATH @@ -86,6 +88,29 @@ "4": "blue", } DISPENSER_COLOR_MAP_PATH = ROOT / "outputs" / "dispenser_color_map.json" +DISPENSER_COLOR_MAP_FAILED_PATH = ROOT / "outputs" / "dispenser_color_map.json.failed" +LATEST_RECIPE_PATH = ROOT / "outputs" / "latest_recipe.json" + + +def measured_color_scan_joints() -> dict[str, str]: + """Return operator-measured color-scan joints from calibration.yaml. + + Falls back to the legacy camera-view joints only if the measured config is + unavailable, so the panel remains usable while still preferring calibration. + """ + + joints = dict(CAMERA_TABLE_VIEW_JOINTS) + if yaml is None or not CALIBRATION_CONFIG_PATH.exists(): + return joints + try: + data = yaml.safe_load(CALIBRATION_CONFIG_PATH.read_text(encoding="utf-8")) or {} + values = data.get("color_scan_pose", {}).get("joints_deg") + if not isinstance(values, list) or len(values) != 6: + return joints + parsed = [float(value) for value in values] + except Exception: + return joints + return {f"j{index + 1}": f"{value:.6g}" for index, value in enumerate(parsed)} def load_command_overrides() -> dict[str, str]: @@ -138,6 +163,155 @@ def _load_dispenser_press_targets() -> dict[str, str]: DISPENSER_PRESS_TARGETS: dict[str, str] = _load_dispenser_press_targets() +def _read_json_file(path: Path) -> Any: + return json.loads(path.read_text(encoding="utf-8")) + + +def _normalize_color_map(raw: Any) -> dict[str, str]: + if not isinstance(raw, dict): + raise ValueError("color map must be a JSON object") + normalized = {str(key): str(value).lower().strip() for key, value in raw.items()} + return {key: normalized.get(key, "") for key in ("1", "2", "3", "4")} + + +def _compact_dispenser_sequence(sequence: list[str]) -> str: + groups: list[str] = [] + index = 0 + while index < len(sequence): + dispenser_id = sequence[index] + count = 1 + index += 1 + while index < len(sequence) and sequence[index] == dispenser_id: + count += 1 + index += 1 + groups.append(f"{dispenser_id}x{count}") + return ",".join(groups) + + +def dispenser_color_map_status() -> dict[str, Any]: + """Read outputs/dispenser_color_map.json and derive physical dispenser order. + + If the color scan result is missing or unusable, fall back to a conservative + physical dispenser sweep (1,2,3,4 once each) per operator request. The + `.failed` file is still reported so the operator can see why fallback was + selected. + """ + + issues: list[str] = [] + failed_map: dict[str, str] | None = None + if DISPENSER_COLOR_MAP_FAILED_PATH.exists(): + try: + failed_map = _normalize_color_map(_read_json_file(DISPENSER_COLOR_MAP_FAILED_PATH)) + except Exception as exc: + issues.append(f"failed-file read error: {exc}") + + if not DISPENSER_COLOR_MAP_PATH.exists(): + issues.append(f"missing color map: {DISPENSER_COLOR_MAP_PATH}") + if failed_map and all(value == "unknown" for value in failed_map.values()): + issues.append(f"failed map is all unknown: {DISPENSER_COLOR_MAP_FAILED_PATH}") + fallback_sequence = ["1", "2", "3", "4"] + return { + "ok": True, + "fallback": True, + "fallback_reason": "; ".join(issues), + "map": None, + "failed_map": failed_map, + "recipe": None, + "sequence": fallback_sequence, + "sequence_csv": ",".join(fallback_sequence), + "sequence_compact": _compact_dispenser_sequence(fallback_sequence), + "source": str(DISPENSER_COLOR_MAP_PATH), + "failed_source": str(DISPENSER_COLOR_MAP_FAILED_PATH), + "issues": issues, + } + + try: + color_map = _normalize_color_map(_read_json_file(DISPENSER_COLOR_MAP_PATH)) + except Exception as exc: + issues.append(f"color map read error: {exc}") + fallback_sequence = ["1", "2", "3", "4"] + return { + "ok": True, + "fallback": True, + "fallback_reason": "; ".join(issues), + "map": None, + "failed_map": failed_map, + "recipe": None, + "sequence": fallback_sequence, + "sequence_csv": ",".join(fallback_sequence), + "sequence_compact": _compact_dispenser_sequence(fallback_sequence), + "source": str(DISPENSER_COLOR_MAP_PATH), + "failed_source": str(DISPENSER_COLOR_MAP_FAILED_PATH), + "issues": issues, + } + + unknown_ids = [did for did, color in color_map.items() if not color or color == "unknown"] + if unknown_ids: + issues.append(f"unknown dispenser colors: {','.join(unknown_ids)}") + + if not LATEST_RECIPE_PATH.exists(): + issues.append(f"missing recipe: {LATEST_RECIPE_PATH}") + recipe = None + else: + try: + recipe = _read_json_file(LATEST_RECIPE_PATH) + except Exception as exc: + recipe = None + issues.append(f"recipe read error: {exc}") + + color_to_id: dict[str, str] = {} + for dispenser_id, color in color_map.items(): + if not color or color == "unknown": + continue + if color in color_to_id: + issues.append(f"duplicate color mapping: {color}") + color_to_id[color] = dispenser_id + + sequence: list[str] = [] + if isinstance(recipe, dict): + colors = recipe.get("colors") or [] + pumps = recipe.get("pumps") or {} + if not isinstance(colors, list) or not colors: + issues.append("recipe.colors is empty or invalid") + if not isinstance(pumps, dict): + pumps = {} + for raw_color in colors if isinstance(colors, list) else []: + color = str(raw_color).lower().strip() + dispenser_id = color_to_id.get(color) + if not dispenser_id: + issues.append(f"recipe color has no dispenser: {color}") + continue + try: + count = int(pumps.get(color, 1)) + except (TypeError, ValueError): + issues.append(f"invalid pump count for color: {color}") + continue + if count < 1: + issues.append(f"pump count must be >=1 for color: {color}") + continue + sequence.extend([dispenser_id] * count) + + if not sequence: + issues.append("no executable dispenser sequence derived; using fallback 1,2,3,4") + sequence = ["1", "2", "3", "4"] + + return { + "ok": True, + "fallback": bool(issues), + "fallback_reason": "; ".join(issues) if issues else "", + "map": color_map, + "failed_map": failed_map, + "recipe": recipe, + "sequence": sequence, + "sequence_csv": ",".join(sequence), + "sequence_compact": _compact_dispenser_sequence(sequence), + "source": str(DISPENSER_COLOR_MAP_PATH), + "failed_source": str(DISPENSER_COLOR_MAP_FAILED_PATH), + "recipe_source": str(LATEST_RECIPE_PATH), + "issues": issues, + } + + def _number_list(value: Any, *, length: int, label: str) -> list[float]: if not isinstance(value, list) or len(value) < length: raise ValueError(f"{label} must be a list with at least {length} numeric values") @@ -208,6 +382,60 @@ class Step: False, "디스펜서 박스와 감지 텀블러를 /collision_object로 publish; direct Doosan 명령은 아직 이 장면을 자동 회피에 쓰지 않음", ), + Step( + "rviz_cocktail_collision_preview", + "RViz 칵테일 전체 동작 미리보기 / 충돌영역 반영", + "run", + "tools/run/run_cocktail_collision_rviz_preview.sh", + True, + False, + "가상 Doosan+MoveIt RViz에서 컵 놓기→프레스→다시 잡기 전체 코스를 충돌 오브젝트 포함으로 검증. 실로봇 명령은 보내지 않음", + ), + Step( + "stop_cocktail_motion_preview", + "RViz/가상 칵테일 preview 정리", + "run", + "tools/run/stop_cocktail_motion_preview.sh", + True, + False, + "실제 로봇 실행 전에 virtual/emulator/RViz preview 세션을 정리해 real 서비스와 섞이지 않게 함", + ), + Step( + "check_one_click_cocktail_ready", + "실제 통합 칵테일 실행 readiness 확인", + "run", + "tools/run/check_one_click_cocktail_ready.sh", + True, + False, + "real/virtual 세션, Doosan motion 서비스, RG2 서비스를 확인하고 현재 one-click 실행 가능 상태를 출력", + ), + Step( + "check_one_click_cocktail_result", + "실제 통합 칵테일 결과 로그 확인", + "run", + "tools/run/check_one_click_cocktail_result.sh", + True, + False, + "one-click 실제 실행 로그에서 컵놓기→프레스→다시잡기 완료 증거와 실패 marker를 판정", + ), + Step( + "run_one_click_cocktail_real", + "실제 통합 칵테일 one-click 실행", + "run", + "tools/run/run_one_click_cocktail_real.sh", + True, + True, + "실제 로봇 연결/그리퍼/충돌장면 준비 후 컵놓기→프레스→다시잡기 통합 사이클을 한 번에 실행", + ), + Step( + "run_cocktail_now_real", + "실제 칵테일 NOW 실행", + "run", + "tools/run/run_cocktail_now_real.sh", + True, + True, + "preview 정리, readiness/config 검증, 실제 로봇 연결, 컵놓기→프레스→다시잡기와 결과 판정을 한 진입점으로 실행", + ), Step("home_robot", "로봇 원위치 / HOME", "run", "tools/run/direct_movej_joints.py --j1 0 --j2 0 --j3 90 --j4 0 --j5 90 --j6 0", True, True, "실제모션 후보: HOME 관절값 [0, 0, 90, 0, 90, 0]"), Step( "lift_robot", @@ -227,6 +455,15 @@ class Step: True, "색상 스캔 전 기본 카메라 보기 포즈로 이동. color_scan_pose: [0,10,32,0,100,90]°", ), + Step( + "rviz_color_scan_pose_preview", + "색상 스캔 자세 RViz 미리보기 / 무모션", + "background", + "tools/run/show_color_scan_pose_rviz.sh", + True, + False, + "RViz-only /joint_states로 color_scan_pose [0,10,32,0,100,90]°를 표시. 실제 로봇 명령 없음", + ), Step( "color_scan", "디스펜서 색상 스캔", @@ -423,6 +660,8 @@ class Step: "run_rule_based_shake_real.sh", "run_cup_target_then_shake_rviz.sh", "cup_target_then_shake_rviz.launch.py", + "show_color_scan_pose_rviz.sh", + "color_scan_pose_rviz.launch.py", "run_rule_based_dispenser_then_shake_sim.sh", "tumbler_shake_sequence.launch.py", "tumbler_shake_sequence_node", @@ -833,6 +1072,25 @@ def ros_service_names(timeout_sec: float = 6.0) -> tuple[set[str], str]: return {line.strip() for line in output.splitlines() if line.strip().startswith("/")}, output +def ros_node_names(timeout_sec: float = 4.0) -> tuple[set[str], str]: + rc, output = ros2_call("ros2 node list --no-daemon", timeout_sec=timeout_sec) + if rc != 0: + return set(), output + return {line.strip() for line in output.splitlines() if line.strip().startswith("/")}, output + + +def doosan_virtual_nodes_present(service_prefix: str, timeout_sec: float = 4.0) -> tuple[bool, str]: + clean = service_prefix.strip("/") or "dsr01" + nodes, output = ros_node_names(timeout_sec=timeout_sec) + found = sorted( + node for node in nodes + if node in {f"/{clean}/virtual_node", "/virtual_node"} or node.endswith("/virtual_node") + ) + if found: + return True, "virtual Doosan node(s) detected: " + ", ".join(found) + "\n" + output + return False, output + + # Per-process service cache: once a service is confirmed ready, skip re-checking # for SERVICE_CACHE_TTL seconds. Avoids ~2s `ros2 service list` calls per step. _service_ready_cache: dict[str, float] = {} @@ -1635,6 +1893,7 @@ def target_xyz_for_step(step_key: str) -> list[float] | None: def requires_collision_scene_step(key: str) -> bool: return ( key == "shake_closed_cup" + or key == "run_color_recipe_sequence" or key.startswith("move_to_dispenser_") or key.startswith("pick_from_dispenser_") ) @@ -1642,26 +1901,39 @@ def requires_collision_scene_step(key: str) -> bool: def with_collision_scene_prereq(selected: list[str]) -> list[str]: ordered = list(dict.fromkeys(selected)) - if "side_grip" in ordered: - # PR #20 node also moves to camera-home internally, but the supervised - # panel must make the operator-visible sequence explicit and safe: - # lift the robot first, then start RealSense, then run manual side-grip. - side_index = ordered.index("side_grip") - prerequisites = ["lift_robot", "start_camera"] - for prereq in reversed(prerequisites): - if prereq not in ordered: - ordered.insert(side_index, prereq) - - if "color_scan" in ordered: - color_index = ordered.index("color_scan") - prerequisites = ["move_to_color_scan_pose", "start_camera"] - for prereq in reversed(prerequisites): - if prereq not in ordered: - ordered.insert(color_index, prereq) - - if "run_color_recipe_sequence" in ordered and "connect_gripper" not in ordered: - run_index = ordered.index("run_color_recipe_sequence") - ordered.insert(run_index, "connect_gripper") + + def ensure_before(target: str, prerequisites: list[str]) -> None: + if target not in ordered: + return + for prereq in prerequisites: + if prereq in ordered: + continue + target_index = ordered.index(target) + ordered.insert(target_index, prereq) + + # PR #20 side-grip is the cup acquisition step. Make the real-motion and + # perception prerequisites explicit, but do not move already-queued steps; + # this preserves the operator's full-flow order. + ensure_before( + "side_grip", + ["connect_robot", "status_check", "connect_gripper", "start_camera", "lift_robot"], + ) + + # Color classification must aim the robot at the measured color-scan pose + # before sampling the dispenser image. If the camera is already running from + # side-grip, keep it there; otherwise start it before color_scan. + ensure_before( + "color_scan", + ["connect_robot", "status_check", "move_to_color_scan_pose", "start_camera"], + ) + + # The measured dispenser recipe assumes the cup has already been grasped by + # side_grip or an equivalent operator-verified step. Here we only ensure the + # real-motion services and gripper service are available before the cycle. + ensure_before( + "run_color_recipe_sequence", + ["connect_robot", "status_check", "connect_gripper"], + ) if any(requires_collision_scene_step(key) for key in ordered): ordered = [key for key in ordered if key != "start_collision_scene"] @@ -1681,6 +1953,10 @@ def run_timeout_for_step(step: Step) -> float: return 900.0 if step.key == "run_color_recipe_sequence": return 1200.0 + if step.key == "run_one_click_cocktail_real": + return 1500.0 + if step.key == "rviz_cocktail_collision_preview": + return 1500.0 if step.key == "place_cup_holder": return 240.0 return 180.0 @@ -1722,8 +1998,7 @@ def shell_env(payload: dict[str, Any]) -> dict[str, str]: env["RT_HOST"] = str( payload.get("rt_host") or env.get("RT_HOST") - or infer_rt_host(env["ROBOT_HOST"]) - or "192.168.137.50" + or DEFAULT_RT_HOST ) env["DOOSAN_REAL_MOTION_CONFIRM"] = "ENABLE_DOOSAN_REAL_MOTION_BRINGUP" return env @@ -1751,8 +2026,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe rt_host = str( payload.get("rt_host") or os.environ.get("RT_HOST") - or infer_rt_host(robot_host) - or "" + or DEFAULT_RT_HOST ) return ( f"cd {ROOT} && ROBOT_HOST={shlex.quote(robot_host)} " @@ -1787,6 +2061,64 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "python3 tools/run/run_color_recipe_sequence.py --execute --confirm" f"{direct_ids_arg}" ) + if step.key == "rviz_cocktail_collision_preview": + recipe_dispenser_ids = str(payload.get("recipe_dispenser_ids") or "").strip() + recipe_env = "" + if recipe_dispenser_ids: + recipe_env = f"RECIPE_DISPENSER_IDS={shlex.quote(recipe_dispenser_ids)} " + return ( + f"cd {ROOT} && {ROS_SETUP} && " + f"{recipe_env}DISPENSER_COLLISION_OBJECTS=1 " + "tools/run/run_cocktail_collision_rviz_preview.sh" + ) + if step.key == "stop_cocktail_motion_preview": + return f"cd {ROOT} && tools/run/stop_cocktail_motion_preview.sh" + if step.key == "check_one_click_cocktail_ready": + robot_host = str(payload.get("robot_host") or os.environ.get("ROBOT_HOST") or DEFAULT_ROBOT_HOST) + robot_name = str(payload.get("robot_name") or os.environ.get("ROBOT_NAME") or service_prefix) + recipe_dispenser_ids = str(payload.get("recipe_dispenser_ids") or "").strip() + recipe_env = "" + if recipe_dispenser_ids: + recipe_env = f"RECIPE_DISPENSER_IDS={shlex.quote(recipe_dispenser_ids)} " + return ( + f"cd {ROOT} && {ROS_SETUP} && " + f"{recipe_env}" + f"ROBOT_HOST={shlex.quote(robot_host)} ROBOT_NAME={shlex.quote(robot_name)} SERVICE_PREFIX={shlex.quote(service_prefix)} " + "tools/run/check_one_click_cocktail_ready.sh" + ) + if step.key == "check_one_click_cocktail_result": + return ( + f"cd {ROOT} && {ROS_SETUP} && " + f"SERVICE_PREFIX={shlex.quote(service_prefix)} " + "tools/run/check_one_click_cocktail_result.sh" + ) + if step.key == "run_one_click_cocktail_real": + recipe_dispenser_ids = str(payload.get("recipe_dispenser_ids") or "").strip() + recipe_env = "" + if recipe_dispenser_ids: + recipe_env = f"RECIPE_DISPENSER_IDS={shlex.quote(recipe_dispenser_ids)} " + robot_host = str(payload.get("robot_host") or os.environ.get("ROBOT_HOST") or DEFAULT_ROBOT_HOST) + robot_name = str(payload.get("robot_name") or os.environ.get("ROBOT_NAME") or service_prefix) + return ( + f"cd {ROOT} && {ROS_SETUP} && " + "REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE " + f"{recipe_env}" + f"ROBOT_HOST={shlex.quote(robot_host)} ROBOT_NAME={shlex.quote(robot_name)} SERVICE_PREFIX={shlex.quote(service_prefix)} " + "tools/run/run_one_click_cocktail_real.sh" + ) + if step.key == "run_cocktail_now_real": + recipe_dispenser_ids = str(payload.get("recipe_dispenser_ids") or "").strip() + recipe_arg = "" + if recipe_dispenser_ids: + recipe_arg = f" {shlex.quote(recipe_dispenser_ids)}" + robot_host = str(payload.get("robot_host") or os.environ.get("ROBOT_HOST") or DEFAULT_ROBOT_HOST) + robot_name = str(payload.get("robot_name") or os.environ.get("ROBOT_NAME") or service_prefix) + return ( + f"cd {ROOT} && {ROS_SETUP} && " + "REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE " + f"ROBOT_HOST={shlex.quote(robot_host)} ROBOT_NAME={shlex.quote(robot_name)} SERVICE_PREFIX={shlex.quote(service_prefix)} " + f"tools/run/run_cocktail_now_real.sh{recipe_arg}" + ) if step.key == "lift_robot": joints = { name: str(os.environ.get(f"CAMERA_TABLE_VIEW_{name.upper()}", value)) @@ -1810,10 +2142,13 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "--execute --confirm ENABLE_DIRECT_MOVEJ" ) if step.key == "move_to_color_scan_pose": + joints = measured_color_scan_joints() return ( f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/direct_movej_joints.py " f"--service-prefix {service_prefix} " - "--j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90 " + f"--j1 {shlex.quote(joints['j1'])} --j2 {shlex.quote(joints['j2'])} " + f"--j3 {shlex.quote(joints['j3'])} --j4 {shlex.quote(joints['j4'])} " + f"--j5 {shlex.quote(joints['j5'])} --j6 {shlex.quote(joints['j6'])} " "--velocity 30 --acceleration 30 --timeout-sec 60 " "--execute --confirm ENABLE_DIRECT_MOVEJ" ) @@ -2137,7 +2472,7 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: return {"key": step.key, "status": "blocked", "output": step.note} if step.real_motion and not payload.get("armed"): return {"key": step.key, "status": "blocked", "output": "실제 모션 허용 체크가 꺼져 있습니다."} - if step.real_motion: + if step.real_motion and step.key not in {"run_one_click_cocktail_real", "run_cocktail_now_real"}: service_prefix = str(payload.get("service_prefix") or "dsr01") gripper_ready, gripper_output = ensure_gripper_services(step, payload, service_prefix) if not gripper_ready: @@ -2225,29 +2560,40 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: if step.key == "connect_robot": ready, ready_output, _svc = motion_services_ready(env["SERVICE_PREFIX"]) if ready: - robot_ready, robot_ready_output = doosan_robot_ready(env["SERVICE_PREFIX"]) - if not robot_ready: + virtual_present, virtual_output = doosan_virtual_nodes_present(env["SERVICE_PREFIX"]) + if virtual_present: + cleanup_events = cleanup_doosan_stack(grace_sec=3.0) + restart_output = ( + "기존 Doosan motion 서비스가 보이지만 virtual_node가 감지되어 실제 로봇 연결로 재시작합니다.\n" + f"--- virtual nodes ---\n{virtual_output}\n" + "--- cleanup ---\n" + + ("\n".join(cleanup_events) if cleanup_events else "no cleanup events") + + "\n" + ) + else: + robot_ready, robot_ready_output = doosan_robot_ready(env["SERVICE_PREFIX"]) + if not robot_ready: + return { + "key": step.key, + "status": "blocked", + "output": ( + "Doosan motion 서비스는 보이지만 로봇이 motion-ready 상태가 아닙니다. " + "재시작하지 않습니다.\n" + f"{ready_output}\n" + "티치펜던트/컨트롤러에서 빨간 상태(SAFE_OFF/보호정지/서보 상태)를 해제해 " + "STATE_STANDBY(1)로 만든 뒤 다시 확인하세요.\n" + f"{robot_ready_output}" + ), + } return { "key": step.key, - "status": "blocked", + "status": "running", "output": ( - "Doosan motion 서비스는 보이지만 로봇이 motion-ready 상태가 아닙니다. " + "이미 실제 Doosan motion 서비스가 보이고 로봇이 STATE_STANDBY(1)입니다. " "재시작하지 않습니다.\n" - f"{ready_output}\n" - "티치펜던트/컨트롤러에서 빨간 상태(SAFE_OFF/보호정지/서보 상태)를 해제해 " - "STATE_STANDBY(1)로 만든 뒤 다시 확인하세요.\n" - f"{robot_ready_output}" + f"{ready_output}\n{robot_ready_output}" ), } - return { - "key": step.key, - "status": "running", - "output": ( - "이미 Doosan motion 서비스가 보이고 로봇이 STATE_STANDBY(1)입니다. " - "재시작하지 않습니다.\n" - f"{ready_output}\n{robot_ready_output}" - ), - } old = processes.get(step.key) if old and old.poll() is None: ready, waited_output = wait_for_motion_services_ready( @@ -2278,37 +2624,34 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: "pid": old.pid, } log_tail = tail_file(process_logs.get(step.key)) - return { - "key": step.key, - "status": "starting", - "output": ( - "로봇 연결 프로세스가 이미 시작 중입니다. 반복 재시작하지 않습니다.\n" - "motion 서비스가 아직 없으면 티치펜던트/컨트롤러 상태, 네트워크, RT_HOST를 확인하세요.\n" - "정말 죽였다가 다시 시작하려면 '실행 중지' 후 '로봇 연결 / 스마트 재연결'을 다시 누르세요.\n" - f"pid={old.pid}\n" - f"--- readiness ---\n{waited_output}\n" - f"--- log tail ---\n{log_tail}" - ), - "pid": old.pid, - } + cleanup_events = cleanup_doosan_stack(grace_sec=3.0) + time.sleep(1.5) + restart_output = ( + "기존 로봇 연결 프로세스가 motion 서비스를 준비하지 못해 재연결합니다.\n" + f"pid={old.pid}\n" + f"--- readiness ---\n{waited_output}\n" + f"--- previous log tail ---\n{log_tail}\n" + "--- cleanup ---\n" + + ("\n".join(cleanup_events) if cleanup_events else "no cleanup events") + + "\n" + ) existing_pid, existing_cmd = find_existing_doosan_launch() - if existing_pid is not None: - return { - "key": step.key, - "status": "starting", - "output": ( - "기존 Doosan bringup이 아직 실행/시작 중이라 반복 재시작하지 않습니다.\n" - "motion 서비스가 없으면 로봇 컨트롤러 안전상태/비상정지/보호정지/네트워크/RT_HOST를 먼저 확인하세요.\n" - "정말 중복 노드를 정리하고 다시 시작하려면 '실행 중지' 후 '로봇 연결 / 스마트 재연결'을 다시 누르세요.\n" - f"pid={existing_pid}\ncmd={existing_cmd[:500]}\n" - f"--- readiness ---\n{ready_output}" - ), - "pid": existing_pid, - } - cleanup_events = cleanup_doosan_stack() - # Give DDS/service discovery a short moment to forget killed duplicate nodes. - time.sleep(1.5) - restart_output = "\n".join(cleanup_events) + if existing_pid is not None and not restart_output: + cleanup_events = cleanup_doosan_stack(grace_sec=3.0) + time.sleep(1.5) + restart_output = ( + "기존 Doosan bringup이 있으나 motion 서비스가 준비되지 않아 재연결합니다.\n" + f"pid={existing_pid}\ncmd={existing_cmd[:500]}\n" + f"--- readiness ---\n{ready_output}\n" + "--- cleanup ---\n" + + ("\n".join(cleanup_events) if cleanup_events else "no cleanup events") + + "\n" + ) + if not restart_output: + cleanup_events = cleanup_doosan_stack() + # Give DDS/service discovery a short moment to forget killed duplicate nodes. + time.sleep(1.5) + restart_output = "\n".join(cleanup_events) elif step.key == "connect_gripper": cleanup_events = cleanup_rg2_stack() # DDS may keep stale service names briefly after a killed RG2 wrapper. @@ -2602,7 +2945,7 @@ def do_GET(self) -> None: self.send_json(data) return if path == "/api/dispenser_color_map": - self.send_json({"map": DISPENSER_PRESS_TARGETS}) + self.send_json(dispenser_color_map_status()) return if path == "/api/camera_snapshot.jpg": ok, body, error = camera_snapshot_jpeg() @@ -2678,7 +3021,15 @@ def log_message(self, fmt: str, *args: Any) -> None: def main() -> int: host = os.environ.get("AZAS_PANEL_HOST", "127.0.0.1") port = int(os.environ.get("AZAS_PANEL_PORT", "8765")) - server = ThreadingHTTPServer((host, port), Handler) + try: + server = ThreadingHTTPServer((host, port), Handler) + except OSError as exc: + if exc.errno == errno.EADDRINUSE: + print(f"[Azas] panel port is already in use: http://{host}:{port}", flush=True) + print("[Azas] Open the existing panel, or start a second one with:", flush=True) + print(f" AZAS_PANEL_PORT={port + 1} bash tools/run/run_robot_pipeline_control_panel.sh", flush=True) + return 98 + raise print(f"[Azas] Robot pipeline panel: http://{host}:{port}") print("[Azas] Press Ctrl+C to stop the panel server.") try: diff --git a/tools/run/run_cocktail_collision_rviz_preview.sh b/tools/run/run_cocktail_collision_rviz_preview.sh new file mode 100755 index 0000000..c3b14b8 --- /dev/null +++ b/tools/run/run_cocktail_collision_rviz_preview.sh @@ -0,0 +1,110 @@ +#!/usr/bin/env bash +set -euo pipefail + +# RViz-only collision-aware preview for the measured dispenser course cycle. +# +# This is intentionally a simulation/preview entrypoint: +# - forces virtual Doosan bringup through run_course_dispenser_press_cycle_rviz.sh +# - publishes measured dispenser collision objects into the MoveIt PlanningScene +# - runs the full cup-place -> press -> re-grasp course cycle, not PRESS_ONLY +# +# Input: +# RECIPE_DISPENSER_IDS=1x1,3x2,4x1 # preferred +# or DISPENSER_ID=1 PRESS_COUNT=2 # single fallback + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +COURSE_SCRIPT="${ROOT_DIR}/tools/run/run_course_dispenser_press_cycle_rviz.sh" + +parse_sequence() { + local raw="$1" + local normalized part item did count + normalized="${raw//;/,}" + IFS=',' read -r -a parts <<<"${normalized}" || true + for part in "${parts[@]}"; do + item="$(echo "${part}" | tr '[:upper:]' '[:lower:]' | xargs)" + [[ -z "${item}" ]] && continue + if [[ "${item}" == *x* ]]; then + did="${item%%x*}" + count="${item#*x}" + elif [[ "${item}" == *:* ]]; then + did="${item%%:*}" + count="${item#*:}" + else + did="${item}" + count="1" + fi + did="$(echo "${did}" | xargs)" + count="$(echo "${count}" | xargs)" + if [[ ! "${did}" =~ ^[1-4]$ || ! "${count}" =~ ^[0-9]+$ || "${count}" -lt 1 ]]; then + echo "invalid dispenser sequence item: ${item}" >&2 + return 2 + fi + printf '%s %s\n' "${did}" "${count}" + done +} + +RAW_SEQUENCE="${RECIPE_DISPENSER_IDS:-}" +if [[ -z "${RAW_SEQUENCE}" ]]; then + RAW_SEQUENCE="${DISPENSER_ID:-1}x${PRESS_COUNT:-1}" +fi + +mapfile -t SEQUENCE_GROUPS < <(parse_sequence "${RAW_SEQUENCE}") || true +if [[ "${#SEQUENCE_GROUPS[@]}" -lt 1 ]]; then + echo "[Azas] No dispenser sequence to preview. Set RECIPE_DISPENSER_IDS=1x1,3x2." >&2 + exit 2 +fi + +echo "[Azas] RViz cocktail collision preview sequence: ${RAW_SEQUENCE}" +echo "[Azas] Full cycle mode: cup-place -> press -> re-grasp, collision objects enabled." + +first=1 +last_index=$(("${#SEQUENCE_GROUPS[@]}" - 1)) +for index in "${!SEQUENCE_GROUPS[@]}"; do + group="${SEQUENCE_GROUPS[$index]}" + read -r did count <<<"${group}" + if [[ "${first}" == "1" ]]; then + start_doosan="${START_DOOSAN:-auto}" + first=0 + else + # Each course-script invocation owns and cleans up its bringup unless it is + # kept alive at the end, so later groups must be allowed to auto-start or + # reuse the virtual session instead of assuming the first one still exists. + start_doosan="${START_DOOSAN:-auto}" + fi + if [[ "${index}" -eq "${last_index}" ]]; then + keep_after="${KEEP_ALIVE_AFTER_DONE:-0}" + preserve_after=0 + else + # Do not block between groups; keep RViz/virtual bringup alive only after + # the final group so a full recipe such as 1x1,3x2,4x1 can actually play. + keep_after=0 + preserve_after=1 + fi + if [[ "${index}" -eq 0 ]]; then + reset_existing="${RESET_EXISTING_VIRTUAL_PREVIEW:-1}" + replace_rviz="${REPLACE_EXISTING_RVIZ:-1}" + else + # Reuse the virtual Doosan/RViz session preserved by the previous group. + # Resetting/replacing here makes the orange robot disappear between steps. + reset_existing=0 + replace_rviz=0 + fi + echo "[Azas] Preview dispenser ${did} x${count}" + RVIZ_ONLY=1 \ + PRESS_ONLY=0 \ + DISPENSER_COLLISION_ENABLED=1 \ + DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS:-1}" \ + REMOVE_COURSE_WORKSPACE_WALLS="${REMOVE_COURSE_WORKSPACE_WALLS:-1}" \ + DISPENSER_ID="${did}" \ + PRESS_COUNT="${count}" \ + START_DOOSAN="${start_doosan}" \ + KEEP_ALIVE_AFTER_DONE="${keep_after}" \ + PRESERVE_PREVIEW_SESSION_AFTER_DONE="${preserve_after}" \ + KEEP_RVIZ_ON_FAIL="${KEEP_RVIZ_ON_FAIL:-1}" \ + RVIZ_MODE="${RVIZ_MODE:-clean}" \ + REPLACE_EXISTING_RVIZ="${replace_rviz}" \ + RESET_EXISTING_VIRTUAL_PREVIEW="${reset_existing}" \ + bash "${COURSE_SCRIPT}" +done + +echo "[Azas] RViz cocktail collision preview completed." diff --git a/tools/run/run_cocktail_now_real.sh b/tools/run/run_cocktail_now_real.sh new file mode 100755 index 0000000..bfeaab8 --- /dev/null +++ b/tools/run/run_cocktail_now_real.sh @@ -0,0 +1,81 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Short final entrypoint for the real integrated cocktail dispenser cycle. +# It intentionally delegates to the guarded one-click script instead of +# duplicating motion logic. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS:-${1:-1x1}}" +ROBOT_HOST="${ROBOT_HOST:-192.168.1.100}" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +ROBOT_NAME="${ROBOT_NAME:-${SERVICE_PREFIX}}" +SKIP_PREVIEW_STOP="${SKIP_PREVIEW_STOP:-0}" +DRY_RUN="${DRY_RUN:-0}" + +if [[ "${1:-}" == "--help" || "${1:-}" == "-h" ]]; then + cat < config/readiness guard -> real Doosan/RG2/collision setup -> + cup-place -> RG2 full-open -> safe lift -> close empty gripper -> measured press pump(s) -> re-grasp/lift -> result check. + +Env: + RECIPE_DISPENSER_IDS=1x2 same as first positional argument + ROBOT_HOST=192.168.1.100 + ROBOT_NAME=dsr01 defaults to SERVICE_PREFIX + SERVICE_PREFIX=dsr01 + SKIP_PREVIEW_STOP=1 do not run preview cleanup first + DRY_RUN=1 print real one-click commands without motion +USAGE + exit 0 +fi + +if [[ "${REAL_COCKTAIL_CONFIRM:-}" != "ENABLE_REAL_COCKTAIL_SEQUENCE" ]]; then + echo "[Azas] Refusing real cocktail-now run without explicit confirmation." >&2 + echo "[Azas] Re-run with: REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE" >&2 + exit 2 +fi + +cd "${ROOT_DIR}" + +echo "[Azas] Cocktail NOW real cycle: ${RECIPE_DISPENSER_IDS}" +echo "[Azas] robot_host=${ROBOT_HOST} robot_name=${ROBOT_NAME} service_prefix=${SERVICE_PREFIX}" + +if [[ "${SKIP_PREVIEW_STOP}" != "1" && "${SKIP_PREVIEW_STOP}" != "true" ]]; then + bash tools/run/stop_cocktail_motion_preview.sh +fi + +set +e +if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + TCP_HARD_BLOCK_FOR_READY="${TCP_HARD_BLOCK:-0}" +else + TCP_HARD_BLOCK_FOR_READY="${TCP_HARD_BLOCK:-1}" +fi +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS}" \ +ROBOT_HOST="${ROBOT_HOST}" \ +ROBOT_NAME="${ROBOT_NAME}" \ +SERVICE_PREFIX="${SERVICE_PREFIX}" \ +TCP_HARD_BLOCK="${TCP_HARD_BLOCK_FOR_READY}" \ +bash tools/run/check_one_click_cocktail_ready.sh +READY_RC=$? +set -e + +if [[ "${READY_RC}" -eq 2 ]]; then + echo "[Azas] Refusing to continue: readiness reported a hard real-motion block." >&2 + exit 2 +fi +if [[ "${READY_RC}" -ne 0 ]]; then + echo "[Azas] Readiness is not fully green yet (rc=${READY_RC}); continuing because one-click can start missing real Doosan/RG2 nodes after its own guards." +fi + +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS}" \ +ROBOT_HOST="${ROBOT_HOST}" \ +ROBOT_NAME="${ROBOT_NAME}" \ +SERVICE_PREFIX="${SERVICE_PREFIX}" \ +DRY_RUN="${DRY_RUN}" \ +bash tools/run/run_one_click_cocktail_real.sh diff --git a/tools/run/run_course_dispenser_press_cycle_rviz.sh b/tools/run/run_course_dispenser_press_cycle_rviz.sh index 040f19d..e6194c9 100755 --- a/tools/run/run_course_dispenser_press_cycle_rviz.sh +++ b/tools/run/run_course_dispenser_press_cycle_rviz.sh @@ -5,27 +5,42 @@ set -euo pipefail # 1) Doosan MoveIt bringup as in 25장 (virtual now, real later by MODE/HOST) # 2) Azas MoveItPy node follows 26~28장: plan() -> robot.execute(blocking=True) # 3) RViz robot motion is controller-backed /joint_states. No fake joint publisher. -# 4) Default RVIZ_MODE=bringup keeps the course/MoveIt RViz, including the orange planned/goal robot display. +# 4) Default RVIZ_MODE=clean replaces noisy MoveIt MotionPlanning RViz with a +# lean RobotModel/marker view so the planned-path ghost robot does not flicker. ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" MODE="${MODE:-virtual}" +ROBOT_NAME="${ROBOT_NAME:-dsr01}" HOST="${HOST:-127.0.0.1}" PORT="${PORT:-12345}" MODEL="${MODEL:-m0609}" COLOR="${COLOR:-white}" RT_HOST="${RT_HOST:-192.168.137.50}" +JOINT_STATES_TOPIC="${JOINT_STATES_TOPIC:-/${ROBOT_NAME}/joint_states}" +MOVEIT_CONTROLLER_ACTION="${MOVEIT_CONTROLLER_ACTION:-/${ROBOT_NAME}/dsr_moveit_controller/follow_joint_trajectory}" +CONTROLLER_SETTLE_SEC="${CONTROLLER_SETTLE_SEC:-5}" +START_DOOSAN="${START_DOOSAN:-auto}" # auto|1|0; auto reuses an existing Doosan/MoveIt session. +RVIZ_ONLY="${RVIZ_ONLY:-0}" # 1 forces virtual/sim bringup so robot.execute cannot command real hardware. START_DELAY_SEC="${START_DELAY_SEC:-22}" JOINT_WAIT_SEC="${JOINT_WAIT_SEC:-60}" DISPENSER_ID="${DISPENSER_ID:-1}" PRESS_COUNT="${PRESS_COUNT:-2}" +PRESS_ONLY="${PRESS_ONLY:-0}" # 1 = measured press joints + Z-only pump only; skips cup place/return IK. RVIZ_MODE="${RVIZ_MODE:-clean}" # bringup|clean|none -RVIZ_CONFIG="${RVIZ_CONFIG:-${ROOT_DIR}/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz}" +KEEP_RVIZ_ON_FAIL="${KEEP_RVIZ_ON_FAIL:-0}" +KEEP_ALIVE_AFTER_DONE="${KEEP_ALIVE_AFTER_DONE:-1}" +PRESERVE_PREVIEW_SESSION_AFTER_DONE="${PRESERVE_PREVIEW_SESSION_AFTER_DONE:-0}" +REPLACE_EXISTING_RVIZ="${REPLACE_EXISTING_RVIZ:-0}" +RESET_EXISTING_VIRTUAL_PREVIEW="${RESET_EXISTING_VIRTUAL_PREVIEW:-0}" +RVIZ_CONFIG="${RVIZ_CONFIG:-${ROOT_DIR}/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz}" +COURSE_RVIZ_CONFIG="${COURSE_RVIZ_CONFIG:-/home/ssu/ros2_ws/install/dsr_moveit_config_m0609/share/dsr_moveit_config_m0609/launch/moveit.rviz}" DISPENSER_COLLISION_ENABLED="${DISPENSER_COLLISION_ENABLED:-1}" # The measured combined box is the glass-bottle/body area, not the press button/head. # Keep markers visible in RViz by default, but do not feed this draft body box into # MoveIt collision checking for the press stroke unless explicitly requested. DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS:-1}" +DISPENSER_COLLISION_EXCLUDE_IDS="${DISPENSER_COLLISION_EXCLUDE_IDS:-dispenser_head_nozzle_merged_horizontal_spout_box}" REMOVE_COURSE_WORKSPACE_WALLS="${REMOVE_COURSE_WORKSPACE_WALLS:-1}" SHOW_LINK6_GRIPPER="${SHOW_LINK6_GRIPPER:-1}" DISPENSER_COLLISION_CONFIG="${DISPENSER_COLLISION_CONFIG:-${ROOT_DIR}/install/azas_bringup/share/azas_bringup/config/measured_dispenser_collision.yaml}" @@ -34,7 +49,23 @@ if [[ ! -f "${DISPENSER_COLLISION_CONFIG}" ]]; then fi mkdir -p "${LOG_DIR}" +if [[ "${RVIZ_ONLY}" == "1" || "${RVIZ_ONLY}" == "true" ]]; then + MODE=virtual + HOST=127.0.0.1 + if [[ "${KEEP_RVIZ_ON_FAIL}" == "0" ]]; then + KEEP_RVIZ_ON_FAIL=1 + fi + echo "[Azas] RVIZ_ONLY=${RVIZ_ONLY}: forcing MODE=virtual HOST=127.0.0.1 START_DOOSAN=${START_DOOSAN}" + echo "[Azas] RVIZ_ONLY=${RVIZ_ONLY}: RVIZ_MODE=${RVIZ_MODE}; KEEP_RVIZ_ON_FAIL=${KEEP_RVIZ_ON_FAIL}" + pkill -f 'workspace_collision_scene_node' 2>/dev/null || true + echo "[Azas] RVIZ_ONLY=${RVIZ_ONLY}: stopped stale workspace_collision_scene_node publishers." +fi + cleanup() { + if [[ "${PRESERVE_PREVIEW_SESSION_AFTER_DONE}" == "1" || "${PRESERVE_PREVIEW_SESSION_AFTER_DONE}" == "true" ]]; then + echo "[Azas] PRESERVE_PREVIEW_SESSION_AFTER_DONE=${PRESERVE_PREVIEW_SESSION_AFTER_DONE}: keeping virtual Doosan/RViz preview session for the next group." + return 0 + fi for pid in "${PIDS[@]:-}"; do if [[ -n "${pid}" ]] && kill -0 "${pid}" 2>/dev/null; then kill "${pid}" 2>/dev/null || true @@ -56,38 +87,73 @@ set -u STRICT_SINGLE_SESSION="${STRICT_SINGLE_SESSION:-1}" +existing="$(pgrep -af 'dsr_bringup2_moveit|move_group|ros2_control_node|run_emulator|DRCF' | grep -v "$$" | grep -v 'pgrep -af' || true)" +if [[ -n "${existing}" && ( "${RVIZ_ONLY}" == "1" || "${RVIZ_ONLY}" == "true" ) && "${RVIZ_MODE}" == "bringup" && ( "${START_DOOSAN}" == "auto" || "${START_DOOSAN}" == "1" || "${START_DOOSAN}" == "true" ) && ( "${RESET_EXISTING_VIRTUAL_PREVIEW}" == "1" || "${RESET_EXISTING_VIRTUAL_PREVIEW}" == "true" ) ]]; then + if echo "${existing}" | grep -qE 'mode:=virtual|run_emulator|DRCF'; then + echo "[Azas] Resetting existing virtual Doosan/MoveIt preview so teaching RViz gets robot_description parameters." + echo "${existing}" + KILL_RVIZ=1 "${ROOT_DIR}/tools/run/stop_cocktail_motion_preview.sh" || true + sleep 3 + existing="$(pgrep -af 'dsr_bringup2_moveit|move_group|ros2_control_node|run_emulator|DRCF' | grep -v "$$" | grep -v 'pgrep -af' || true)" + START_DOOSAN=1 + before_rviz="$(pgrep -x rviz2 || true)" + else + echo "[Azas] Existing Doosan/MoveIt session does not look virtual; refusing to reset it from RVIZ_ONLY preview." >&2 + fi +fi if [[ "${STRICT_SINGLE_SESSION}" == "1" ]]; then - existing="$(pgrep -af 'dsr_bringup2_moveit|move_group|ros2_control_node|run_emulator' | grep -v "$$" || true)" if [[ -n "${existing}" ]]; then - echo '[Azas] Refusing: an existing Doosan/MoveIt session is running. Stop it first to avoid RViz state jumping.' >&2 - echo "${existing}" >&2 - exit 2 + if [[ "${START_DOOSAN}" == "auto" || "${START_DOOSAN}" == "0" || "${START_DOOSAN}" == "false" ]]; then + START_DOOSAN=0 + echo "[Azas] Reusing existing Doosan/MoveIt session; waiting on ${JOINT_STATES_TOPIC}." + echo "${existing}" + else + echo '[Azas] Refusing: an existing Doosan/MoveIt session is running. Stop it first to avoid RViz state jumping.' >&2 + echo "${existing}" >&2 + exit 2 + fi fi fi +if [[ "${START_DOOSAN}" == "auto" ]]; then + START_DOOSAN=1 +fi -if pgrep -af 'm0609_shake_joint_state_node|side_grasp_ik_preview_node' >/dev/null; then +if pgrep -af 'm0609_shake_joint_state_node|side_grasp_ik_preview_node' | grep -v 'pgrep -af' >/dev/null; then echo '[Azas] Refusing: fake RViz joint publisher is still running.' >&2 - pgrep -af 'm0609_shake_joint_state_node|side_grasp_ik_preview_node' >&2 || true + pgrep -af 'm0609_shake_joint_state_node|side_grasp_ik_preview_node' | grep -v 'pgrep -af' >&2 || true exit 1 fi before_rviz="$(pgrep -x rviz2 || true)" -ros2 launch dsr_bringup2 dsr_bringup2_moveit.launch.py \ - mode:="${MODE}" \ - model:="${MODEL}" \ - host:="${HOST}" \ - port:="${PORT}" \ - color:="${COLOR}" \ - rt_host:="${RT_HOST}" \ - >"${LOG_DIR}/course_dispenser_bringup.log" 2>&1 & -PIDS+=("$!") +if [[ "${START_DOOSAN}" == "1" || "${START_DOOSAN}" == "true" ]]; then + ros2 launch dsr_bringup2 dsr_bringup2_moveit.launch.py \ + name:="${ROBOT_NAME}" \ + mode:="${MODE}" \ + model:="${MODEL}" \ + host:="${HOST}" \ + port:="${PORT}" \ + color:="${COLOR}" \ + rt_host:="${RT_HOST}" \ + >"${LOG_DIR}/course_dispenser_bringup.log" 2>&1 & + PIDS+=("$!") -sleep "${START_DELAY_SEC}" + echo "[Azas] Doosan launch: name=${ROBOT_NAME} mode=${MODE} host=${HOST} port=${PORT} rt_host=${RT_HOST}" + sleep "${START_DELAY_SEC}" +else + echo "[Azas] Doosan launch skipped: START_DOOSAN=${START_DOOSAN}" + : >"${LOG_DIR}/course_dispenser_bringup.log" +fi +echo "[Azas] Waiting for controller joint states on ${JOINT_STATES_TOPIC}" +echo "[Azas] MoveIt controller action: ${MOVEIT_CONTROLLER_ACTION}" +if [[ "${PRESS_ONLY}" == "1" || "${PRESS_ONLY}" == "true" ]]; then + echo "[Azas] PRESS_ONLY=${PRESS_ONLY}: RViz will show measured press joints + Z-only pump strokes only." + echo "[Azas] PRESS_ONLY=${PRESS_ONLY}: skipping cup placement/return IK paths so press motion can be judged directly." +fi joint_deadline=$((SECONDS + JOINT_WAIT_SEC)) while (( SECONDS < joint_deadline )); do - if timeout 3 ros2 topic echo /joint_states --once >"${LOG_DIR}/course_dispenser_joint_state_once.txt" 2>/dev/null; then + if timeout 3 ros2 topic echo "${JOINT_STATES_TOPIC}" --once >"${LOG_DIR}/course_dispenser_joint_state_once.txt" 2>/dev/null; then if grep -q '^header:' "${LOG_DIR}/course_dispenser_joint_state_once.txt"; then break fi @@ -95,33 +161,58 @@ while (( SECONDS < joint_deadline )); do sleep 1 done if ! grep -q '^header:' "${LOG_DIR}/course_dispenser_joint_state_once.txt" 2>/dev/null; then - echo '[Azas] No fresh /joint_states. MoveItPy cannot mirror the robot in RViz.' >&2 + echo "[Azas] No fresh ${JOINT_STATES_TOPIC}. MoveItPy cannot mirror the robot in RViz." >&2 if grep -qE 'Failed to initialize hardware|Wrong state or command interface configuration|INITIAL STATE CALL FAILURE|process has died' "${LOG_DIR}/course_dispenser_bringup.log" 2>/dev/null; then echo '[Azas] Doosan virtual bringup failed before joint_state_broadcaster became available.' >&2 - echo "[Azas] Current launch args: MODE=${MODE} HOST=${HOST} PORT=${PORT} MODEL=${MODEL}" >&2 + echo "[Azas] Current launch args: NAME=${ROBOT_NAME} MODE=${MODE} HOST=${HOST} PORT=${PORT} MODEL=${MODEL} RT_HOST=${RT_HOST}" >&2 echo '[Azas] If an emulator was already running, stop stale Doosan emulator/controller processes and rerun.' >&2 fi tail -100 "${LOG_DIR}/course_dispenser_bringup.log" >&2 || true exit 1 fi +action_deadline=$((SECONDS + 30)) +while (( SECONDS < action_deadline )); do + if timeout 3 ros2 action list >"${LOG_DIR}/course_dispenser_action_list.txt" 2>/dev/null; then + if grep -qx "${MOVEIT_CONTROLLER_ACTION}" "${LOG_DIR}/course_dispenser_action_list.txt"; then + break + fi + fi + sleep 1 +done +if ! grep -qx "${MOVEIT_CONTROLLER_ACTION}" "${LOG_DIR}/course_dispenser_action_list.txt" 2>/dev/null; then + echo "[Azas] Warning: ${MOVEIT_CONTROLLER_ACTION} was not observed before cycle launch." >&2 + tail -80 "${LOG_DIR}/course_dispenser_action_list.txt" >&2 || true +else + echo "[Azas] Controller action observed; settling ${CONTROLLER_SETTLE_SEC}s before MoveItPy execution." + sleep "${CONTROLLER_SETTLE_SEC}" +fi + if [[ "${DISPENSER_COLLISION_ENABLED}" == "1" || "${DISPENSER_COLLISION_ENABLED}" == "true" ]]; then if [[ "${DISPENSER_COLLISION_OBJECTS}" == "1" || "${DISPENSER_COLLISION_OBJECTS}" == "true" ]]; then DISPENSER_COLLISION_OBJECTS_BOOL=true else DISPENSER_COLLISION_OBJECTS_BOOL=false fi + if [[ "${REMOVE_COURSE_WORKSPACE_WALLS}" == "1" || "${REMOVE_COURSE_WORKSPACE_WALLS}" == "true" ]]; then + REMOVE_COURSE_WORKSPACE_WALLS_BOOL=true + else + REMOVE_COURSE_WORKSPACE_WALLS_BOOL=false + fi ros2 run azas_motion measured_dispenser_collision_scene_node \ --ros-args \ -p config_path:="${DISPENSER_COLLISION_CONFIG}" \ -p publish_period_sec:=1.0 \ -p publish_collision_objects:="${DISPENSER_COLLISION_OBJECTS_BOOL}" \ - -p remove_course_workspace_collision_objects:="${REMOVE_COURSE_WORKSPACE_WALLS}" \ + -p collision_object_exclude_ids:="${DISPENSER_COLLISION_EXCLUDE_IDS}" \ + -p remove_course_workspace_collision_objects:="${REMOVE_COURSE_WORKSPACE_WALLS_BOOL}" \ + -p clear_markers_before_publish:=false \ -p publish_markers:=true \ >"${LOG_DIR}/measured_dispenser_collision_scene.log" 2>&1 & PIDS+=("$!") echo "[Azas] Dispenser combined box represents bottle/body only; press pre/contact is derived from press_contact_joints_deg FK, not from this box." echo "[Azas] DISPENSER_COLLISION_OBJECTS=${DISPENSER_COLLISION_OBJECTS} (1=add to MoveIt collision scene, 0=RViz markers only)." + echo "[Azas] DISPENSER_COLLISION_EXCLUDE_IDS=${DISPENSER_COLLISION_EXCLUDE_IDS} (marker-only IDs; not used to block press-contact planning)." echo "[Azas] REMOVE_COURSE_WORKSPACE_WALLS=${REMOVE_COURSE_WORKSPACE_WALLS} (1=remove stale side_grip_workspace_* walls that can collide with link_2 in this course path)." sleep 2 if [[ "${DISPENSER_COLLISION_OBJECTS}" == "1" || "${DISPENSER_COLLISION_OBJECTS}" == "true" ]]; then @@ -154,19 +245,44 @@ if [[ "${SHOW_LINK6_GRIPPER}" == "1" || "${SHOW_LINK6_GRIPPER}" == "true" ]]; th echo "[Azas] SHOW_LINK6_GRIPPER=${SHOW_LINK6_GRIPPER}: publishing RG2 link_6 TF/markers on /azas/link6_gripper/markers." fi +if [[ "${DISPENSER_COLLISION_OBJECTS}" == "0" || "${DISPENSER_COLLISION_OBJECTS}" == "false" ]]; then + python3 "${ROOT_DIR}/tools/run/remove_moveit_collision_objects.py" \ + >"${LOG_DIR}/remove_moveit_collision_objects.log" 2>&1 || { + echo "[Azas] Warning: failed to remove stale MoveIt collision objects." >&2 + tail -80 "${LOG_DIR}/remove_moveit_collision_objects.log" >&2 || true + } + sleep 1 +fi + if [[ "${RVIZ_MODE}" == "clean" ]]; then # dsr_bringup2_moveit launches its default RViz unconditionally. Replace only # the RViz processes that appeared after this script started, preserving any # pre-existing RViz windows. before_lines="$(printf '%s\n' ${before_rviz:-})" for pid in $(pgrep -x rviz2 || true); do - if ! grep -qx "${pid}" <<<"${before_lines}"; then + if [[ "${REPLACE_EXISTING_RVIZ}" == "1" || "${REPLACE_EXISTING_RVIZ}" == "true" ]] || ! grep -qx "${pid}" <<<"${before_lines}"; then kill "${pid}" 2>/dev/null || true wait "${pid}" 2>/dev/null || true fi done rviz2 -d "${RVIZ_CONFIG}" >"${LOG_DIR}/course_dispenser_clean_rviz.log" 2>&1 & PIDS+=("$!") +elif [[ "${RVIZ_MODE}" == "bringup" && ! ( "${START_DOOSAN}" == "1" || "${START_DOOSAN}" == "true" ) ]]; then + # Reusing an already-running Doosan/MoveIt session does not reopen the + # teaching-material RViz. Open that exact config so the preview shows the + # course-style orange MoveIt robot, not the clean debug RobotModel view. + if [[ "${REPLACE_EXISTING_RVIZ}" == "1" || "${REPLACE_EXISTING_RVIZ}" == "true" ]]; then + for pid in $(pgrep -x rviz2 || true); do + kill "${pid}" 2>/dev/null || true + wait "${pid}" 2>/dev/null || true + done + fi + if pgrep -x rviz2 >/dev/null; then + echo "[Azas] Reusing existing RViz window for course-material orange robot view." + else + rviz2 -d "${COURSE_RVIZ_CONFIG}" >"${LOG_DIR}/course_dispenser_bringup_rviz.log" 2>&1 & + PIDS+=("$!") + fi elif [[ "${RVIZ_MODE}" == "none" ]]; then before_lines="$(printf '%s\n' ${before_rviz:-})" for pid in $(pgrep -x rviz2 || true); do @@ -180,24 +296,40 @@ fi ros2 launch azas_bringup dispenser_press_cycle_moveit.launch.py \ dispenser_id:="${DISPENSER_ID}" \ press_count:="${PRESS_COUNT}" \ + press_only:="${PRESS_ONLY}" \ + joint_states_topic:="${JOINT_STATES_TOPIC}" \ + moveit_controller_action:="${MOVEIT_CONTROLLER_ACTION}" \ trajectory_time_scale:="${TRAJECTORY_TIME_SCALE:-8.0}" \ - press_up_m:="${PRESS_UP_M:-0.02}" \ + press_up_m:="${PRESS_UP_M:-0.05}" \ cup_pre_grasp_backoff_m:="${CUP_PRE_GRASP_BACKOFF_M:-0.08}" \ cup_release_retract_m:="${CUP_RELEASE_RETRACT_M:-0.05}" \ planning_time_sec:="${PLANNING_TIME_SEC:-5.0}" \ >"${LOG_DIR}/course_dispenser_cycle.log" 2>&1 -if grep -qE 'process has died|FAILED:|ABORT|GOAL_TOLERANCE_VIOLATED|No motion plan found' "${LOG_DIR}/course_dispenser_cycle.log"; then +if grep -qE 'process has died|FAILED:|ABORT|GOAL_TOLERANCE_VIOLATED|No motion plan found|Action client not connected to action server|Failed to send trajectory|MoveIt execution failed' "${LOG_DIR}/course_dispenser_cycle.log"; then echo '[Azas] Dispenser cycle failed. See log:' >&2 tail -120 "${LOG_DIR}/course_dispenser_cycle.log" >&2 || true + if [[ "${KEEP_RVIZ_ON_FAIL}" == "1" || "${KEEP_RVIZ_ON_FAIL}" == "true" ]]; then + echo '[Azas] KEEP_RVIZ_ON_FAIL is enabled; leaving RViz/bringup open for inspection. Press Ctrl+C in this terminal to close.' >&2 + trap - EXIT + wait + fi exit 3 fi if ! grep -q 'DONE:' "${LOG_DIR}/course_dispenser_cycle.log"; then echo '[Azas] Dispenser cycle did not report DONE. See log:' >&2 tail -120 "${LOG_DIR}/course_dispenser_cycle.log" >&2 || true + if [[ "${KEEP_RVIZ_ON_FAIL}" == "1" || "${KEEP_RVIZ_ON_FAIL}" == "true" ]]; then + echo '[Azas] KEEP_RVIZ_ON_FAIL is enabled; leaving RViz/bringup open for inspection. Press Ctrl+C in this terminal to close.' >&2 + trap - EXIT + wait + fi exit 4 fi -echo '[Azas] Dispenser press cycle finished: MoveItPy plan -> robot.execute -> controller /joint_states -> RViz RobotModel.' +echo "[Azas] Dispenser press cycle finished: MoveItPy plan -> robot.execute -> controller ${JOINT_STATES_TOPIC} -> RViz RobotModel." echo "[Azas] Logs: ${LOG_DIR}/course_dispenser_bringup.log ${LOG_DIR}/course_dispenser_cycle.log ${LOG_DIR}/measured_dispenser_collision_scene.log ${LOG_DIR}/collision_object_samples.txt ${LOG_DIR}/measured_dispenser_collision_markers.txt" -wait +if [[ "${KEEP_ALIVE_AFTER_DONE}" == "1" || "${KEEP_ALIVE_AFTER_DONE}" == "true" ]]; then + echo "[Azas] KEEP_ALIVE_AFTER_DONE=${KEEP_ALIVE_AFTER_DONE}: leaving RViz/virtual bringup open. Press Ctrl+C to close." + wait +fi diff --git a/tools/run/run_doosan_real_m0609.sh b/tools/run/run_doosan_real_m0609.sh index d53fbba..6d64dd4 100755 --- a/tools/run/run_doosan_real_m0609.sh +++ b/tools/run/run_doosan_real_m0609.sh @@ -11,7 +11,7 @@ ROBOT_HOST="${ROBOT_HOST:-}" ROBOT_PORT="${ROBOT_PORT:-12345}" MODEL="${MODEL:-m0609}" COLOR="${COLOR:-white}" -RT_HOST="${RT_HOST:-192.168.137.50}" +RT_HOST="${RT_HOST:-0.0.0.0}" DOOSAN_REAL_MOTION_CONFIRM="${DOOSAN_REAL_MOTION_CONFIRM:-}" SHOW_ARGS_ONLY="${SHOW_ARGS_ONLY:-false}" diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index f0d9903..3aca08d 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -709,13 +709,19 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: steps: list[tuple[list[float], str, float, float]] = [] if joint_space_press: - self.move_posx( - [x_mm, y_mm, pre_z, rx, ry, rz], - label="high pre pose before measured press joint", - velocity=self.args.press_travel_velocity, - acceleration=self.args.press_travel_acceleration, - timeout_sec=self.args.press_timeout_sec, - ) + if self.args.press_move_configured_prepose_before_joint: + self.move_posx( + [x_mm, y_mm, pre_z, rx, ry, rz], + label="high pre pose before measured press joint", + velocity=self.args.press_travel_velocity, + acceleration=self.args.press_travel_acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + else: + print( + "[Azas] joint-space press: skipping configured Cartesian pre pose; " + "measured press contact joints are authoritative" + ) self.movej( contact_joints, label="move to measured press contact joints exactly", @@ -1102,6 +1108,16 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--press-reset-joint-acceleration", type=float, default=50.0) parser.add_argument("--press-contact-joint-velocity", type=float, default=12.0) parser.add_argument("--press-contact-joint-acceleration", type=float, default=18.0) + parser.add_argument( + "--press-move-configured-prepose-before-joint", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "When measured press_contact_joints_deg exists, optionally move to " + "calibration press_pose_xyz_m + pre_lift before MoveJoint. Default false: " + "use the measured joints as the authoritative press target." + ), + ) parser.add_argument("--press-post-retreat-after-sequence", action=argparse.BooleanOptionalAction, default=True) parser.add_argument("--press-post-retreat-dx-m", type=float, default=-0.120) parser.add_argument("--press-post-retreat-dy-m", type=float, default=0.0) diff --git a/tools/run/run_one_click_cocktail_real.sh b/tools/run/run_one_click_cocktail_real.sh new file mode 100755 index 0000000..db7529f --- /dev/null +++ b/tools/run/run_one_click_cocktail_real.sh @@ -0,0 +1,379 @@ +#!/usr/bin/env bash +set -euo pipefail + +# One-command REAL robot path for the integrated cocktail dispenser cycle. +# Sequence: +# 1) connect/reuse real Doosan M0609 service namespace, +# 2) connect/reuse RG2 set_width service, +# 3) publish measured dispenser/tumbler collision scene, +# 4) run cup-place -> full-open gripper -> safe lift -> close empty gripper +# -> measured press pump(s) -> re-grasp/lift cup. +# +# This script intentionally does not ask for cup coordinates. The cup pose is +# supplied by the existing vision/pose pipeline and the measured dispenser poses. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +LOG_DIR="${LOG_DIR:-${ROOT_DIR}/log/manual}" +INTEGRATED_LOG="${INTEGRATED_LOG:-${LOG_DIR}/one_click_real_integrated_recipe.log}" +ROBOT_HOST="${ROBOT_HOST:-192.168.1.100}" +RT_HOST="${RT_HOST:-0.0.0.0}" +RG2_IP="${RG2_IP:-192.168.1.1}" +RG2_PORT="${RG2_PORT:-502}" +ROBOT_PORT="${ROBOT_PORT:-12345}" +TCP_CHECK_SEC="${TCP_CHECK_SEC:-2}" +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS:-${DISPENSER_IDS:-1x1}}" +SERVICE_PREFIX="${SERVICE_PREFIX:-${ROBOT_NAME:-dsr01}}" +ROBOT_NAME="${ROBOT_NAME:-${SERVICE_PREFIX}}" +REAL_COCKTAIL_CONFIRM="${REAL_COCKTAIL_CONFIRM:-}" +KEEP_CONNECTION_AFTER_DONE="${KEEP_CONNECTION_AFTER_DONE:-1}" +DRY_RUN="${DRY_RUN:-0}" +WAIT_SERVICE_SEC="${WAIT_SERVICE_SEC:-45}" +COLLISION_CONFIG="${COLLISION_CONFIG:-${ROOT_DIR}/src/azas_bringup/config/measured_dispenser_collision.yaml}" +RG2_OPEN_SETTLE_SECONDS="${RG2_OPEN_SETTLE_SECONDS:-5.0}" +GRIPPER_SETTLE_SECONDS="${GRIPPER_SETTLE_SECONDS:-2.0}" +PRESS_PRE_LIFT_M="${PRESS_PRE_LIFT_M:-0.300}" +PRESS_TRANSIT_HEIGHT_M="${PRESS_TRANSIT_HEIGHT_M:-0.300}" +PRESS_DEPTH_M="${PRESS_DEPTH_M:-0.080}" +ONE_CLICK_STAGE="init" + +mkdir -p "${LOG_DIR}" + +summarize_failure() { + local rc="$1" + if [[ "${rc}" == "0" ]]; then + return 0 + fi + echo "[Azas] FAILED real one-click cocktail cycle rc=${rc} stage=${ONE_CLICK_STAGE}" >&2 + echo "[Azas] Evidence logs:" >&2 + echo " integrated=${INTEGRATED_LOG}" >&2 + echo " doosan=${LOG_DIR}/one_click_real_doosan.log" >&2 + echo " gripper=${LOG_DIR}/one_click_real_gripper.log" >&2 + echo " collision=${LOG_DIR}/one_click_real_collision_scene.log" >&2 + if [[ -f "${INTEGRATED_LOG}" ]]; then + echo "--- integrated tail ---" >&2 + tail -80 "${INTEGRATED_LOG}" >&2 || true + fi + if [[ -f "${LOG_DIR}/one_click_real_doosan.log" ]]; then + if grep -qE 'Timeout: connect timed out|Connect Failed Please check network state|DRCF connecting ERROR' "${LOG_DIR}/one_click_real_doosan.log"; then + echo "[Azas] DIAGNOSIS: Doosan controller connection timed out before motion services were usable." >&2 + echo "[Azas] CHECK: ROBOT_HOST=${ROBOT_HOST}, controller network, pendant state, and stop virtual preview before retry." >&2 + echo "[Azas] NEXT: bash tools/run/stop_cocktail_motion_preview.sh" >&2 + echo "[Azas] NEXT: RECIPE_DISPENSER_IDS=${RECIPE_DISPENSER_IDS} bash tools/run/check_one_click_cocktail_ready.sh || true" >&2 + elif grep -qE 'Wrong state or command interface configuration|missing state interfaces|missing command interfaces' "${LOG_DIR}/one_click_real_doosan.log"; then + echo "[Azas] DIAGNOSIS: Doosan ros2_control hardware interfaces did not initialize; usually caused by connection failure or stale/aborted bringup." >&2 + fi + fi +} + +usage() { + cat <&2 + echo "[Azas] This can move the real robot, actuate RG2, and press the dispenser." >&2 + echo "[Azas] Re-run with: REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE" >&2 + exit 2 +fi + +if [[ "${ROBOT_HOST}" == "127.0.0.1" || "${ROBOT_HOST}" == "localhost" ]]; then + echo "[Azas] Refusing real cocktail cycle: ROBOT_HOST=${ROBOT_HOST} is not a real controller IP." >&2 + exit 2 +fi + +trap 'summarize_failure "$?"' EXIT + +run_or_print() { + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + printf '[DRY_RUN] %q ' "$@" + printf '\n' + else + "$@" + fi +} + +source_ros() { + set +u + source /opt/ros/humble/setup.bash + source /home/ssu/ws_moveit/install/setup.bash 2>/dev/null || true + source /home/ssu/ros2_ws/install/setup.bash + if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then + source "${ROOT_DIR}/install/setup.bash" + else + source "${ROOT_DIR}/install/local_setup.bash" + fi + set -u +} + +wait_for_ros_service() { + local service="$1" + local label="$2" + local deadline=$((SECONDS + WAIT_SERVICE_SEC)) + while (( SECONDS < deadline )); do + if ros2 service list 2>/dev/null | grep -qx "${service}"; then + echo "[Azas] ${label} ready: ${service}" + return 0 + fi + sleep 1 + done + echo "[Azas] Timeout waiting for ${label}: ${service}" >&2 + return 1 +} + +wait_for_motion_services() { + wait_for_ros_service "/${SERVICE_PREFIX}/motion/move_joint" "Doosan move_joint" + wait_for_ros_service "/${SERVICE_PREFIX}/motion/move_line" "Doosan move_line" + wait_for_ros_service "/${SERVICE_PREFIX}/motion/move_wait" "Doosan move_wait" + wait_for_ros_service "/${SERVICE_PREFIX}/motion/fkin" "Doosan fkin" + wait_for_ros_service "/${SERVICE_PREFIX}/motion/ikin" "Doosan ikin" + wait_for_ros_service "/${SERVICE_PREFIX}/motion/check_motion" "Doosan check_motion" + wait_for_ros_service "/${SERVICE_PREFIX}/system/get_robot_state" "Doosan get_robot_state" + wait_for_ros_service "/${SERVICE_PREFIX}/aux_control/get_current_posj" "Doosan get_current_posj" + wait_for_ros_service "/${SERVICE_PREFIX}/aux_control/get_current_posx" "Doosan get_current_posx" +} + +check_robot_tcp_before_bringup() { + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] check TCP ${ROBOT_HOST}:${ROBOT_PORT} before starting real Doosan bringup" + return 0 + fi + if command -v nc >/dev/null 2>&1; then + if timeout "${TCP_CHECK_SEC}s" nc -z "${ROBOT_HOST}" "${ROBOT_PORT}" >/dev/null 2>&1; then + echo "[Azas] Doosan TCP reachable: ${ROBOT_HOST}:${ROBOT_PORT}" + return 0 + fi + echo "[Azas] Refusing to start real Doosan bringup: ${ROBOT_HOST}:${ROBOT_PORT} is not reachable." >&2 + echo "[Azas] Check controller IP/network/pendant state, then rerun readiness." >&2 + return 2 + fi + echo "[Azas] nc not installed; skipping Doosan TCP preflight." +} + +service_exists() { + local service="$1" + ros2 service list 2>/dev/null | grep -qx "${service}" +} + +call_empty_service() { + local service="$1" + local type="$2" + python3 "${ROOT_DIR}/tools/run/ros_call_empty_service.py" "${service}" "${type}" --timeout 8.0 +} + +verify_doosan_motion_ready() { + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] verify /${SERVICE_PREFIX}/system/get_robot_state == robot_state=1 and /${SERVICE_PREFIX}/motion/check_motion status=0" + return 0 + fi + local state_output motion_output + echo "[Azas] Verifying Doosan robot state before integrated motion." + state_output="$(call_empty_service "/${SERVICE_PREFIX}/system/get_robot_state" "dsr_msgs2/srv/GetRobotState")" + echo "--- get_robot_state ---" + echo "${state_output}" + if ! grep -Eq '(^|[[:space:]])(robot_state|state)=1($|[[:space:]])' <<<"${state_output}"; then + echo "[Azas] Refusing integrated motion: robot_state is not STATE_STANDBY(1)." >&2 + return 2 + fi + motion_output="$(call_empty_service "/${SERVICE_PREFIX}/motion/check_motion" "dsr_msgs2/srv/CheckMotion")" + echo "--- check_motion ---" + echo "${motion_output}" + if ! grep -Eq '(^|[[:space:]])status=0($|[[:space:]])' <<<"${motion_output}"; then + echo "[Azas] Refusing integrated motion: check_motion status is not 0." >&2 + return 2 + fi +} + +start_real_doosan_if_needed() { + local virtual_matches + virtual_matches="$(pgrep -af 'dsr_bringup2_moveit|run_emulator|DRCF|ros2_control_node' | grep -v "$$" | grep -v 'run_one_click_cocktail_real.sh' | grep -v 'pgrep -af' | grep -v 'grep -E' | grep -E 'mode:=virtual|run_emulator|DRCF' || true)" + if [[ -n "${virtual_matches}" ]]; then + echo "[Azas] Refusing real cocktail cycle: an active Doosan session looks VIRTUAL/emulated." >&2 + echo "[Azas] Stop the RViz/virtual preview before real motion, then rerun this script." >&2 + echo "[Azas] Command: bash tools/run/stop_cocktail_motion_preview.sh" >&2 + echo "${virtual_matches}" >&2 + return 2 + fi + + if service_exists "/${SERVICE_PREFIX}/motion/move_joint"; then + echo "[Azas] Reusing existing non-virtual Doosan services under /${SERVICE_PREFIX}; checking full service set." + wait_for_motion_services + return 0 + fi + check_robot_tcp_before_bringup + echo "[Azas] Starting real Doosan bringup: ROBOT_HOST=${ROBOT_HOST} ROBOT_NAME=${ROBOT_NAME} RT_HOST=${RT_HOST}" + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] ROBOT_HOST=${ROBOT_HOST} ROBOT_NAME=${ROBOT_NAME} RT_HOST=${RT_HOST} DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP tools/run/run_doosan_real_m0609.sh &" + return 0 + fi + ( + cd "${ROOT_DIR}" + ROBOT_HOST="${ROBOT_HOST}" ROBOT_NAME="${ROBOT_NAME}" RT_HOST="${RT_HOST}" \ + DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP \ + tools/run/run_doosan_real_m0609.sh + ) >"${LOG_DIR}/one_click_real_doosan.log" 2>&1 & + DOOSAN_PID=$! + echo "[Azas] Doosan pid=${DOOSAN_PID} log=${LOG_DIR}/one_click_real_doosan.log" + wait_for_motion_services +} + +start_gripper_if_needed() { + if service_exists "/jarvis/rg2/set_width"; then + echo "[Azas] Reusing existing RG2 services; checking full service set." + wait_for_ros_service "/jarvis/rg2/set_width" "RG2 set_width" + wait_for_ros_service "/jarvis/rg2/open" "RG2 open" + wait_for_ros_service "/jarvis/rg2/close" "RG2 close" + return 0 + fi + echo "[Azas] Starting RG2 service wrapper: ${RG2_IP}:${RG2_PORT}" + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] ros2 launch azas_gripper rg2_trigger.launch.py ip:=${RG2_IP} port:=${RG2_PORT} connect:=true open_width:=1100 close_width:=0 force:=300 settle_seconds:=0.6 &" + return 0 + fi + ( + cd "${ROOT_DIR}" + source_ros + source "${ROOT_DIR}/install/azas_gripper/share/azas_gripper/package.bash" 2>/dev/null || true + ros2 launch "${ROOT_DIR}/install/azas_gripper/share/azas_gripper/launch/rg2_trigger.launch.py" \ + ip:="${RG2_IP}" port:="${RG2_PORT}" connect:=true open_width:=1100 close_width:=0 force:=300 settle_seconds:=0.6 + ) >"${LOG_DIR}/one_click_real_gripper.log" 2>&1 & + GRIPPER_PID=$! + echo "[Azas] RG2 pid=${GRIPPER_PID} log=${LOG_DIR}/one_click_real_gripper.log" + wait_for_ros_service "/jarvis/rg2/set_width" "RG2 set_width" + wait_for_ros_service "/jarvis/rg2/open" "RG2 open" + wait_for_ros_service "/jarvis/rg2/close" "RG2 close" +} + +start_collision_scene() { + echo "[Azas] Starting measured dispenser/tumbler collision publishers." + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] measured_dispenser_collision_scene_node + tumbler_collision_scene_node &" + return 0 + fi + pkill -f 'measured_dispenser_collision_scene_node' 2>/dev/null || true + pkill -f 'tumbler_collision_scene_node' 2>/dev/null || true + sleep 0.5 + ( + cd "${ROOT_DIR}" + source_ros + python3 -m azas_motion.measured_dispenser_collision_scene_node \ + --ros-args \ + -p config_path:="${COLLISION_CONFIG}" \ + -p publish_period_sec:=2.0 \ + -p collision_object_exclude_ids:=dispenser_head_nozzle_merged_horizontal_spout_box \ + -p remove_course_workspace_collision_objects:=true & + python3 -m azas_motion.tumbler_collision_scene_node \ + --ros-args \ + -p action:=publish_detected \ + -p object_id:=detected_tumbler \ + -p use_lidded_height:=true + ) >"${LOG_DIR}/one_click_real_collision_scene.log" 2>&1 & + COLLISION_PID=$! + echo "[Azas] collision pid=${COLLISION_PID} log=${LOG_DIR}/one_click_real_collision_scene.log" + sleep 2 +} + +run_integrated_recipe() { + echo "[Azas] Running integrated cocktail dispenser cycle: ${RECIPE_DISPENSER_IDS}" + echo "[Azas] Cycle: cup-place -> RG2 full-open -> high lift -> close empty gripper -> press pump(s) -> re-grasp/lift." + echo "[Azas] press_pre_lift_m=${PRESS_PRE_LIFT_M} press_transit_height_m=${PRESS_TRANSIT_HEIGHT_M} press_depth_m=${PRESS_DEPTH_M}" + echo "[Azas] gripper_open_settle_seconds=${RG2_OPEN_SETTLE_SECONDS} gripper_settle_seconds=${GRIPPER_SETTLE_SECONDS}" + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + run_or_print \ + python3 "${ROOT_DIR}/tools/run/run_measured_dispenser_recipe_sequence.py" \ + --dispenser-ids "${RECIPE_DISPENSER_IDS}" \ + --service-prefix "${SERVICE_PREFIX}" \ + --press-pre-lift-m "${PRESS_PRE_LIFT_M}" \ + --press-transit-height-m "${PRESS_TRANSIT_HEIGHT_M}" \ + --press-depth-m "${PRESS_DEPTH_M}" \ + --gripper-open-settle-seconds "${RG2_OPEN_SETTLE_SECONDS}" \ + --gripper-settle-seconds "${GRIPPER_SETTLE_SECONDS}" \ + --execute \ + --confirm ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE + return 0 + fi + + set +e + python3 "${ROOT_DIR}/tools/run/run_measured_dispenser_recipe_sequence.py" \ + --dispenser-ids "${RECIPE_DISPENSER_IDS}" \ + --service-prefix "${SERVICE_PREFIX}" \ + --press-pre-lift-m "${PRESS_PRE_LIFT_M}" \ + --press-transit-height-m "${PRESS_TRANSIT_HEIGHT_M}" \ + --press-depth-m "${PRESS_DEPTH_M}" \ + --gripper-open-settle-seconds "${RG2_OPEN_SETTLE_SECONDS}" \ + --gripper-settle-seconds "${GRIPPER_SETTLE_SECONDS}" \ + --execute \ + --confirm ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE 2>&1 | tee "${INTEGRATED_LOG}" + local rc="${PIPESTATUS[0]}" + set -e + return "${rc}" +} + +print_post_run_evidence() { + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] post-run evidence would sample current posj/posx and integrated log." + return 0 + fi + echo "[Azas] POST-RUN EVIDENCE: integrated sequence returned success." + if [[ -f "${INTEGRATED_LOG}" ]] && grep -q '\[PASS\] measured dispenser recipe sequence completed' "${INTEGRATED_LOG}"; then + echo "[Azas] PASS marker found in ${INTEGRATED_LOG}" + else + echo "[Azas] WARN: integrated command returned 0 but PASS marker was not found in ${INTEGRATED_LOG}" >&2 + fi + SAMPLE_CURRENT_POSE=0 \ + INTEGRATED_LOG="${INTEGRATED_LOG}" \ + SERVICE_PREFIX="${SERVICE_PREFIX}" \ + bash "${ROOT_DIR}/tools/run/check_one_click_cocktail_result.sh" + echo "--- final current_posj sample ---" + call_empty_service "/${SERVICE_PREFIX}/aux_control/get_current_posj" "dsr_msgs2/srv/GetCurrentPosj" || true + echo "--- final current_posx sample ---" + call_empty_service "/${SERVICE_PREFIX}/aux_control/get_current_posx" "dsr_msgs2/srv/GetCurrentPosx" || true +} + +source_ros +ONE_CLICK_STAGE="config_preflight" +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS}" \ + "${ROOT_DIR}/tools/run/check_one_click_cocktail_config.sh" +ONE_CLICK_STAGE="start_real_doosan" +start_real_doosan_if_needed +ONE_CLICK_STAGE="verify_doosan_motion_ready" +verify_doosan_motion_ready +ONE_CLICK_STAGE="start_gripper" +start_gripper_if_needed +ONE_CLICK_STAGE="start_collision_scene" +start_collision_scene +ONE_CLICK_STAGE="run_integrated_recipe" +run_integrated_recipe +ONE_CLICK_STAGE="post_run_evidence" +print_post_run_evidence +ONE_CLICK_STAGE="done" + +if [[ "${KEEP_CONNECTION_AFTER_DONE}" == "1" || "${KEEP_CONNECTION_AFTER_DONE}" == "true" ]]; then + echo "[Azas] DONE. Real robot/RG2/collision background nodes were left running for inspection/reuse." + echo "[Azas] Logs: ${INTEGRATED_LOG} ${LOG_DIR}/one_click_real_doosan.log ${LOG_DIR}/one_click_real_gripper.log ${LOG_DIR}/one_click_real_collision_scene.log" +else + echo "[Azas] DONE. KEEP_CONNECTION_AFTER_DONE=0 requested; stopping nodes started by this script." + for pid in "${COLLISION_PID:-}" "${GRIPPER_PID:-}" "${DOOSAN_PID:-}"; do + if [[ -n "${pid}" ]] && kill -0 "${pid}" 2>/dev/null; then + kill "${pid}" 2>/dev/null || true + fi + done +fi diff --git a/tools/run/show_cocktail_motion_preview.sh b/tools/run/show_cocktail_motion_preview.sh new file mode 100755 index 0000000..34164ee --- /dev/null +++ b/tools/run/show_cocktail_motion_preview.sh @@ -0,0 +1,53 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Short operator command for the full cocktail dispenser motion preview. +# This is RViz/virtual only: it never commands the real robot. +# Sequence shown: cup-place -> open gripper -> safe lift -> close empty gripper +# -> measured press pump(s) -> re-grasp/lift. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS:-${1:-1x1}}" +DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS:-1}" +KEEP_ALIVE_AFTER_DONE="${KEEP_ALIVE_AFTER_DONE:-1}" +RESET_EXISTING_VIRTUAL_PREVIEW="${RESET_EXISTING_VIRTUAL_PREVIEW:-1}" +REPLACE_EXISTING_RVIZ="${REPLACE_EXISTING_RVIZ:-1}" +# Default to the Doosan teaching-material MoveIt RViz ("orange robot") view. +# Operators can still request the lean debug RobotModel view with RVIZ_MODE=clean. +RVIZ_MODE="${RVIZ_MODE:-bringup}" + +usage() { + cat < RG2 open -> safe Z lift -> RG2 close -> press pump(s) -> re-grasp/lift." + +cd "${ROOT_DIR}" +RECIPE_DISPENSER_IDS="${RECIPE_DISPENSER_IDS}" \ +DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS}" \ +KEEP_ALIVE_AFTER_DONE="${KEEP_ALIVE_AFTER_DONE}" \ +RESET_EXISTING_VIRTUAL_PREVIEW="${RESET_EXISTING_VIRTUAL_PREVIEW}" \ +REPLACE_EXISTING_RVIZ="${REPLACE_EXISTING_RVIZ}" \ +RVIZ_MODE="${RVIZ_MODE}" \ +bash tools/run/run_cocktail_collision_rviz_preview.sh diff --git a/tools/run/show_color_scan_pose_rviz.sh b/tools/run/show_color_scan_pose_rviz.sh new file mode 100755 index 0000000..ab69df8 --- /dev/null +++ b/tools/run/show_color_scan_pose_rviz.sh @@ -0,0 +1,35 @@ +#!/usr/bin/env bash +set -euo pipefail + +# RViz-only preview of the dispenser color-classification camera pose. +# It publishes visual /joint_states for [0, 10, 32, 0, 100, 90] deg and +# never calls a Doosan motion service. + +ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" + +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-79}" +export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" + +set +u +source /opt/ros/humble/setup.bash +if [[ -f /home/ssu/ros2_ws/install/setup.bash ]]; then + source /home/ssu/ros2_ws/install/setup.bash +fi +if [[ -f /home/ssu/ws_moveit/install/setup.bash ]]; then + source /home/ssu/ws_moveit/install/setup.bash +fi +if [[ -f "${ROOT}/install/setup.bash" ]]; then + source "${ROOT}/install/setup.bash" +else + source "${ROOT}/install/local_setup.bash" +fi +set -u + +echo "[Azas] RViz color scan pose preview" +echo "[Azas] joints_deg=[0, 10, 32, 0, 100, 90]" +echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID}" +echo "[Azas] RViz-only: robot model loops HOME -> color scan pose -> HOME; no real robot motion command will be sent" + +exec ros2 launch "${ROOT}/src/azas_bringup/launch/color_scan_pose_rviz.launch.py" \ + use_rviz:="${USE_RVIZ:-true}" \ + preview_mode:="${PREVIEW_MODE:-color_scan_pose_move}" diff --git a/tools/run/stop_cocktail_motion_preview.sh b/tools/run/stop_cocktail_motion_preview.sh new file mode 100755 index 0000000..9c73cf4 --- /dev/null +++ b/tools/run/stop_cocktail_motion_preview.sh @@ -0,0 +1,96 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Stop only the RViz/virtual cocktail preview stack. This is intended before +# real robot execution so /dsr01 services are not accidentally backed by the +# virtual Doosan emulator. + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +DRY_RUN="${DRY_RUN:-0}" +KILL_RVIZ="${KILL_RVIZ:-1}" + +kill_tree() { + local pid="$1" + local child + for child in $(pgrep -P "${pid}" 2>/dev/null || true); do + kill_tree "${child}" + done + if kill -0 "${pid}" 2>/dev/null; then + if [[ "${DRY_RUN}" == "1" || "${DRY_RUN}" == "true" ]]; then + echo "[DRY_RUN] kill ${pid} $(ps -p "${pid}" -o comm= 2>/dev/null || true)" + else + kill "${pid}" 2>/dev/null || true + fi + fi +} + +collect_roots() { + { + # Use ps instead of pgrep -f for launch processes because long ROS launch + # argv lines can be truncated differently by pgrep on some systems. + ps -eo pid=,args= \ + | grep -E 'run_cocktail_collision_rviz_preview.sh|run_course_dispenser_press_cycle_rviz.sh' \ + | grep -v "$$" \ + | grep -v 'stop_cocktail_motion_preview.sh' \ + | grep -v 'grep -E' \ + | awk '{print $1}' || true + ps -eo pid=,args= \ + | grep 'dsr_bringup2_moveit.launch.py' \ + | grep 'mode:=virtual' \ + | grep -v "$$" \ + | grep -v 'stop_cocktail_motion_preview.sh' \ + | grep -v 'grep ' \ + | awk '{print $1}' || true + ps -eo pid=,args= \ + | grep -E 'run_emulator|./DRCF M0609|/DRCF M0609' \ + | grep -v "$$" \ + | grep -v 'stop_cocktail_motion_preview.sh' \ + | grep -v 'grep -E' \ + | awk '{print $1}' || true + if [[ "${KILL_RVIZ}" == "1" || "${KILL_RVIZ}" == "true" ]]; then + ps -eo pid=,args= \ + | grep 'rviz2' \ + | grep -E 'azas_cocktail_collision_preview|dsr_moveit_config_m0609.*/moveit.rviz' \ + | grep -v "$$" \ + | grep -v 'stop_cocktail_motion_preview.sh' \ + | grep -v 'grep ' \ + | awk '{print $1}' || true + fi + } | sort -n | uniq | grep -v "^$$$" || true +} + +echo "[Azas] Stopping virtual/RViz cocktail preview stack. Real robot processes are not targeted." +mapfile -t roots < <(collect_roots) +if [[ "${#roots[@]}" -eq 0 ]]; then + echo "[Azas] No cocktail preview processes found." + exit 0 +fi + +for pid in "${roots[@]}"; do + if kill -0 "${pid}" 2>/dev/null; then + echo "[Azas] stopping preview pid=${pid} cmd=$(ps -p "${pid}" -o args= 2>/dev/null || true)" + kill_tree "${pid}" + fi +done + +if [[ "${DRY_RUN}" != "1" && "${DRY_RUN}" != "true" ]]; then + sleep 2 + # Escalate only matching preview/emulator remnants, not arbitrary real bringup. + for pid in $(collect_roots); do + if kill -0 "${pid}" 2>/dev/null; then + echo "[Azas] force stopping lingering preview pid=${pid}" + kill -9 "${pid}" 2>/dev/null || true + fi + done + sleep 1 + lingering="$(collect_roots)" + if [[ -n "${lingering}" ]]; then + echo "[Azas] Warning: preview processes still visible after stop:" >&2 + for pid in ${lingering}; do + echo " ${pid} $(ps -p "${pid}" -o args= 2>/dev/null || true)" >&2 + done + exit 1 + fi +fi + +echo "[Azas] Preview stop complete." diff --git a/tools/smoke/smoke_one_click_cocktail_no_motion.sh b/tools/smoke/smoke_one_click_cocktail_no_motion.sh new file mode 100755 index 0000000..8004803 --- /dev/null +++ b/tools/smoke/smoke_one_click_cocktail_no_motion.sh @@ -0,0 +1,161 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" +cd "${ROOT_DIR}" + +TMP_OUT="$(mktemp)" +PLAN_OUT="$(mktemp)" +RESULT_LOG="$(mktemp)" +trap 'rm -f "${TMP_OUT}" "${PLAN_OUT}" "${RESULT_LOG}"' EXIT + +echo "[Azas smoke] one-click cocktail no-motion smoke" + +bash -n \ + tools/run/run_one_click_cocktail_real.sh \ + tools/run/run_cocktail_now_real.sh \ + tools/run/report_cocktail_now_status.sh \ + tools/run/check_one_click_cocktail_config.sh \ + tools/run/check_one_click_cocktail_ready.sh \ + tools/run/check_one_click_cocktail_result.sh \ + tools/run/show_cocktail_motion_preview.sh \ + tools/run/run_cocktail_collision_rviz_preview.sh \ + tools/run/stop_cocktail_motion_preview.sh + +DRY_RUN=1 bash tools/run/stop_cocktail_motion_preview.sh >"${TMP_OUT}" 2>&1 || { + cat "${TMP_OUT}" >&2 + exit 1 +} +grep -q -- 'Stopping virtual/RViz cocktail preview stack' "${TMP_OUT}" +grep -Eq -- 'No cocktail preview processes found|Preview stop complete' "${TMP_OUT}" + +grep -q -- 'dsr_bringup2_moveit.launch.py' tools/run/stop_cocktail_motion_preview.sh +grep -q -- 'mode:=virtual' tools/run/stop_cocktail_motion_preview.sh +grep -Eq -- './DRCF M0609|/DRCF M0609' tools/run/stop_cocktail_motion_preview.sh +grep -q -- 'TCP_HARD_BLOCK_FOR_READY' tools/run/run_cocktail_now_real.sh +grep -q -- 'TCP_HARD_BLOCK' tools/run/check_one_click_cocktail_ready.sh + +# Make the rest of the dry-run smoke deterministic even if an operator left the +# RViz/virtual preview open. This does not target real robot processes. +bash tools/run/stop_cocktail_motion_preview.sh >"${TMP_OUT}" 2>&1 +grep -Eq -- 'No cocktail preview processes found|Preview stop complete' "${TMP_OUT}" + +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +DRY_RUN=1 \ +SERVICE_PREFIX=not_running \ +RECIPE_DISPENSER_IDS=1x2 \ +ROBOT_HOST=192.168.1.100 \ +bash tools/run/run_one_click_cocktail_real.sh >"${TMP_OUT}" 2>&1 + +grep -q -- '--dispenser-ids' "${TMP_OUT}" +grep -q -- '1x2' "${TMP_OUT}" +grep -q -- 'check TCP 192.168.1.100:12345 before starting real Doosan bringup' "${TMP_OUT}" +grep -q -- '--press-pre-lift-m' "${TMP_OUT}" +grep -q -- '--press-depth-m' "${TMP_OUT}" +grep -q -- 'post-run evidence would sample current posj/posx' "${TMP_OUT}" + +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +DRY_RUN=1 \ +SKIP_PREVIEW_STOP=1 \ +SERVICE_PREFIX=dsr01 \ +ROBOT_HOST=192.168.1.100 \ +bash tools/run/run_cocktail_now_real.sh 1x2 >"${TMP_OUT}" 2>&1 +grep -q -- 'Cocktail NOW real cycle: 1x2' "${TMP_OUT}" +grep -q -- 'recipe_dispenser_ids=1x2' "${TMP_OUT}" +grep -q -- 'robot_name=dsr01 service_prefix=dsr01' "${TMP_OUT}" +grep -q -- 'TCP_HARD_BLOCK=0' "${TMP_OUT}" || true +grep -q -- 'Running integrated cocktail dispenser cycle: 1x2' "${TMP_OUT}" + +REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE \ +DRY_RUN=1 \ +SERVICE_PREFIX=not_running \ +ROBOT_HOST=192.168.1.100 \ +bash tools/run/run_cocktail_now_real.sh 1x2 >"${TMP_OUT}" 2>&1 +grep -q -- 'Stopping virtual/RViz cocktail preview stack' "${TMP_OUT}" +grep -q -- 'Cocktail NOW real cycle: 1x2' "${TMP_OUT}" +grep -q -- 'robot_name=not_running service_prefix=not_running' "${TMP_OUT}" +grep -q -- 'check TCP 192.168.1.100:12345 before starting real Doosan bringup' "${TMP_OUT}" +grep -q -- 'Starting real Doosan bringup: ROBOT_HOST=192.168.1.100 ROBOT_NAME=not_running' "${TMP_OUT}" +grep -q -- 'Running integrated cocktail dispenser cycle: 1x2' "${TMP_OUT}" + +python3 tools/run/run_measured_dispenser_recipe_sequence.py \ + --dispenser-ids 1x2 \ + --confirm ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE >"${PLAN_OUT}" 2>&1 +grep -q -- 'dispenser_ids=1,1' "${PLAN_OUT}" +grep -q -- 'grouped_press_counts=1x2' "${PLAN_OUT}" +grep -q -- 'integrated move/release -> integrated press 2 time(s) -> integrated re-grasp/lift' "${PLAN_OUT}" +grep -q -- '\[PASS\] measured dispenser recipe sequence completed' "${PLAN_OUT}" + +RECIPE_DISPENSER_IDS=1x2 bash tools/run/check_one_click_cocktail_config.sh >"${TMP_OUT}" 2>&1 +grep -q -- '\[PASS\] one-click cocktail config preflight OK' "${TMP_OUT}" +grep -q -- 'dispenser_ids=1,1' "${TMP_OUT}" +grep -q -- 'grouped_press_counts=1x2' "${TMP_OUT}" + +cat >"${RESULT_LOG}" <<'LOG' +[Azas] RG2 full-open release complete; continuing only after open settle wait +[Azas] RG2 close empty gripper for dispenser press: sent RG2 set_width command width_units=0 force_units=300 +[Azas] move to measured press contact joints exactly: joints_deg=[15.12, 40.50, 32.87, -33.75, 51.99, 28.07] +[Azas] press dispenser pump 1/2: posx=[1,2,3,4,5,6] +[Azas] press dispenser pump 2/2: posx=[1,2,3,4,5,6] +[Azas] RG2 soft side-grasp: sent RG2 set_width command width_units=750 force_units=250 +[Azas] post-grasp lift: posx=[1,2,3,4,5,6] +[PASS] measured dispenser recipe sequence completed +LOG +SAMPLE_CURRENT_POSE=0 INTEGRATED_LOG="${RESULT_LOG}" bash tools/run/check_one_click_cocktail_result.sh >/dev/null + +python3 - <<'PY' +from pathlib import Path +import importlib.util +import yaml + +expected_press_joints = { + "1": [15.12, 40.50, 32.87, -33.75, 51.99, 28.07], + "2": [6.36, 39.76, 30.07, -14.08, 55.67, 27.99], + "3": [-0.29, 40.29, 28.38, -5.98, 55.25, 14.33], + "4": [-7.04, 40.77, 28.37, 4.55, 54.02, 0.22], +} + +calibration = yaml.safe_load(Path('src/azas_bringup/config/calibration.yaml').read_text()) +for dispenser_id, expected in expected_press_joints.items(): + actual = calibration['dispenser_outlets'][dispenser_id]['press_contact_joints_deg'] + assert len(actual) == 6, (dispenser_id, actual) + assert all(abs(float(a) - e) < 1e-6 for a, e in zip(actual, expected)), (dispenser_id, actual, expected) + +recipe_source = Path('tools/run/run_measured_dispenser_recipe_sequence.py').read_text() +assert 'default=False' in recipe_source and '--press-move-configured-prepose-before-joint' in recipe_source +assert 'measured press contact joints are authoritative' in recipe_source +print('[Azas smoke] measured press joints and joint-first press path OK') + +path = Path('tools/run/robot_pipeline_control_server.py') +spec = importlib.util.spec_from_file_location('robot_pipeline_control_server', path) +mod = importlib.util.module_from_spec(spec) +import sys +sys.modules[spec.name] = mod +spec.loader.exec_module(mod) +config = { + 'recipe_dispenser_ids': '1x2', + 'robot_host': '192.168.1.100', + 'service_prefix': 'dsr01', +} +one_click_step = next(s for s in mod.STEPS if s.key == 'run_one_click_cocktail_real') +cmd = mod.command_for(one_click_step, config) +assert 'REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE' in cmd +assert 'RECIPE_DISPENSER_IDS=1x2' in cmd +assert 'ROBOT_NAME=dsr01' in cmd +assert 'run_one_click_cocktail_real.sh' in cmd +ready_step = next(s for s in mod.STEPS if s.key == 'check_one_click_cocktail_ready') +ready = mod.command_for(ready_step, config) +assert 'check_one_click_cocktail_ready.sh' in ready +assert 'ROBOT_NAME=dsr01' in ready +result_step = next(s for s in mod.STEPS if s.key == 'check_one_click_cocktail_result') +result = mod.command_for(result_step, config) +assert 'check_one_click_cocktail_result.sh' in result +now_step = next(s for s in mod.STEPS if s.key == 'run_cocktail_now_real') +now_cmd = mod.command_for(now_step, config) +assert 'REAL_COCKTAIL_CONFIRM=ENABLE_REAL_COCKTAIL_SEQUENCE' in now_cmd +assert 'ROBOT_NAME=dsr01' in now_cmd +assert 'run_cocktail_now_real.sh 1x2' in now_cmd +print('[Azas smoke] panel command generation OK') +PY + +echo "[PASS] one-click cocktail no-motion smoke" From 57b1ccaa2a66bc3782a5de661fca6cf46e51cc13 Mon Sep 17 00:00:00 2001 From: suuu0719 Date: Mon, 8 Jun 2026 16:12:45 +0900 Subject: [PATCH 28/88] Revert "Fix ArUco marker test compatibility across OpenCV versions" This reverts commit d44217ef1b2f02e2e1bfb06e1e4744301ad654fd. --- src/azas_perception/test/test_depth_and_detection_logic.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/azas_perception/test/test_depth_and_detection_logic.py b/src/azas_perception/test/test_depth_and_detection_logic.py index 5819edc..39dc783 100644 --- a/src/azas_perception/test/test_depth_and_detection_logic.py +++ b/src/azas_perception/test/test_depth_and_detection_logic.py @@ -72,10 +72,7 @@ def test_detect_red_circle_marker_uses_lid_roi(): def test_detect_aruco_marker_uses_configured_dictionary_and_roi(): image = np.full((160, 160, 3), 255, dtype=np.uint8) dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) - if hasattr(cv2.aruco, "generateImageMarker"): - marker_image = cv2.aruco.generateImageMarker(dictionary, 7, 60) - else: - marker_image = cv2.aruco.drawMarker(dictionary, 7, 60) + marker_image = cv2.aruco.generateImageMarker(dictionary, 7, 60) image[50:110, 50:110] = cv2.cvtColor(marker_image, cv2.COLOR_GRAY2BGR) roi = ImageRoi(30, 30, 130, 130) From 4c6fbb2d574011390b429b2176e4c34e624a0a08 Mon Sep 17 00:00:00 2001 From: suuu0719 Date: Mon, 8 Jun 2026 16:12:45 +0900 Subject: [PATCH 29/88] Revert "Fix flaky ROS setup in colcon CI job" This reverts commit 098e6465eb612effdec912c309861ad4fdc29727. --- .github/workflows/ci.yml | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 9b7129d..0f7b648 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -12,21 +12,14 @@ jobs: steps: - uses: actions/checkout@v4 - - name: Set up ROS apt repository - run: | - sudo add-apt-repository universe - sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ - -o /usr/share/keyrings/ros-archive-keyring.gpg - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ - http://packages.ros.org/ros2/ubuntu \ - $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | \ - sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: humble - name: Install package dependencies run: | sudo apt-get update - sudo apt-get install -y python3-colcon-common-extensions python3-colcon-ros python3-rosdep ros-humble-ament-cmake-python - sudo rosdep init || true + sudo apt-get install -y python3-colcon-common-extensions python3-colcon-ros ros-humble-ament-cmake-python rosdep update rosdep install --from-paths src --ignore-src --rosdistro humble -y \ --skip-keys "ament_python moveit_py dsr_description2 dsr_moveit_config_m0609 dsr_msgs2" From cafe749b41f330614c25c8837d3c5e59f6d2b13b Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 10:15:48 +0900 Subject: [PATCH 30/88] Enhance dispenser collision handling and improve recipe execution parameters - Set REMOVE_COURSE_WORKSPACE_WALLS to 0 in run_cocktail_collision_rviz_preview.sh and run_course_dispenser_press_cycle_rviz.sh to keep safety walls active. - Introduce workspace collision handling in run_course_dispenser_press_cycle_rviz.sh with logging for collision markers. - Update run_color_recipe_sequence.py to include fallback dispenser sequence and additional execution parameters for measured dispenser recipe sequences. - Implement retry logic for pose reading in run_measured_dispenser_recipe_sequence.py to enhance robustness during motion execution. - Add new arguments for regrasp and lift parameters, including minimum transit height and approach offsets, to improve post-press handling. - Modify press handling logic to allow for joint-space fallback and improve verification of target positions during motion. --- docs/lid_gripper_pipeline.md | 16 + docs/robot_pipeline_control.html | 240 +++++++- src/azas_bringup/config/calibration.yaml | 4 +- .../rviz/azas_cocktail_collision_preview.rviz | 10 + .../rviz/azas_dispenser_sequence.rviz | 10 + .../rviz/azas_dispenser_sequence_clean.rviz | 10 + src/azas_bringup/rviz/color_scan_pose.rviz | 10 + .../rviz/link6_gripper_tcp_debug.rviz | 10 + .../launch/yolo_cup_uprighting.launch.py | 26 +- ...measured_dispenser_collision_scene_node.py | 74 +++ .../workspace_collision_scene_node.py | 70 +++ .../dsr_practice/yolo_cup_pick_node.py | 17 +- tools/perception/dispenser_color_scan.py | 236 +++++++- tools/run/dispenser_color_scan_ros.sh | 8 +- tools/run/place_side_grip_cup_in_holder.py | 166 ++++- tools/run/robot_pipeline_control_server.py | 566 ++++++++++++++++-- .../run_cocktail_collision_rviz_preview.sh | 2 +- tools/run/run_color_recipe_sequence.py | 61 +- .../run_course_dispenser_press_cycle_rviz.sh | 25 +- .../run_measured_dispenser_recipe_sequence.py | 448 ++++++++++++-- 20 files changed, 1805 insertions(+), 204 deletions(-) diff --git a/docs/lid_gripper_pipeline.md b/docs/lid_gripper_pipeline.md index 096fa54..b522f77 100644 --- a/docs/lid_gripper_pipeline.md +++ b/docs/lid_gripper_pipeline.md @@ -146,3 +146,19 @@ ros2 launch azas_bringup lid_sticker_grip_planning.launch.py \ Press `p` only after the preview shows a stable `detected:lid` overlay and `/jarvis/lid_gripper/lid_pose` is publishing in `base_link`. + +### 강개발자 컵 뚜껑 잡고 닫기 preset + +The operator panel exposes this as `lid_grip_close` / **뚜껑 잡고 닫기**. It is a real-motion, supervised preset for the field-taught lid close sequence supplied on 2026-06-08: + +- requires Robot, RealSense, and RG2 service sessions to be up on the same `ROS_DOMAIN_ID`, +- starts `lid_sticker_grip_planning.launch.py` with ArUco marker id `14`, 30 mm marker length, visual refine, RG2 preopen/grasp widths, and hardware gates enabled, +- waits for the preview `p` trigger before motion, +- performs lid grasp/lift, transfers to the measured twist target, runs the pre-seat periodic tool motion, then closes by stepped J6 rotation. + +Safety assumptions for this preset: + +- The `lid_twist_target_*` and `lid_twist_r*` values are operator-supplied teach-point values, not generated calibration. +- The preset keeps the existing explicit hardware gate: `hardware_confirm:=ENABLE_REAL_ROBOT_MOTION` plus panel real-motion arming. +- Speed limits remain conservative for the press/transfer path (`press_velocity=5`, `transfer_velocity=25`, `turn_velocity=30`, `acceleration=15`). +- Failure behavior is fail-closed before motion if required Doosan/RG2 services are absent, if no base-link lid pose exists, if IK precheck fails, if visual refinement exceeds stability thresholds, or if post-motion verification fails. diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index c008858..568e845 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -350,9 +350,9 @@ background: #fff; box-shadow: var(--shadow); overflow: hidden; - grid-column: 2; - grid-row: 3; - max-height: 240px; + grid-column: 1 / -1; + grid-row: auto; + max-height: none; } .camera-head { display: flex; @@ -368,14 +368,14 @@ .camera-tools { display: flex; flex-wrap: wrap; align-items: center; justify-content: flex-end; gap: 8px; } .camera-status { color: var(--muted); font-size: 13px; font-weight: 850; } .camera-frame { - min-height: 160px; + min-height: 420px; display: grid; place-items: center; background: #020617; } .camera-frame img { width: 100%; - max-height: 220px; + max-height: 620px; object-fit: contain; display: block; color: #dbeafe; @@ -383,7 +383,7 @@ text-align: center; } .camera-result { - max-height: 150px; + max-height: 260px; overflow: auto; margin: 0; padding: 10px 12px; @@ -675,7 +675,7 @@ line-height: 1.45; white-space: pre-wrap; word-break: break-word; - max-height: 220px; + max-height: 620px; overflow: auto; } .step.kind-background { border-left-color: var(--purple); } @@ -704,8 +704,8 @@ .log-panel { position: static; - height: min(72vh, 760px); - min-height: 520px; + height: min(78vh, 860px); + min-height: 620px; display: grid; grid-template-rows: 48px 1fr; border: 1px solid #1e293b; @@ -799,7 +799,10 @@ } .operator-button-grid small { font-size: 11px; font-weight: 800; opacity: 0.92; } #startPrepBtn { background: #1d4ed8 !important; border-color: #1d4ed8 !important; color: #fff !important; } + #cupUprightingBtn { background: #0f766e !important; border-color: #0f766e !important; color: #fff !important; } #sideGripBtn { background: #ea580c !important; border-color: #ea580c !important; color: #fff !important; } + #pickLidBtn { background: #be123c !important; border-color: #be123c !important; color: #fff !important; } + #lidGripCloseBtn { background: #9f1239 !important; border-color: #9f1239 !important; color: #fff !important; } #colorScanJsonBtn { background: #7c3aed !important; border-color: #7c3aed !important; color: #fff !important; } #recipeCycleBtn { background: #b91c1c !important; border-color: #b91c1c !important; color: #fff !important; } #fullCocktailRealBtn { background: #047857 !important; border-color: #047857 !important; color: #fff !important; } @@ -843,18 +846,21 @@

Azas Robot Pipeline Control

- + - + + - + + + - +
@@ -894,6 +900,86 @@

Azas Robot Pipeline Control

+
+ 강개발자 뚜껑 잡고 닫기 참고 명령어 +
+
+ 뚜껑 잡고 닫기 버튼은 강개발자 로직인 lid_grip_close 단계입니다. + 이 로직은 ArUco marker ID 14로 뚜껑 pose를 잡고, RG2로 뚜껑을 파지한 뒤 컵/컵홀더 위치로 이동해서 J6 twist로 뚜껑을 닫습니다. + 디스펜서 프레스나 컵홀더 배치가 아니라 뚜껑 파지→이동→닫기 전용 실제모션입니다. + 실행 전 로봇 연결, 연결 확인, 그리퍼 연결, 카메라 연결이 필요합니다. +
+
cd /home/ssu/Azas
+source /opt/ros/humble/setup.bash
+source /home/ssu/ws_moveit/install/setup.bash
+source /home/ssu/ros2_ws/install/setup.bash
+source /home/ssu/Azas/install/setup.bash
+
+ros2 launch azas_bringup lid_sticker_grip_planning.launch.py \
+  marker_type:=aruco \
+  require_lid_detection:=false \
+  allow_aruco_only_after_grip_request:=false \
+  aruco_only_after_grip_request_sec:=20.0 \
+  aruco_dictionary:=DICT_4X4_50 \
+  aruco_marker_id:=14 \
+  aruco_marker_length_m:=0.03 \
+  use_aruco_axis_for_orientation:=true \
+  use_lid_pose_yaw_for_pick:=true \
+  visual_refine_before_grasp:=true \
+  visual_refine_sample_count:=5 \
+  visual_refine_timeout_sec:=3.0 \
+  enable_hardware:=true \
+  hardware_confirm:=ENABLE_REAL_ROBOT_MOTION \
+  allow_service_control_without_moveit:=true \
+  service_prefix:=/dsr01 \
+  enable_gripper_service_calls:=true \
+  gripper_set_service:=/jarvis/rg2/set_width \
+  gripper_preopen_width_m:=0.110 \
+  gripper_grasp_width_m:=0.020 \
+  gripper_force_n:=12.0 \
+  enable_lid_twist_after_grasp:=true \
+  lid_twist_target_x_m:=0.422959106 \
+  lid_twist_target_y_m:=0.223224869 \
+  lid_twist_target_z_m:=0.166827988 \
+  lid_twist_transfer_clearance_m:=0.12 \
+  lid_twist_force_rotation_mode:=j6 \
+  lid_twist_preseat_periodic_before_turn:=true \
+  lid_twist_rz_delta_deg:=300.0 \
+  lid_twist_turn_step_deg:=50.0 \
+  lid_twist_min_z_m:=0.140 \
+  lid_twist_max_z_m:=0.220
+
+
+ +
+ 컵홀더 배치 MoveIt 참고 명령어 +
+
+ 패널의 4. 컵홀더에 컵 옮기기는 이제 Doosan 직선 MoveLine이 아니라 MoveItPy 경로계획으로 실행됩니다. + 관절 한계로 직선 이동이 멈추는 경우를 피하기 위해 --motion-backend moveit을 사용합니다. + 좌표는 새로 만들지 않고 calibration.yamlcup_holder.side_grip_place 실측 pose만 사용합니다. +
+
cd /home/ssu/Azas
+source /opt/ros/humble/setup.bash
+source /home/ssu/ws_moveit/install/setup.bash
+source /home/ssu/ros2_ws/install/setup.bash
+source /home/ssu/Azas/install/setup.bash
+
+python3 tools/run/place_side_grip_cup_in_holder.py \
+  --service-prefix dsr01 \
+  --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml \
+  --motion-backend moveit \
+  --moveit-planning-pipeline ompl \
+  --moveit-planner-id RRTConnectkConfigDefault \
+  --moveit-planning-time-sec 8.0 \
+  --moveit-planning-attempts 5 \
+  --moveit-velocity-scaling 0.08 \
+  --moveit-acceleration-scaling 0.06 \
+  --place-final-z-offset-m -0.020 \
+  --execute --confirm ENABLE_CUP_HOLDER_PLACE
+
+
+
PR #20 side-grip 참고 명령어
@@ -917,8 +1003,8 @@

Azas Robot Pipeline Control

imgsz:=640 \ device:=cpu \ target_class:=cup \ - auto_pick:=true \ - auto_pick_interval:=3.0 \ + auto_pick:=false \ + auto_pick_interval:=8.0 \ depth_patch_radius:=7 \ min_depth_valid_ratio:=0.03 \ min_depth_m:=0.15 \ @@ -931,8 +1017,8 @@

Azas Robot Pipeline Control

side_short_stage_backoff_m:=0.08 \ side_grasp_stop_backoff_m:=0.04 \ side_close_underreach_m:=0.03 \ - side_low_retry_lift_m:=0.03 \ - side_low_retry_attempts:=5 \ + side_low_retry_lift_m:=0.0 \ + side_low_retry_attempts:=0 \ side_linear_approach_enabled:=true \ side_final_slide_enabled:=false \ side_fixed_grasp_z_enabled:=true \ @@ -940,7 +1026,7 @@

Azas Robot Pipeline Control

side_project_bbox_center_to_fixed_z:=true \ side_candidate_plan_check_enabled:=true \ side_move_to_initial_center_before_close:=false \ - verify_motion:=true \ + verify_motion:=false \ move_to_camera_home:=true \ move_joint_home_before_camera_home:=false \ camera_home_mode:=joint \ @@ -988,6 +1074,7 @@

디버그 단계

실행 로그
+
@@ -1100,6 +1187,8 @@

RealSense 카메라 화면

let itemStatuses = new Map(); let isRunning = false; let nextQueueId = 1; + let runningLogTimer = null; + let activeLogKeys = new Set(); function escapeHtml(value) { return String(value ?? "") @@ -1225,22 +1314,43 @@

RealSense 카메라 화면

} function needsCollisionScene(key) { - return key === "shake_closed_cup" + return [ + "home_robot", + "lift_robot", + "side_grip_camera_home", + "lid_view_pose", + "move_to_color_scan_pose", + "side_grip", + "lid_grip_close", + "place_cup_holder", + "shake_closed_cup", + "run_color_recipe_sequence", + ].includes(key) || key.startsWith("move_to_dispenser_") + || key.startsWith("press_dispenser_") || key.startsWith("pick_from_dispenser_"); } function withCollisionScenePrereq(queue) { const items = queue.map((item) => ({...item})); - const sideIndex = items.findIndex((item) => item.key === "side_grip"); - if (sideIndex >= 0) { - const prereqs = ["lift_robot", "start_camera"]; - for (const prereq of [...prereqs].reverse()) { - if (!items.some((item) => item.key === prereq)) { - items.splice(sideIndex, 0, {id: `q${nextQueueId++}`, key: prereq, injected: true}); + function ensureBefore(targetKey, prereqKeys) { + let targetIndex = items.findIndex((item) => item.key === targetKey); + if (targetIndex < 0) return; + for (const prereq of prereqKeys) { + const existingIndex = items.findIndex((item) => item.key === prereq); + if (existingIndex >= 0) { + items.splice(existingIndex, 1); + if (existingIndex < targetIndex) targetIndex -= 1; } } + targetIndex = items.findIndex((item) => item.key === targetKey); + prereqKeys.forEach((prereq, offset) => { + items.splice(targetIndex + offset, 0, {id: `q${nextQueueId++}`, key: prereq, injected: true}); + }); } + ensureBefore("side_grip", ["start_camera", "side_grip_camera_home"]); + ensureBefore("color_scan", ["connect_robot", "status_check", "move_to_color_scan_pose", "start_camera"]); + ensureBefore("cup_uprighting", ["connect_robot", "status_check", "start_camera"]); if (!items.some((item) => needsCollisionScene(item.key))) return items; const withoutScene = items.filter((item) => item.key !== "start_collision_scene"); return [{id: `q${nextQueueId++}`, key: "start_collision_scene", injected: true}, ...withoutScene]; @@ -1465,6 +1575,7 @@

RealSense 카메라 화면

isRunning = true; setStepStatus(key, "running", itemId); log.textContent = `바로 실행 중: ${step?.label || key}`; + startRunningLogPolling([key]); focusLog(); try { const body = payload(); @@ -1491,6 +1602,8 @@

RealSense 카메라 화면

log.textContent = String(err); } finally { isRunning = false; + activeLogKeys = new Set(); + refreshRunningLogs(true); updateSelectedCount(); } } @@ -1541,6 +1654,50 @@

RealSense 카메라 화면

: `칵테일 ${dispenserId} 블록 추가가 일부 실패했습니다: ${added}/${keys.length}`; } + function formatRunningLogs(items) { + const active = items.filter((item) => !activeLogKeys.size || activeLogKeys.has(item.key)); + if (!active.length) return ""; + return active.map((item) => { + const status = item.status || "running"; + const code = item.returncode === null || item.returncode === undefined ? "" : ` rc=${item.returncode}`; + const path = item.log_path ? `\n--- log: ${item.log_path}` : ""; + const tail = item.tail || "(아직 로그 출력 없음)"; + return `### ${item.key} pid=${item.pid} ${status}${code}${path}\n${tail}`; + }).join("\n\n"); + } + + async function refreshRunningLogs(force = false) { + if (!isRunning && !force && !runningLogTimer) return; + try { + const res = await fetch(`/api/running_logs?t=${Date.now()}`); + const data = await res.json(); + const items = Array.isArray(data.logs) ? data.logs : []; + const body = formatRunningLogs(items); + if (body) { + log.textContent = `실시간 터미널 로그 (${new Date().toLocaleTimeString()})\nstdout/stderr tail을 그대로 표시합니다.\n\n${body}`; + } else if (force) { + log.textContent = `실시간 터미널 로그 (${new Date().toLocaleTimeString()})\n현재 패널이 추적 중인 실행 프로세스 로그가 없습니다.`; + } + } catch (err) { + if (force) log.textContent = String(err); + } + } + + function startRunningLogPolling(keys = []) { + for (const key of keys) activeLogKeys.add(key); + if (runningLogTimer) return; + runningLogTimer = window.setInterval(() => refreshRunningLogs(false), 1000); + refreshRunningLogs(false); + } + + function stopRunningLogPolling() { + if (runningLogTimer) { + window.clearInterval(runningLogTimer); + runningLogTimer = null; + } + activeLogKeys = new Set(); + } + function focusLog() { logPanel.scrollIntoView({behavior: "smooth", block: "nearest"}); } @@ -1738,7 +1895,7 @@

RealSense 카메라 화면

queueOnly(["connect_gripper"], "그리퍼 연결을 큐에 추가했습니다."); }); document.getElementById("connectCameraBtn")?.addEventListener("click", () => { - queueOnly(["start_camera"], "RealSense 카메라 연결을 큐에 추가했습니다."); + queueOnly(["start_camera"], "RealSense 카메라 연결을 큐에 추가했습니다. 640x480x30 저부하 프로파일로 시작합니다."); }); document.getElementById("cameraViewBtn")?.addEventListener("click", () => { queueOnly(["start_camera_view"], "rqt_image_view 카메라 화면 보기를 큐에 추가했습니다."); @@ -1746,12 +1903,21 @@

RealSense 카메라 화면

document.getElementById("yoloDetectBtn")?.addEventListener("click", () => { queueOnly(["detect_cup_lid"], "YOLO 컵/뚜껑 인식 토픽 시작을 큐에 추가했습니다. 이 스텝은 화면 창을 띄우지 않습니다."); }); + document.getElementById("cupUprightingBtn")?.addEventListener("click", () => { + queueOnly(["connect_robot", "status_check", "start_camera", "cup_uprighting"], "소명/누운 컵 직립화 로직을 큐에 추가했습니다. 실제 모션 허용 체크가 필요합니다."); + }); document.getElementById("sideGripBtn")?.addEventListener("click", () => { - queueOnly(["connect_robot", "status_check", "connect_gripper", "start_camera", "side_grip"], "창현/PR #20 RealSense side-grip 로직을 큐에 추가했습니다."); + queueOnly(["start_camera", "side_grip_camera_home", "side_grip"], "창현/PR #20 RealSense side-grip 로직을 큐에 추가했습니다. 로봇 연결은 현재 세션을 사용하고, 카메라 홈 자세 이동 후 실행합니다."); + }); + document.getElementById("pickLidBtn")?.addEventListener("click", () => { + queueOnly(["start_camera", "pick_lid"], "뚜껑 grip pose 계획 로직을 큐에 추가했습니다. 실제 로봇 모션은 실행하지 않습니다."); + }); + document.getElementById("lidGripCloseBtn")?.addEventListener("click", () => { + queueOnly(["connect_robot", "status_check", "connect_gripper", "start_camera", "lid_view_pose", "lid_grip_close"], "강개발자 lid_grip_close를 큐에 추가했습니다. 뚜껑 보기 자세→ArUco 14 인식→RG2 파지→컵 위치 이동→J6 twist 닫기 실제모션입니다."); }); document.getElementById("colorScanJsonBtn")?.addEventListener("click", () => { - queueOnly(["connect_robot", "status_check", "move_to_color_scan_pose", "start_camera", "color_scan"], "로봇 연결 확인 → 디스펜서 카메라 조준 자세 이동 → RealSense → dispenser_color_map.json 저장을 큐에 추가했습니다."); + queueOnly(["connect_robot", "status_check", "move_to_color_scan_pose", "start_camera", "color_scan"], "로봇 연결 확인 → 검증된 색상 스캔 자세 이동 → 저부하 RealSense → 1.5초/5프레임 안정화 → 화면의 색상 핸들 직접 검출 → JSON/debug 이미지 저장을 큐에 추가했습니다."); }); document.getElementById("colorJsonCheckBtn")?.addEventListener("click", refreshColorScanResult); document.getElementById("manualColorJsonBtn")?.addEventListener("click", saveManualColorMap); @@ -1761,12 +1927,12 @@

RealSense 카메라 화면

queueOnly(["run_color_recipe_sequence"], "레시피 기반 디스펜서 사이클을 큐에 추가했습니다."); }); document.getElementById("cupHolderBtn")?.addEventListener("click", () => { - queueOnly(["place_cup_holder"], "컵홀더 배치를 큐에 추가했습니다."); + queueOnly(["place_cup_holder"], "컵홀더 배치를 큐에 추가했습니다. 이 단계는 MoveItPy 경로계획으로 pre_place→place_final→RG2 open→retreat를 실행합니다."); }); document.getElementById("fullCocktailRealBtn")?.addEventListener("click", async () => { try { await resolveRecipeDispenserIdsFromJsonOrFallback(); } catch (err) { log.textContent = String(err); focusLog(); return; } - queueOnly(["connect_robot", "status_check", "connect_gripper", "start_camera", "side_grip", "move_to_color_scan_pose", "color_scan", "run_color_recipe_sequence", "place_cup_holder"], "전체 플로우를 큐에 추가했습니다. (컵 side-grip → 색상 JSON → 레시피 사이클 → 컵홀더)", {clear: true}); + queueOnly(["connect_robot", "status_check", "connect_gripper", "start_camera", "side_grip_camera_home", "side_grip", "move_to_color_scan_pose", "color_scan", "run_color_recipe_sequence", "place_cup_holder"], "전체 플로우를 큐에 추가했습니다. (카메라 홈→컵 side-grip → 검증자세 색상 핸들 JSON → 레시피 사이클 → MoveIt 컵홀더 배치)", {clear: true}); }); document.getElementById("oneClickResultBtn")?.addEventListener("click", () => { queueOnly(["check_one_click_cocktail_result"], "결과확인을 큐에 추가했습니다."); @@ -1776,6 +1942,8 @@

RealSense 카메라 화면

const selected = withCollisionScenePrereq(selectedQueue); resetResultBadges(); isRunning = Boolean(selected.length); + activeLogKeys = new Set(selected.map((item) => item.key)); + if (isRunning) startRunningLogPolling(); renderFlow(selected[0]?.id || ""); log.textContent = selected.length ? `실행 중... (${selected.length}개 단계, 큐 순서대로 실행)` : "선택된 단계가 없습니다."; focusLog(); @@ -1785,7 +1953,8 @@

RealSense 카메라 화면

for (const [index, item] of selected.entries()) { setStepStatus(item.key, "running", item.id); renderFlow(item.id); - log.textContent = `실행 중 ${index + 1}/${selected.length}: ${stepByKey(item.key)?.label || item.key}`; + log.textContent = `실행 중 ${index + 1}/${selected.length}: ${stepByKey(item.key)?.label || item.key}\n로그를 불러오는 중...`; + refreshRunningLogs(false); const body = payload(); body.selected = [item.key]; const res = await fetch("/api/run", { @@ -1822,12 +1991,15 @@

RealSense 카메라 화면

log.textContent = String(err); } finally { isRunning = false; + activeLogKeys = new Set(); + refreshRunningLogs(true); updateSelectedCount(); } }); document.getElementById("stop").addEventListener("click", async () => { focusLog(); + stopRunningLogPolling(); const res = await fetch("/api/stop", {method: "POST"}); log.textContent = JSON.stringify(await res.json(), null, 2); }); @@ -1837,6 +2009,12 @@

RealSense 카메라 화면

const res = await fetch("/api/cleanup", {method: "POST"}); log.textContent = JSON.stringify(await res.json(), null, 2); }); + document.getElementById("liveLogBtn")?.addEventListener("click", () => { + activeLogKeys = new Set(); + startRunningLogPolling(); + refreshRunningLogs(true); + focusLog(); + }); document.getElementById("clearLog").addEventListener("click", () => { log.textContent = ""; }); document.getElementById("commandModalClose").addEventListener("click", closeCommandModal); commandModalBackdrop.addEventListener("click", (event) => { diff --git a/src/azas_bringup/config/calibration.yaml b/src/azas_bringup/config/calibration.yaml index 511209c..1012a42 100644 --- a/src/azas_bringup/config/calibration.yaml +++ b/src/azas_bringup/config/calibration.yaml @@ -18,9 +18,9 @@ hand_eye: rpy_rad: null # 확인 필요: fill from npy after deciding canonical file # Saved robot pose for dispenser color scanning. -# 직접 측정한 디스펜서 색상 스캔 포즈 (2026-06-05). +# 2026-06-08 현재 카메라 화면에서 디스펜서 핸들이 보이고 visible-handle 색상 검출이 통과한 실측 자세. color_scan_pose: - source: operator_measured + source: operator_measured_visible_handle_verified ee_link: link_6 joints_deg: [0.0, 10.0, 32.0, 0.0, 100.0, 90.0] joints_rad: [0.0, 0.1745, 0.5585, 0.0, 1.7453, 1.5708] diff --git a/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz b/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz index 0391e93..c028c71 100644 --- a/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz +++ b/src/azas_bringup/rviz/azas_cocktail_collision_preview.rviz @@ -27,6 +27,16 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/workspace_collision/markers + Name: Workspace Safety Walls/Floor + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: diff --git a/src/azas_bringup/rviz/azas_dispenser_sequence.rviz b/src/azas_bringup/rviz/azas_dispenser_sequence.rviz index b620a17..8fe33f7 100644 --- a/src/azas_bringup/rviz/azas_dispenser_sequence.rviz +++ b/src/azas_bringup/rviz/azas_dispenser_sequence.rviz @@ -29,6 +29,16 @@ Visualization Manager: Update Interval: 0 Value: false Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/workspace_collision/markers + Name: Workspace Safety Walls/Floor + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: diff --git a/src/azas_bringup/rviz/azas_dispenser_sequence_clean.rviz b/src/azas_bringup/rviz/azas_dispenser_sequence_clean.rviz index c8405d4..922e74c 100644 --- a/src/azas_bringup/rviz/azas_dispenser_sequence_clean.rviz +++ b/src/azas_bringup/rviz/azas_dispenser_sequence_clean.rviz @@ -27,6 +27,16 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/workspace_collision/markers + Name: Workspace Safety Walls/Floor + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: false Marker Topic: diff --git a/src/azas_bringup/rviz/color_scan_pose.rviz b/src/azas_bringup/rviz/color_scan_pose.rviz index 1952e61..03b0ec0 100644 --- a/src/azas_bringup/rviz/color_scan_pose.rviz +++ b/src/azas_bringup/rviz/color_scan_pose.rviz @@ -29,6 +29,16 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/workspace_collision/markers + Name: Workspace Safety Walls/Floor + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Marker Topic: diff --git a/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz b/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz index adf0f00..6a05ca7 100644 --- a/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz +++ b/src/azas_bringup/rviz/link6_gripper_tcp_debug.rviz @@ -29,6 +29,16 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Marker Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /azas/workspace_collision/markers + Name: Workspace Safety Walls/Floor + Value: true - Class: rviz_default_plugins/RobotModel Description Topic: Depth: 5 diff --git a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py index 912cdd6..c2b90ac 100644 --- a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py +++ b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py @@ -1,4 +1,6 @@ from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare @@ -26,7 +28,27 @@ def generate_launch_description(): [FindPackageShare("azas_cup_uprighting"), "config", "moveit_py.yaml"] ) - # 3. 컵 직립화(Uprighting) 노드 실행 및 파라미터 주입 + # 3. 공통 안전/충돌 장면: side-grip, dispenser, cup-uprighting이 같은 바닥/벽/디스펜서 기준을 보도록 통일 + workspace_collision_scene = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare("azas_bringup"), + "launch", + "workspace_collision_scene.launch.py", + ]) + ), + launch_arguments={ + "publish_collision_objects": "true", + "table_collision_enabled": "true", + "table_collision_expand_to_workspace_walls": "true", + "workspace_boundary_collision_enabled": "true", + "dispenser_collision_enabled": "true", + "dispenser_collision_publish_objects": "true", + "dispenser_collision_publish_markers": "true", + }.items(), + ) + + # 4. 컵 직립화(Uprighting) 노드 실행 및 파라미터 주입 yolo_cup_uprighting_node = Node( package="azas_cup_uprighting", executable="yolo_cup_uprighting", @@ -38,4 +60,4 @@ def generate_launch_description(): ], ) - return LaunchDescription([yolo_cup_uprighting_node]) \ No newline at end of file + return LaunchDescription([workspace_collision_scene, yolo_cup_uprighting_node]) \ No newline at end of file diff --git a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py index 87ad676..e63bab4 100644 --- a/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py +++ b/src/azas_motion/azas_motion/measured_dispenser_collision_scene_node.py @@ -18,6 +18,7 @@ DEFAULT_CONFIG_PATH = ( "/home/ssu/Azas/src/azas_bringup/config/measured_dispenser_collision.yaml" ) +DEFAULT_CALIBRATION_PATH = "/home/ssu/Azas/src/azas_bringup/config/calibration.yaml" LEGACY_DISPENSER_COLLISION_OBJECT_IDS = ( "dispenser_body_box", @@ -129,6 +130,7 @@ def __init__(self) -> None: super().__init__("measured_dispenser_collision_scene_node") self.declare_parameter("config_path", DEFAULT_CONFIG_PATH) + self.declare_parameter("calibration_path", DEFAULT_CALIBRATION_PATH) self.declare_parameter("publish_period_sec", 2.0) self.declare_parameter("publish_collision_objects", True) self.declare_parameter("publish_markers", True) @@ -143,6 +145,10 @@ def __init__(self) -> None: self.get_parameter("config_path").get_parameter_value().string_value ) self.config = self._load_config(config_path) + calibration_path = Path( + self.get_parameter("calibration_path").get_parameter_value().string_value + ) + self.calibration = self._load_optional_calibration(calibration_path) self.frame_id = self.config.get("metadata", {}).get("frame_id", "base_link") self.collision_pub = self.create_publisher( @@ -218,6 +224,24 @@ def _load_config(self, config_path: Path) -> dict[str, Any]: self.get_logger().info(f"Loaded measured dispenser collision config: {config_path}") return config + def _load_optional_calibration(self, calibration_path: Path) -> dict[str, Any]: + if not calibration_path.exists(): + self.get_logger().warning( + f"calibration config does not exist; dispenser outlet markers disabled: {calibration_path}" + ) + return {} + with calibration_path.open("r", encoding="utf-8") as stream: + calibration = yaml.safe_load(stream) or {} + if not isinstance(calibration, dict): + self.get_logger().warning( + f"calibration config is not a YAML map; outlet markers disabled: {calibration_path}" + ) + return {} + self.get_logger().info( + f"Loaded dispenser outlet/press marker calibration: {calibration_path}" + ) + return calibration + def _warn_about_draft_status(self) -> None: metadata = self.config.get("metadata", {}) status = str(metadata.get("status", "unknown")) @@ -445,6 +469,56 @@ def _make_markers( label.text = hold_name markers.append(label) + outlet_marker_id = 3000 + outlets = self.calibration.get("dispenser_outlets", {}) + if isinstance(outlets, dict): + for dispenser_id in sorted(outlets.keys(), key=lambda x: int(x) if str(x).isdigit() else str(x)): + outlet_config = outlets.get(dispenser_id) or {} + if not isinstance(outlet_config, dict): + continue + for field_name, namespace, color in ( + ("outlet_pose_xyz_m", "measured_dispenser_outlet_points", (0.0, 1.0, 0.0, 0.95)), + ("press_pose_xyz_m", "measured_dispenser_press_points", (1.0, 0.0, 0.9, 0.95)), + ): + point = outlet_config.get(field_name) + if point is None: + continue + marker = Marker() + marker.header.frame_id = self.frame_id + marker.header.stamp = stamp + marker.ns = namespace + marker.id = outlet_marker_id + outlet_marker_id += 1 + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.pose = _pose(point, [0.0, 0.0, 0.0, 1.0]) + marker.scale.x = 0.025 + marker.scale.y = 0.025 + marker.scale.z = 0.025 + marker.color.r = color[0] + marker.color.g = color[1] + marker.color.b = color[2] + marker.color.a = color[3] + markers.append(marker) + + label = Marker() + label.header.frame_id = self.frame_id + label.header.stamp = stamp + label.ns = f"{namespace}_labels" + label.id = outlet_marker_id + outlet_marker_id += 1 + label.type = Marker.TEXT_VIEW_FACING + label.action = Marker.ADD + label.pose = _pose(point, [0.0, 0.0, 0.0, 1.0]) + label.pose.position.z += 0.045 + label.scale.z = 0.03 + label.color.r = color[0] + label.color.g = color[1] + label.color.b = color[2] + label.color.a = 1.0 + label.text = f"dispenser_{dispenser_id}_{field_name.replace('_xyz_m', '')}" + markers.append(label) + return MarkerArray(markers=markers) diff --git a/src/azas_motion/azas_motion/workspace_collision_scene_node.py b/src/azas_motion/azas_motion/workspace_collision_scene_node.py index e0d6960..88f10ad 100644 --- a/src/azas_motion/azas_motion/workspace_collision_scene_node.py +++ b/src/azas_motion/azas_motion/workspace_collision_scene_node.py @@ -9,6 +9,7 @@ import yaml from geometry_msgs.msg import Pose from moveit_msgs.msg import CollisionObject +from visualization_msgs.msg import Marker, MarkerArray from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy @@ -140,6 +141,9 @@ def __init__(self) -> None: self.collision_pub = self.create_publisher( CollisionObject, "/collision_object", transient_qos(10) ) + self.marker_pub = self.create_publisher( + MarkerArray, "/azas/workspace_collision/markers", transient_qos(10) + ) self._publish_scene() period = self.get_parameter("publish_period_sec").get_parameter_value().double_value @@ -278,7 +282,73 @@ def _workspace_wall_collision_objects(self) -> list[CollisionObject]: ), ] + + def _make_box_marker( + self, + marker_id: int, + object_id: str, + center_xyz: list[float], + size_xyz: list[float], + color_rgba: tuple[float, float, float, float], + ) -> Marker: + marker = Marker() + marker.header.frame_id = self.frame_id + marker.ns = "azas_workspace_collision" + marker.id = marker_id + marker.type = Marker.CUBE + marker.action = Marker.ADD + marker.pose.position.x = float(center_xyz[0]) + marker.pose.position.y = float(center_xyz[1]) + marker.pose.position.z = float(center_xyz[2]) + marker.pose.orientation.w = 1.0 + marker.scale.x = float(size_xyz[0]) + marker.scale.y = float(size_xyz[1]) + marker.scale.z = float(size_xyz[2]) + marker.color.r = float(color_rgba[0]) + marker.color.g = float(color_rgba[1]) + marker.color.b = float(color_rgba[2]) + marker.color.a = float(color_rgba[3]) + marker.text = object_id + return marker + + def _make_workspace_markers(self) -> MarkerArray: + markers = MarkerArray() + marker_id = 0 + table = self._table_collision_object() + if table is not None and table.primitives and table.primitive_poses: + primitive = table.primitives[0] + pose = table.primitive_poses[0] + markers.markers.append( + self._make_box_marker( + marker_id, + table.id, + [pose.position.x, pose.position.y, pose.position.z], + list(primitive.dimensions), + (0.05, 0.70, 0.20, 0.35), + ) + ) + marker_id += 1 + + for collision_object in self._workspace_wall_collision_objects(): + if not collision_object.primitives or not collision_object.primitive_poses: + continue + primitive = collision_object.primitives[0] + pose = collision_object.primitive_poses[0] + markers.markers.append( + self._make_box_marker( + marker_id, + collision_object.id, + [pose.position.x, pose.position.y, pose.position.z], + list(primitive.dimensions), + (0.10, 0.45, 1.00, 0.28), + ) + ) + marker_id += 1 + return markers + def _publish_scene(self) -> None: + self.marker_pub.publish(self._make_workspace_markers()) + if not self.publish_collision_objects: return diff --git a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py index 82289b8..5f6f8de 100644 --- a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -1454,6 +1454,8 @@ def wait_until_gripper_idle(self, timeout_sec=GRIPPER_OPEN_TIMEOUT_SEC): return False def open_gripper_max(self, wait=False): + if not self.wait_until_gripper_idle(): + return False self.get_logger().info( f"Open gripper to max width={GRIPPER_OPEN_WIDTH} " f"({GRIPPER_OPEN_WIDTH / 10.0:.1f} mm)" @@ -1902,6 +1904,15 @@ def execute_side_grasp_plan(self, plan: SideGraspPlan): ): return False + log.info("open gripper at outside high side-staging pose") + if not self.open_gripper_max(wait=True): + return False + if self.gripper_open_settle_sec > 0.0: + log.info( + f"wait {self.gripper_open_settle_sec:.2f}s for RG2 full-open before low approach" + ) + time.sleep(self.gripper_open_settle_sec) + active_pre_z = None for attempt in range(self.side_low_retry_attempts + 1): try_pre_z = plan.pre_z + attempt * self.side_low_retry_lift_m @@ -1984,12 +1995,6 @@ def pick_and_place_side(self, base_xyz): f"close=({candidate.guarded_grasp_xy[0]:.3f}, {candidate.guarded_grasp_xy[1]:.3f}, {candidate.pre_z:.3f})" ) - self.open_gripper_max(wait=False) - if self.gripper_open_settle_sec > 0.0: - log.info( - f"wait {self.gripper_open_settle_sec:.2f}s for RG2 full-open before low approach" - ) - time.sleep(self.gripper_open_settle_sec) if not self.move_to_side_prepose_if_configured(cup_base): return False if not self.move_joint1_clearance_before_side_grip(): diff --git a/tools/perception/dispenser_color_scan.py b/tools/perception/dispenser_color_scan.py index 2a39889..9800300 100644 --- a/tools/perception/dispenser_color_scan.py +++ b/tools/perception/dispenser_color_scan.py @@ -12,7 +12,9 @@ from __future__ import annotations import argparse +import itertools import json +import math import sys from pathlib import Path @@ -41,6 +43,18 @@ CROP_HALF_PX = 60 # half-side of crop box around projected pixel +# HSV ranges for the physical dispenser handle colors in the current booth. +# This is intentionally image-space only: it does not create robot poses or +# calibration values. When the handles are visible, left-to-right order maps to +# dispenser IDs 1..4. +VISIBLE_HANDLE_HSV_RANGES = { + "red": ((0, 80, 60, 10, 255, 255), (170, 80, 60, 179, 255, 255)), + "yellow": ((20, 80, 60, 40, 255, 255),), + "green": ((40, 60, 50, 85, 255, 255),), + "blue": ((85, 80, 60, 130, 255, 255),), +} + + def load_dispenser_ids() -> list[str]: """Return dispenser IDs from calibration.yaml, falling back to 1-4.""" try: @@ -115,6 +129,127 @@ def classify_image_file(path: Path) -> str: return result.color +def detect_visible_handle_color_map( + frame_bgr: "np.ndarray", + dispenser_ids: list[str], + *, + debug_image_path: Path | None = None, +) -> dict[str, str] | None: + """Detect colored dispenser handles directly from the camera image. + + The earlier TF projection path can be wrong if hand-eye/camera extrinsics are + stale, even when the handles are plainly visible. This fallback uses only + the visible colored handle blobs and assigns IDs by horizontal order. + """ + if cv2 is None: + return None + import numpy as np # type: ignore + + img_h, img_w = frame_bgr.shape[:2] + hsv = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2HSV) + candidates_by_color: dict[str, list[tuple[float, str, int, int, int, int, float, float, float]]] = {} + min_area = max(150.0, float(img_w * img_h) * 0.00035) + min_w = max(8, int(round(img_w * 0.012))) + min_h = max(20, int(round(img_h * 0.055))) + max_w = max(80, int(round(img_w * 0.140))) + max_h = max(90, int(round(img_h * 0.240))) + + for color, ranges in VISIBLE_HANDLE_HSV_RANGES.items(): + mask = np.zeros((img_h, img_w), dtype=np.uint8) + for lo_h, lo_s, lo_v, hi_h, hi_s, hi_v in ranges: + mask |= cv2.inRange( + hsv, + np.array([lo_h, lo_s, lo_v], dtype=np.uint8), + np.array([hi_h, hi_s, hi_v], dtype=np.uint8), + ) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((3, 3), dtype=np.uint8)) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((7, 7), dtype=np.uint8)) + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + color_candidates: list[tuple[float, str, int, int, int, int, float, float, float]] = [] + for contour in contours: + area = float(cv2.contourArea(contour)) + x, y, w, h = cv2.boundingRect(contour) + # Booth-specific visual gate: handles are vertical colored blobs in + # the upper/middle image, not the operator clothes, chairs, or cup. + if area < min_area or w < min_w or h < min_h or w > max_w or h > max_h: + continue + if not (0.06 * img_h <= y <= 0.42 * img_h): + continue + if not (0.25 * img_w <= x <= 0.90 * img_w): + continue + score = area + float(h) * 10.0 + center_x = float(x) + float(w) * 0.5 + center_y = float(y) + float(h) * 0.5 + color_candidates.append((score, color, x, y, w, h, area, center_x, center_y)) + color_candidates.sort(key=lambda item: item[0], reverse=True) + if color_candidates: + candidates_by_color[color] = color_candidates[:6] + + if len(candidates_by_color) != len(dispenser_ids): + print( + f"[dispenser_color_scan] visible-handle fallback found {len(candidates_by_color)}/{len(dispenser_ids)} " + "colored handles; falling back to TF projection", + file=sys.stderr, + ) + return None + + # One large false-positive blob can beat the real handle by area (chairs, + # clothes, or table reflection). The four dispenser handles are physically + # on one horizontal row, so choose the one-candidate-per-color combination + # with the best row consistency instead of blindly taking max area per color. + best_combo: tuple[float, tuple[tuple[float, str, int, int, int, int, float, float, float], ...]] | None = None + for combo in itertools.product(*(candidates_by_color[color] for color in sorted(candidates_by_color))): + centers_x = [item[7] for item in combo] + if len(set(round(x) for x in centers_x)) != len(combo): + continue + centers_y = [item[8] for item in combo] + mean_y = sum(centers_y) / float(len(centers_y)) + row_std = math.sqrt(sum((y - mean_y) ** 2 for y in centers_y) / float(len(centers_y))) + area_score = sum(item[6] for item in combo) + score = area_score - 200.0 * row_std + if best_combo is None or score > best_combo[0]: + best_combo = (score, combo) + + if best_combo is None: + print("[dispenser_color_scan] visible-handle fallback could not choose a non-overlapping color row", file=sys.stderr) + return None + + candidates = list(best_combo[1]) + candidates.sort(key=lambda item: item[2]) + color_map = {did: color for did, (_, color, *_rest) in zip(dispenser_ids, candidates)} + if debug_image_path is not None: + debug = frame_bgr.copy() + palette = { + "red": (0, 0, 255), + "yellow": (0, 255, 255), + "green": (0, 255, 0), + "blue": (255, 0, 0), + } + for did, (_, color, x, y, w, h, area, *_centers) in zip(dispenser_ids, candidates): + bgr = palette.get(color, (255, 255, 255)) + cv2.rectangle(debug, (x, y), (x + w, y + h), bgr, 2) + cv2.putText( + debug, + f"{did}:{color} {int(area)}", + (x, max(y - 8, 18)), + cv2.FONT_HERSHEY_SIMPLEX, + 0.55, + bgr, + 2, + cv2.LINE_AA, + ) + debug_image_path.parent.mkdir(parents=True, exist_ok=True) + cv2.imwrite(str(debug_image_path), debug) + print(f"[dispenser_color_scan] debug image saved: {debug_image_path}") + debug = ", ".join( + f"{did}={color}@box({x},{y},{w},{h})" + for did, (_, color, x, y, w, h, _area, *_centers) in zip(dispenser_ids, candidates) + ) + print(f"[dispenser_color_scan] visible-handle fallback: {debug}") + return color_map + + def scan_from_image_dir(image_dir: Path) -> dict[str, str]: dispenser_ids = load_dispenser_ids() color_map: dict[str, str] = {} @@ -133,7 +268,14 @@ def scan_from_image_dir(image_dir: Path) -> dict[str, str]: return color_map -def scan_from_ros() -> dict[str, str]: +def scan_from_ros( + *, + clamp_out_of_frame: bool = True, + visible_handle_fallback: bool = True, + settle_sec: float = 1.5, + sample_frames: int = 5, + debug_image_path: Path | None = None, +) -> dict[str, str]: try: import rclpy # type: ignore from rclpy.qos import qos_profile_sensor_data # type: ignore @@ -148,17 +290,9 @@ def scan_from_ros() -> dict[str, str]: import time - T_gripper2cam = load_hand_eye() - if T_gripper2cam is None: - print("[dispenser_color_scan] ERROR: could not load T_gripper2camera.npy", file=sys.stderr) - sys.exit(1) - - dispenser_positions = load_dispenser_positions() - if not dispenser_positions: - print("[dispenser_color_scan] ERROR: no dispenser positions in calibration.yaml", file=sys.stderr) - sys.exit(1) - frame_bgr = None + frame_count = 0 + first_frame_time: float | None = None cam_info = None def to_bgr(msg: "Image") -> "np.ndarray": @@ -176,9 +310,14 @@ def to_bgr(msg: "Image") -> "np.ndarray": raise RuntimeError(f"unsupported encoding: {msg.encoding}") def image_cb(msg: "Image") -> None: - nonlocal frame_bgr - if frame_bgr is None: - frame_bgr = to_bgr(msg) + nonlocal frame_bgr, frame_count, first_frame_time + now = time.time() + if first_frame_time is None: + first_frame_time = now + if now - first_frame_time < settle_sec: + return + frame_bgr = to_bgr(msg) + frame_count += 1 def info_cb(msg: "CameraInfo") -> None: nonlocal cam_info @@ -191,11 +330,11 @@ def info_cb(msg: "CameraInfo") -> None: node.create_subscription(Image, CAMERA_TOPIC, image_cb, qos_profile_sensor_data) node.create_subscription(CameraInfo, CAMERA_INFO_TOPIC, info_cb, qos_profile_sensor_data) - deadline = time.time() + 8.0 + deadline = time.time() + 8.0 + max(settle_sec, 0.0) try: while rclpy.ok() and time.time() < deadline: rclpy.spin_once(node, timeout_sec=0.1) - if frame_bgr is not None and cam_info is not None: + if frame_bgr is not None and cam_info is not None and frame_count >= max(sample_frames, 1): break finally: pass # keep node alive for TF lookup below @@ -209,6 +348,34 @@ def info_cb(msg: "CameraInfo") -> None: print(f"[dispenser_color_scan] no camera_info from {CAMERA_INFO_TOPIC} within 8s", file=sys.stderr) sys.exit(1) + dispenser_ids = load_dispenser_ids() + print( + f"[dispenser_color_scan] using stabilized frame: " + f"settle_sec={settle_sec:.2f} sample_frames={frame_count} size={frame_bgr.shape[1]}x{frame_bgr.shape[0]}" + ) + if visible_handle_fallback: + visible_map = detect_visible_handle_color_map( + frame_bgr, + dispenser_ids, + debug_image_path=debug_image_path, + ) + if visible_map is not None: + node.destroy_node() + rclpy.shutdown() + return visible_map + + T_gripper2cam = load_hand_eye() + if T_gripper2cam is None: + node.destroy_node(); rclpy.shutdown() + print("[dispenser_color_scan] ERROR: could not load T_gripper2camera.npy", file=sys.stderr) + sys.exit(1) + + dispenser_positions = load_dispenser_positions() + if not dispenser_positions: + node.destroy_node(); rclpy.shutdown() + print("[dispenser_color_scan] ERROR: no dispenser positions in calibration.yaml", file=sys.stderr) + sys.exit(1) + # Get TF: base_link → link_6 (EE) T_base2ee = None try: @@ -255,9 +422,27 @@ def info_cb(msg: "CameraInfo") -> None: y1 = max(0, v - CROP_HALF_PX) y2 = min(img_h, v + CROP_HALF_PX) if x2 <= x1 or y2 <= y1: - print(f"[dispenser_color_scan] dispenser {did}: projected pixel ({u},{v}) out of frame {img_w}x{img_h}", file=sys.stderr) - color_map[did] = "unknown" - continue + if clamp_out_of_frame: + clamped_u = min(max(u, 0), img_w - 1) + clamped_v = min(max(v, 0), img_h - 1) + x1 = max(0, clamped_u - CROP_HALF_PX) + x2 = min(img_w, clamped_u + CROP_HALF_PX) + y1 = max(0, clamped_v - CROP_HALF_PX) + y2 = min(img_h, clamped_v + CROP_HALF_PX) + if x2 > x1 and y2 > y1: + print( + f"[dispenser_color_scan] dispenser {did}: projected pixel ({u},{v}) out of frame {img_w}x{img_h}; " + f"using edge crop around ({clamped_u},{clamped_v})", + file=sys.stderr, + ) + else: + print(f"[dispenser_color_scan] dispenser {did}: projected pixel ({u},{v}) out of frame {img_w}x{img_h}", file=sys.stderr) + color_map[did] = "unknown" + continue + else: + print(f"[dispenser_color_scan] dispenser {did}: projected pixel ({u},{v}) out of frame {img_w}x{img_h}", file=sys.stderr) + color_map[did] = "unknown" + continue crop = frame_bgr[y1:y2, x1:x2] result = classify_bgr_crop(crop) color_map[did] = result.color @@ -271,6 +456,11 @@ def main() -> int: parser.add_argument("--image-dir", default="", help="Directory with dispenser_1.png ~ dispenser_4.png") parser.add_argument("--output", default=str(DEFAULT_OUTPUT), help="Output JSON path") parser.add_argument("--ros", action="store_true", help="Capture from ROS camera topic") + parser.add_argument("--no-clamp-out-of-frame", action="store_true", help="Do not classify edge crop when projected dispenser pixel is just outside the image") + parser.add_argument("--no-visible-handle-fallback", action="store_true", help="Disable visible colored-handle detection and use only TF projection") + parser.add_argument("--settle-sec", type=float, default=1.5, help="Seconds to ignore camera frames before color classification") + parser.add_argument("--sample-frames", type=int, default=5, help="Number of stabilized frames to receive before classifying the latest one") + parser.add_argument("--debug-image", default="", help="Optional path to save visible-handle debug overlay") args = parser.parse_args() if not args.image_dir and not args.ros: @@ -281,7 +471,13 @@ def main() -> int: if args.image_dir: color_map = scan_from_image_dir(Path(args.image_dir)) else: - color_map = scan_from_ros() + color_map = scan_from_ros( + clamp_out_of_frame=not args.no_clamp_out_of_frame, + visible_handle_fallback=not args.no_visible_handle_fallback, + settle_sec=max(args.settle_sec, 0.0), + sample_frames=max(args.sample_frames, 1), + debug_image_path=Path(args.debug_image) if args.debug_image else None, + ) unknown_ids = [did for did, color in color_map.items() if str(color).lower() == "unknown"] if unknown_ids: diff --git a/tools/run/dispenser_color_scan_ros.sh b/tools/run/dispenser_color_scan_ros.sh index 33a3f8f..a768ae2 100755 --- a/tools/run/dispenser_color_scan_ros.sh +++ b/tools/run/dispenser_color_scan_ros.sh @@ -1,7 +1,8 @@ #!/usr/bin/env bash # 디스펜서 색상 스캔 (ROS 모드). -# 로봇이 color_scan_pose (joints [0,10,32,0,100,90]°)에 있어야 합니다. -# 카메라, TF, 로봇 드라이버가 실행 중이어야 합니다. +# 로봇이 color_scan_pose (joints [0,10,32,0,100,90]°)에 있으면 +# 카메라 화면의 색상 핸들을 직접 검출해 왼쪽→오른쪽을 1→4번으로 저장합니다. +# TF 투영은 visible-handle 검출 실패 시 보조 경로로만 사용됩니다. set -euo pipefail ROOT="$(cd "$(dirname "$0")/../.." && pwd)" @@ -21,4 +22,7 @@ source_setup /opt/ros/humble/setup.bash source_setup "$ROOT/install/local_setup.bash" python3 "$ROOT/tools/perception/dispenser_color_scan.py" --ros \ + --settle-sec "${AZAS_COLOR_SCAN_SETTLE_SEC:-1.5}" \ + --sample-frames "${AZAS_COLOR_SCAN_SAMPLE_FRAMES:-5}" \ + --debug-image "$ROOT/outputs/dispenser_color_scan_debug.jpg" \ --output "$ROOT/outputs/dispenser_color_map.json" diff --git a/tools/run/place_side_grip_cup_in_holder.py b/tools/run/place_side_grip_cup_in_holder.py index 7808a36..ce99e79 100755 --- a/tools/run/place_side_grip_cup_in_holder.py +++ b/tools/run/place_side_grip_cup_in_holder.py @@ -11,6 +11,7 @@ import argparse import math import os +import time import subprocess import sys from dataclasses import dataclass @@ -95,6 +96,107 @@ def print_target(target: TargetPose) -> None: ) +def quaternion_from_rpy_rad(roll: float, pitch: float, yaw: float): + from geometry_msgs.msg import Quaternion + + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = Quaternion() + q.w = cr * cp * cy + sr * sp * sy + q.x = sr * cp * cy - cr * sp * sy + q.y = cr * sp * cy + sr * cp * sy + q.z = cr * cp * sy - sr * sp * cy + return q + + +def pose_stamped_from_target(target: TargetPose, frame_id: str): + from geometry_msgs.msg import PoseStamped + + pose = PoseStamped() + pose.header.frame_id = frame_id + pose.pose.position.x = target.xyz_m[0] + pose.pose.position.y = target.xyz_m[1] + pose.pose.position.z = target.xyz_m[2] + pose.pose.orientation = quaternion_from_rpy_rad(*target.rpy_rad) + return pose + + +def plan_and_execute_moveit_pose(robot, arm, params, target: TargetPose, *, args: argparse.Namespace) -> int: + pose = pose_stamped_from_target(target, args.moveit_frame_id) + arm.set_start_state_to_current_state() + arm.set_goal_state(pose_stamped_msg=pose, pose_link=args.moveit_ee_link) + print( + f"[Azas] MoveIt plan step={target.label}: " + f"xyz_m=[{target.xyz_m[0]:.6f}, {target.xyz_m[1]:.6f}, {target.xyz_m[2]:.6f}] " + f"pipeline={args.moveit_planning_pipeline} planner={args.moveit_planner_id}" + ) + sys.stdout.flush() + result = arm.plan(parameters=params) + if not result: + print(f"[FAIL] MoveIt planning failed for {target.label}") + return 1 + if not args.execute: + print(f"[DRY-RUN] MoveIt plan succeeded for {target.label}; --execute not set.") + return 0 + print(f"[Azas] MoveIt execute step={target.label}") + sys.stdout.flush() + ok = robot.execute( + group_name=args.moveit_planning_group, + robot_trajectory=result.trajectory, + blocking=True, + ) + if ok is False: + print(f"[FAIL] MoveIt execution failed for {target.label}") + return 1 + time.sleep(max(args.moveit_waypoint_hold_sec, 0.0)) + return 0 + + +def run_moveit_sequence( + targets: list[TargetPose], + *, + args: argparse.Namespace, +) -> int: + try: + import rclpy + from moveit.planning import MoveItPy, PlanRequestParameters + from azas_motion.side_grasp_ik_preview_node import moveit_config_dict + except Exception as exc: + print(f"[FAIL] MoveIt imports failed: {exc}") + return 1 + + rclpy.init(args=None) + try: + robot = MoveItPy( + node_name="azas_cup_holder_place_moveit_py", + config_dict=moveit_config_dict(args.moveit_robot_model, args.moveit_config_package), + provide_planning_service=False, + ) + arm = robot.get_planning_component(args.moveit_planning_group) + params = PlanRequestParameters(robot) + params.planning_pipeline = args.moveit_planning_pipeline + params.planner_id = args.moveit_planner_id + params.planning_time = args.moveit_planning_time_sec + params.planning_attempts = args.moveit_planning_attempts + params.max_velocity_scaling_factor = args.moveit_velocity_scaling + params.max_acceleration_scaling_factor = args.moveit_acceleration_scaling + if args.moveit_settle_sec > 0.0: + print(f"[Azas] Waiting {args.moveit_settle_sec:.1f}s for MoveIt/controller state to settle") + time.sleep(args.moveit_settle_sec) + for target in targets: + rc = plan_and_execute_moveit_pose(robot, arm, params, target, args=args) + if rc != 0: + return rc + return 0 + finally: + rclpy.shutdown() + + def run_movel( target: TargetPose, *, @@ -202,6 +304,25 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--gripper-open-width-m", type=float, default=0.110) parser.add_argument("--gripper-open-force-n", type=float, default=12.0) parser.add_argument("--gripper-timeout-sec", type=float, default=12.0) + parser.add_argument( + "--motion-backend", + choices=("direct", "moveit"), + default="direct", + help="direct uses Doosan MoveLine services; moveit plans through MoveItPy before execution.", + ) + parser.add_argument("--moveit-frame-id", default="base_link") + parser.add_argument("--moveit-ee-link", default="link_6") + parser.add_argument("--moveit-planning-group", default="manipulator") + parser.add_argument("--moveit-robot-model", default="m0609") + parser.add_argument("--moveit-config-package", default="dsr_moveit_config_m0609") + parser.add_argument("--moveit-planning-pipeline", default="ompl") + parser.add_argument("--moveit-planner-id", default="RRTConnectkConfigDefault") + parser.add_argument("--moveit-planning-time-sec", type=float, default=8.0) + parser.add_argument("--moveit-planning-attempts", type=int, default=5) + parser.add_argument("--moveit-velocity-scaling", type=float, default=0.08) + parser.add_argument("--moveit-acceleration-scaling", type=float, default=0.06) + parser.add_argument("--moveit-settle-sec", type=float, default=3.0) + parser.add_argument("--moveit-waypoint-hold-sec", type=float, default=0.5) parser.add_argument("--execute", action="store_true") parser.add_argument( "--confirm", @@ -239,30 +360,43 @@ def main() -> int: if not args.execute: print("[DRY-RUN] --execute not set; no robot or gripper command will be sent.") - steps = [ - (pre_place, args.approach_velocity, args.approach_acceleration), - (place_final, args.place_velocity, args.place_acceleration), - ] - for target, velocity, acceleration in steps: - rc = run_movel(target, args=args, velocity=velocity, acceleration=acceleration) + if args.motion_backend == "moveit": + print("[Azas] motion_backend=moveit: planning cup-holder transfer with MoveItPy") + rc = run_moveit_sequence([pre_place, place_final], args=args) if rc != 0: - print(f"[FAIL] {target.label} MoveLine failed; aborting sequence.") + print("[FAIL] MoveIt cup-holder approach/place failed; gripper open skipped.") return rc + else: + steps = [ + (pre_place, args.approach_velocity, args.approach_acceleration), + (place_final, args.place_velocity, args.place_acceleration), + ] + for target, velocity, acceleration in steps: + rc = run_movel(target, args=args, velocity=velocity, acceleration=acceleration) + if rc != 0: + print(f"[FAIL] {target.label} MoveLine failed; aborting sequence.") + return rc rc = run_gripper_open(args) if rc != 0: print("[FAIL] gripper open failed; retreat skipped to avoid dragging the cup.") return rc - rc = run_movel( - retreat, - args=args, - velocity=args.retreat_velocity, - acceleration=args.retreat_acceleration, - ) - if rc != 0: - print("[FAIL] retreat MoveLine failed.") - return rc + if args.motion_backend == "moveit": + rc = run_moveit_sequence([retreat], args=args) + if rc != 0: + print("[FAIL] MoveIt retreat failed.") + return rc + else: + rc = run_movel( + retreat, + args=args, + velocity=args.retreat_velocity, + acceleration=args.retreat_acceleration, + ) + if rc != 0: + print("[FAIL] retreat MoveLine failed.") + return rc print("[PASS] cup holder side-grip place sequence completed") return 0 diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 1e6bc67..5908afb 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -377,10 +377,10 @@ class Step: "start_collision_scene", "MoveIt 충돌 장면 시작", "background", - "measured_dispenser_collision_scene_node + tumbler_collision_scene_node", + "workspace_collision_scene.launch.py + rg2_link6_tcp.launch.py + tumbler_collision_scene_node", True, False, - "디스펜서 박스와 감지 텀블러를 /collision_object로 publish; direct Doosan 명령은 아직 이 장면을 자동 회피에 쓰지 않음", + "safety.yaml 바닥/양쪽 벽, measured dispenser 박스, link_6 부착 RG2 그리퍼 envelope, 감지 텀블러를 PlanningScene/RViz로 publish", ), Step( "rviz_cocktail_collision_preview", @@ -446,14 +446,23 @@ class Step: True, "기본 카메라 보기 관절 자세: [0, 10, 32, 0, 100, 90]°", ), + Step( + "side_grip_camera_home", + "side-grip 카메라 홈 자세", + "run", + "tools/run/direct_movej_joints.py --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0", + True, + True, + "창현 side-grip 노드의 camera_home_mode:=joint 기본 관절 자세로 이동해 컵 인식 시야를 맞춤", + ), Step( "move_to_color_scan_pose", - "색상 스캔/카메라 보기 포즈 이동 [0,10,32,0,100,90]°", + "색상 스캔 검증 포즈 이동 [visible-handle]", "run", "tools/run/direct_movej_joints.py --j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90 --velocity 30 --acceleration 30 --execute --confirm ENABLE_DIRECT_MOVEJ", True, True, - "색상 스캔 전 기본 카메라 보기 포즈로 이동. color_scan_pose: [0,10,32,0,100,90]°", + "색상 스캔 전 2026-06-08 검증 포즈로 이동. 카메라 화면에서 보이는 디스펜서 핸들 색을 직접 검출할 수 있는 자세", ), Step( "rviz_color_scan_pose_preview", @@ -471,7 +480,7 @@ class Step: "tools/run/dispenser_color_scan_ros.sh", True, False, - "카메라+TF로 디스펜서 1~4 색상을 판별해 outputs/dispenser_color_map.json 저장. 로봇이 color_scan_pose [0,10,32,0,100,90]°에 있어야 함", + "카메라 화면의 visible colored handle blob을 직접 검출해 왼쪽→오른쪽을 디스펜서 1~4로 매핑하고 outputs/dispenser_color_map.json 저장. TF 투영은 보조 경로", ), Step("voice_input", "음성 입력 (STT+LLM 노드 시작)", "background", "ros2 launch azas_voice azas_voice.launch.py", True, False, "STT → /stt_result → llm_recipe_mapper → /azas/voice/recipe_decision"), Step( @@ -496,11 +505,20 @@ class Step: "side_grip", "PR #20 RealSense 컵 인식 후 side grip", "background", - "ros2 launch dsr_practice yolo_cup_pick_node.launch.py auto_pick:=true grasp_mode:=side moveit_controller_name:=/dsr01/dsr_moveit_controller", + "ros2 launch dsr_practice yolo_cup_pick_node.launch.py auto_pick:=false grasp_mode:=side moveit_controller_name:=/dsr01/dsr_moveit_controller", True, True, "auto_pick=true: 컵 감지 즉시 자동 side-grip. 서버 command_for()가 실제 파라미터를 오버라이드함. 패널에서 OpenCV 창에서 확인 후 ESC 종료", ), + Step( + "cup_uprighting", + "소명 누운 컵 세우기 / cup uprighting", + "run", + "ros2 launch azas_cup_uprighting yolo_cup_uprighting.launch.py", + True, + True, + "RealSense + YOLO 기반 누운 컵 직립화. 실제 로봇 모션이며 side-grip 전에 선택 실행", + ), Step("gripper_soft_grasp", "그리퍼 살짝 잡기", "run", "ros2 service call /jarvis/rg2/set_width azas_interfaces/srv/SetGripper", True, True, "큰 컵용: 완전 close 대신 폭 75mm/약한 힘으로 살짝 오므림"), Step( "move_to_dispenser_1", @@ -629,6 +647,24 @@ class Step: False, "YOLO lid + 빨간 원형 스티커 + depth 평면으로 base_link lid pose와 approach/grasp/lift 후보를 발행. 실제 로봇 모션은 실행하지 않음", ), + Step( + "lid_view_pose", + "뚜껑 보기 카메라 자세", + "run", + "tools/run/direct_movej_joints.py --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0", + True, + True, + "강개발자 lid_grip_close 실행 전 손목 카메라가 뚜껑/ArUco를 보도록 이동. 현재는 side-grip 카메라 홈과 동일한 검증 후보 자세", + ), + Step( + "lid_grip_close", + "컵 뚜껑 잡고 닫기 / ArUco lid twist", + "background", + "", + True, + True, + "강개발자 로직: ArUco 14 뚜껑 pose를 p키로 확정한 뒤 RG2 파지→lift→teach point 이동→J6 단계 회전으로 뚜껑을 닫음", + ), Step( "place_cup_holder", "컵을 컵홀더에 놓기 / side grip", @@ -664,6 +700,9 @@ class Step: "ros2_control_node", "controller_manager", "joint_state_broadcaster", + "joint_state_relay_legacy", + "dsr_practice/joint_state_relay", + "joint_state_relay --ros-args", "robot_state_publisher", "virtual_node", "move_group", @@ -688,6 +727,23 @@ class Step: "m0609_shake_joint_state_node", "measured_dispenser_collision_scene_node", "tumbler_collision_scene_node", + "link6_gripper_collision_node", + "rg2_link6_tcp.launch.py", + "azas_rg2_link6_tcp_state_publisher", + "static_transform_publisher", + "--frame-id world --child-frame-id base_link", +) + +COLLISION_SCENE_STACK_PATTERNS = ( + "workspace_collision_scene.launch.py", + "workspace_collision_scene_node", + "measured_dispenser_collision_scene_node", + "tumbler_collision_scene_node", + "link6_gripper_collision_node", + "rg2_link6_tcp.launch.py", + "azas_rg2_link6_tcp_state_publisher", + "static_transform_publisher", + "--frame-id world --child-frame-id base_link", ) RG2_STACK_PATTERNS = ( @@ -939,6 +995,28 @@ def cleanup_side_grip_stack(*, grace_sec: float = 2.0) -> list[str]: return events +def cleanup_collision_scene_stack(*, grace_sec: float = 2.0) -> list[str]: + """Replace stale PlanningScene publishers before starting a shared scene. + + The operator relies on one consistent safety scene for table, walls, + dispenser, detected tumbler, and the RG2 envelope attached to link_6. Stale + duplicate scene publishers make RViz/MoveIt hard to reason about, so the + panel restarts this stack as a single unit. + """ + events: list[str] = [] + old = processes.pop("start_collision_scene", None) + if old is not None: + events.extend(terminate_process_tree(old, label="stored start_collision_scene", grace_sec=grace_sec)) + events.extend( + cleanup_matching_processes( + COLLISION_SCENE_STACK_PATTERNS, + label="collision-scene cleanup", + grace_sec=grace_sec, + ) + ) + return events + + def cleanup_run_step_stack(*, grace_sec: float = 3.0) -> list[str]: """Best-effort cleanup of stale one-shot motion/ROS CLI commands. @@ -1269,7 +1347,25 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: f"/{clean}/motion/check_motion", f"/{clean}/system/get_robot_state", ] - if step.key in {"home_robot", "lift_robot", "move_to_color_scan_pose"}: + if step.key == "lid_view_pose": + return [ + f"/{clean}/motion/move_joint", + f"/{clean}/motion/check_motion", + f"/{clean}/system/get_robot_state", + ] + if step.key == "lid_grip_close": + return [ + "/jarvis/rg2/set_width", + f"/{clean}/motion/move_line", + f"/{clean}/motion/move_joint", + f"/{clean}/motion/move_periodic", + f"/{clean}/motion/ikin", + f"/{clean}/motion/check_motion", + f"/{clean}/system/get_robot_state", + f"/{clean}/aux_control/get_current_posj", + f"/{clean}/aux_control/get_current_posx", + ] + if step.key in {"home_robot", "lift_robot", "side_grip_camera_home", "move_to_color_scan_pose"}: return [ f"/{clean}/motion/move_joint", f"/{clean}/motion/check_motion", @@ -1364,9 +1460,10 @@ def required_service_wait_timeout(step: Step) -> float: or step.key.startswith("pick_from_dispenser_") or step.key == "run_color_recipe_sequence" or step.key == "place_cup_holder" + or step.key == "lid_grip_close" ): return 35.0 - if step.key in {"home_robot", "lift_robot", "move_to_color_scan_pose", "side_grip", "shake_closed_cup"}: + if step.key in {"home_robot", "lift_robot", "side_grip_camera_home", "lid_view_pose", "move_to_color_scan_pose", "side_grip", "shake_closed_cup"}: return 30.0 if step.key == "gripper_soft_grasp": return 12.0 @@ -1449,13 +1546,16 @@ def wait_for_collision_object_sample( timeout_sec: float = 10.0, proc: subprocess.Popen[str] | None = None, ) -> tuple[bool, str]: - """Wait until the MoveIt collision scene publisher emits at least one object.""" + """Wait until workspace objects and the link_6 gripper attachment are visible.""" deadline = time.monotonic() + max(timeout_sec, 0.1) - last_output = "" + last_collision_output = "" + last_attached_output = "" + saw_collision = False + saw_attached_gripper = False while time.monotonic() < deadline: if proc is not None and proc.poll() is not None: return False, "collision scene process exited while waiting\n" + tail_file(process_logs.get("start_collision_scene")) - result = subprocess.run( + collision_result = subprocess.run( ["bash", "-lc", "timeout 2s ros2 topic echo /collision_object --once"], cwd=str(ROOT), env=env, @@ -1465,13 +1565,42 @@ def wait_for_collision_object_sample( timeout=3.0, check=False, ) - last_output = result.stdout[-2000:] - if result.returncode == 0 and "id:" in result.stdout: - return True, "collision object sample observed on /collision_object\n" + last_output + last_collision_output = collision_result.stdout[-2000:] + if collision_result.returncode == 0 and "id:" in collision_result.stdout: + saw_collision = True + + attached_result = subprocess.run( + ["bash", "-lc", "timeout 2s ros2 topic echo /attached_collision_object --once"], + cwd=str(ROOT), + env=env, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + timeout=3.0, + check=False, + ) + last_attached_output = attached_result.stdout[-2000:] + if ( + attached_result.returncode == 0 + and "azas_rg2_gripper_on_link6" in attached_result.stdout + and "link_name: link_6" in attached_result.stdout + ): + saw_attached_gripper = True + + if saw_collision and saw_attached_gripper: + return ( + True, + "collision object sample observed on /collision_object\n" + + last_collision_output + + "\nattached RG2 gripper envelope observed on /attached_collision_object\n" + + last_attached_output, + ) time.sleep(0.5) return False, ( - f"no collision object sample observed on /collision_object within {timeout_sec:.1f}s\n" - f"--- last output ---\n{last_output}" + f"scene readiness incomplete within {timeout_sec:.1f}s " + f"(workspace={saw_collision}, link6_gripper={saw_attached_gripper})\n" + f"--- last /collision_object ---\n{last_collision_output}\n" + f"--- last /attached_collision_object ---\n{last_attached_output}" ) @@ -1753,7 +1882,16 @@ def ensure_gripper_services(step: Step, payload: dict[str, Any], service_prefix: def requires_doosan_motion(step: Step) -> bool: return ( - step.key in {"home_robot", "lift_robot", "side_grip", "shake_closed_cup"} + step.key + in { + "home_robot", + "lift_robot", + "side_grip_camera_home", + "lid_view_pose", + "side_grip", + "lid_grip_close", + "shake_closed_cup", + } or step.key.startswith("move_to_dispenser_") or step.key.startswith("press_dispenser_") or step.key.startswith("pick_from_dispenser_") @@ -1911,9 +2049,25 @@ def target_xyz_for_step(step_key: str) -> list[float] | None: def requires_collision_scene_step(key: str) -> bool: return ( - key == "shake_closed_cup" + key in { + # Direct joint/line motions still need the shared PlanningScene + # visible and current in RViz/operator review. Some of these + # commands do not consume MoveIt collisions directly, but every + # real robot task should run in the same table/wall/dispenser scene. + "home_robot", + "lift_robot", + "side_grip_camera_home", + "lid_view_pose", + "move_to_color_scan_pose", + "side_grip", + "lid_grip_close", + "place_cup_holder", + "shake_closed_cup", + "run_color_recipe_sequence", + } or key == "run_color_recipe_sequence" or key.startswith("move_to_dispenser_") + or key.startswith("press_dispenser_") or key.startswith("pick_from_dispenser_") ) @@ -1924,18 +2078,34 @@ def with_collision_scene_prereq(selected: list[str]) -> list[str]: def ensure_before(target: str, prerequisites: list[str]) -> None: if target not in ordered: return + # Keep prerequisites exactly once and immediately before the target. + # This prevents stale UI/manual selections from producing orders like + # start_camera -> connect_robot -> ... -> start_camera -> target. + target_index = ordered.index(target) for prereq in prerequisites: if prereq in ordered: - continue - target_index = ordered.index(target) - ordered.insert(target_index, prereq) + prereq_index = ordered.index(prereq) + ordered.pop(prereq_index) + if prereq_index < target_index: + target_index -= 1 + target_index = ordered.index(target) + for offset, prereq in enumerate(prerequisites): + ordered.insert(target_index + offset, prereq) # PR #20 side-grip is the cup acquisition step. Make the real-motion and # perception prerequisites explicit, but do not move already-queued steps; # this preserves the operator's full-flow order. ensure_before( "side_grip", - ["connect_robot", "status_check", "connect_gripper", "start_camera", "lift_robot"], + ["start_camera", "side_grip_camera_home"], + ) + ensure_before( + "cup_uprighting", + ["connect_robot", "status_check", "start_camera"], + ) + ensure_before( + "lid_grip_close", + ["connect_robot", "status_check", "connect_gripper", "start_camera", "lid_view_pose"], ) # Color classification must aim the robot at the measured color-scan pose @@ -1970,6 +2140,12 @@ def ensure_before(target: str, prerequisites: list[str]) -> None: def run_timeout_for_step(step: Step) -> float: if step.key == "side_grip": return 900.0 + if step.key == "cup_uprighting": + return 900.0 + if step.key == "side_grip_camera_home": + return 180.0 + if step.key == "lid_grip_close": + return 900.0 if step.key == "run_color_recipe_sequence": return 1200.0 if step.key == "run_one_click_cocktail_real": @@ -2020,6 +2196,9 @@ def shell_env(payload: dict[str, Any]) -> dict[str, str]: or DEFAULT_RT_HOST ) env["DOOSAN_REAL_MOTION_CONFIRM"] = "ENABLE_DOOSAN_REAL_MOTION_BRINGUP" + # Panel-run Python scripts should flush logs while the browser polls + # /api/running_logs; otherwise operators only see output after completion. + env["PYTHONUNBUFFERED"] = "1" return env @@ -2047,12 +2226,30 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe or os.environ.get("RT_HOST") or DEFAULT_RT_HOST ) - return ( - f"cd {ROOT} && ROBOT_HOST={shlex.quote(robot_host)} " + relay_script_path = ROOT / "src" / "dsr_practice" / "dsr_practice" / "joint_state_relay.py" + relay_input = f"/{robot_name.strip('/') or 'dsr01'}/joint_states" + relay_script = ( + "sleep 12; " + f"cd {shlex.quote(str(ROOT))} && {ROS_SETUP}; " + f"if [ -f {shlex.quote(str(relay_script_path))} ]; then " + "echo '[Azas] starting RViz /joint_states relay " + f"{relay_input} -> /joint_states'; " + f"python3 {shlex.quote(str(relay_script_path))} --ros-args " + f"-p input_topic:={shlex.quote(relay_input)} " + "-p output_topic:=/joint_states; " + "else echo '[WARN] joint_state_relay.py not found; RViz may not mirror real robot joints'; fi" + ) + bringup_script = ( + f"cd {shlex.quote(str(ROOT))} && " + f"ROBOT_HOST={shlex.quote(robot_host)} " f"ROBOT_NAME={shlex.quote(robot_name)} RT_HOST={shlex.quote(rt_host)} " "DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP " - f"{step.command}" + f"{step.command} & " + "bringup_pid=$!; " + f"( {relay_script} ) & " + "wait $bringup_pid" ) + return f"bash -lc {shlex.quote(bringup_script)}" if step.key == "status_check": clean = service_prefix.strip("/") or "dsr01" return ( @@ -2068,7 +2265,21 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"timeout 9s python3 {shlex.quote(str(ROOT / 'tools' / 'run' / 'ros_call_empty_service.py'))} " f"/{clean}/motion/check_motion dsr_msgs2/srv/CheckMotion --timeout 8.0 && " "echo '--- trajectory action ---' && " - f"ros2 action info /{clean}/dsr_moveit_controller/follow_joint_trajectory" + f"ros2 action info /{clean}/dsr_moveit_controller/follow_joint_trajectory && " + "echo '--- rviz joint_states relay sample ---' && " + "(timeout 3s ros2 topic echo /joint_states --once || " + "echo '[WARN] no /joint_states sample; RViz robot model may stay frozen even while /dsr01/joint_states moves') && " + "echo '--- lid/ArUco package executables ---' && " + "(ros2 pkg executables azas_perception | grep -E 'lid_sticker_detector_node|hand_eye_static_tf_node' || " + "echo '[WARN] azas_perception lid/hand-eye executables not visible') && " + "(ros2 pkg executables azas_motion | grep -E 'lid_grip_planner_node' || " + "echo '[WARN] azas_motion lid_grip_planner_node executable not visible') && " + "echo '--- lid/ArUco runtime nodes ---' && " + "(ros2 node list | grep -E '/lid_sticker_detector_node|/lid_detection_pose_bridge_node|/lid_grip_planner_node' || " + "echo '[INFO] lid_grip_close nodes are not running yet') && " + "echo '--- lid/ArUco topic samples ---' && " + "(timeout 2s ros2 topic echo /jarvis/lid_gripper/status --once || echo '[INFO] no /jarvis/lid_gripper/status sample yet') && " + "(timeout 2s ros2 topic echo /jarvis/lid_gripper/lid_pose --once || echo '[INFO] no /jarvis/lid_gripper/lid_pose sample yet')" ) if step.key == "run_color_recipe_sequence": recipe_dispenser_ids = str(payload.get("recipe_dispenser_ids") or "").strip() @@ -2153,6 +2364,24 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "--j5-min-deg -135 --j5-max-deg 135 --timeout-sec 60 " "--execute --confirm ENABLE_DIRECT_MOVEJ" ) + if step.key == "side_grip_camera_home": + return ( + f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/direct_movej_joints.py " + f"--service-prefix {service_prefix} " + "--j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 " + "--velocity 20 --acceleration 20 " + "--j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 " + "--execute --confirm ENABLE_DIRECT_MOVEJ" + ) + if step.key == "lid_view_pose": + return ( + f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/direct_movej_joints.py " + f"--service-prefix {service_prefix} " + "--j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 " + "--velocity 15 --acceleration 15 " + "--j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 " + "--execute --confirm ENABLE_DIRECT_MOVEJ" + ) if step.key == "home_robot": return ( f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/direct_movej_joints.py " @@ -2184,18 +2413,34 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe if step.key == "start_collision_scene": return ( f"cd {ROOT} && {ROS_SETUP} && " - "python3 -m azas_motion.measured_dispenser_collision_scene_node & " + "(" + "ros2 launch azas_bringup workspace_collision_scene.launch.py " + "publish_collision_objects:=true " + "table_collision_enabled:=true " + "workspace_boundary_collision_enabled:=true " + "table_collision_expand_to_workspace_walls:=true " + "dispenser_collision_enabled:=true " + "dispenser_collision_publish_objects:=true " + "dispenser_collision_publish_markers:=true & " + "ros2 launch azas_bringup rg2_link6_tcp.launch.py " + "publish_gripper_collision:=true & " + "ros2 run tf2_ros static_transform_publisher " + "--x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 " + "--frame-id world --child-frame-id base_link & " "python3 -m azas_motion.tumbler_collision_scene_node --ros-args " "-p action:=publish_detected " "-p object_id:=detected_tumbler " "-p use_lidded_height:=true" + ")" ) if step.key == "start_camera": return ( f"cd {ROOT} && {ROS_SETUP} && " "ros2 launch realsense2_camera rs_launch.py " "camera_name:=camera " - "enable_color:=true enable_depth:=true align_depth.enable:=true" + "enable_color:=true enable_depth:=true align_depth.enable:=true " + "rgb_camera.color_profile:=640x480x30 " + "depth_module.depth_profile:=640x480x30" ) if step.key == "start_camera_view": return ( @@ -2211,9 +2456,132 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"cd {ROOT} && {ROS_SETUP} && " "ros2 launch azas_bringup lid_sticker_grip_planning.launch.py" ) + if step.key == "lid_grip_close": + clean_prefix = service_prefix if str(service_prefix).startswith("/") else f"/{service_prefix}" + model_path = DEFAULT_YOLO_MODEL_PATH if DEFAULT_YOLO_MODEL_PATH.is_file() else Path("/home/ssu/Downloads/best.pt") + return ( + f"cd {ROOT} && {ROS_SETUP} && " + "ros2 pkg executables azas_perception | grep -q '^azas_perception lid_sticker_detector_node$' && " + "ros2 pkg executables azas_motion | grep -q '^azas_motion lid_grip_planner_node$' || " + "{ echo '[FAIL] lid/ArUco executables missing; build azas_perception azas_motion azas_bringup first'; exit 1; }; " + "DISPLAY=${DISPLAY:-:0} " + "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " + "ros2 launch azas_bringup lid_sticker_grip_planning.launch.py " + f"model_path:={shlex.quote(str(model_path))} " + "marker_type:=aruco " + "require_lid_detection:=false " + "allow_aruco_only_after_grip_request:=false " + "aruco_only_after_grip_request_sec:=20.0 " + "aruco_dictionary:=DICT_4X4_50 " + "aruco_marker_id:=14 " + "aruco_marker_length_m:=0.03 " + "use_aruco_axis_for_orientation:=true " + "aruco_finger_axis_quarter_turns:=0 " + "use_lid_pose_yaw_for_pick:=true " + "lid_pose_yaw_axis:=y " + "lid_pose_yaw_offset_deg:=0.0 " + "lid_pose_yaw_equivalence_deg:=180.0 " + "visual_refine_before_grasp:=true " + "visual_refine_sample_count:=5 " + "visual_refine_timeout_sec:=3.0 " + "visual_refine_max_yaw_std_deg:=3.0 " + "visual_refine_max_position_std_m:=0.005 " + "visual_refine_apply_xy:=true " + "visual_refine_apply_yaw:=true " + "visual_refine_fallback_to_initial_plan:=true " + "enable_hardware:=true " + "hardware_confirm:=ENABLE_REAL_ROBOT_MOTION " + "allow_service_control_without_moveit:=true " + f"service_prefix:={shlex.quote(clean_prefix)} " + "rx:=108.41 " + "ry:=-176.32 " + "rz:=175.98 " + "offset_axis:=base_z " + "surface_offset_m:=0.0 " + "tcp_grasp_offset_x_m:=0.0 " + "tcp_grasp_offset_y_m:=0.0 " + "tcp_grasp_offset_z_m:=-0.040 " + "min_grasp_z_m:=0.025 " + "approach_offset_m:=0.08 " + "lift_offset_m:=0.10 " + "settle_seconds_before_grasp:=0.5 " + "hold_seconds_after_grasp:=3.0 " + "line_velocity:=30.0 " + "line_acceleration:=10.0 " + "move_timeout_sec:=90.0 " + "enable_gripper_service_calls:=true " + "gripper_set_service:=/jarvis/rg2/set_width " + "gripper_preopen_width_m:=0.110 " + "gripper_grasp_width_m:=0.020 " + "gripper_force_n:=12.0 " + "continue_after_gripper_grasp_failure:=true " + "gripper_grasp_failure_wait_sec:=2.0 " + "enable_lid_twist_after_grasp:=true " + "lid_twist_target_x_m:=0.422959106 " + "lid_twist_target_y_m:=0.223224869 " + "lid_twist_target_z_m:=0.166827988 " + "lid_twist_rx:=73.901489 " + "lid_twist_ry:=-178.542740 " + "lid_twist_rz:=117.385612 " + "lid_twist_transfer_clearance_m:=0.12 " + "lid_twist_transfer_max_z_m:=0.60 " + "lid_twist_use_force_control:=false " + "lid_twist_force_rotation_mode:=j6 " + "lid_twist_preseat_periodic_before_turn:=true " + "lid_twist_preseat_periodic_x_amp_mm:=0.0 " + "lid_twist_preseat_periodic_y_amp_mm:=0.0 " + "lid_twist_preseat_periodic_z_amp_mm:=1.0 " + "lid_twist_preseat_periodic_rx_amp_deg:=0.0 " + "lid_twist_preseat_periodic_ry_amp_deg:=0.0 " + "lid_twist_preseat_periodic_rz_amp_deg:=10.0 " + "lid_twist_preseat_periodic_period_sec:=3.6 " + "lid_twist_preseat_periodic_acc_time_sec:=1.0 " + "lid_twist_preseat_periodic_repeat:=2 " + "lid_twist_preseat_periodic_ref:=tool " + "lid_twist_rz_delta_deg:=300.0 " + "lid_twist_turn_step_deg:=50.0 " + "lid_twist_release_lift_m:=0.03 " + "lid_twist_min_z_m:=0.140 " + "lid_twist_max_z_m:=0.220 " + "lid_twist_transfer_velocity:=25.0 " + "lid_twist_press_velocity:=5.0 " + "lid_twist_turn_velocity:=30.0 " + "lid_twist_acceleration:=15.0 " + "lid_twist_hold_seconds_before_turn:=0.0 " + "lid_twist_hold_seconds_after_turn:=0.5" + ) + if step.key == "cup_uprighting": + return ( + f"cd {ROOT} && {ROS_SETUP} && " + "if ! ros2 pkg prefix azas_cup_uprighting >/dev/null 2>&1 || " + "! ros2 pkg executables azas_perception | grep -q '^azas_perception hand_eye_static_tf_node$'; then " + "echo '[Azas] cup_uprighting/hand_eye 실행파일이 없어 필요한 패키지를 빌드합니다.'; " + "colcon build --symlink-install --packages-select azas_perception azas_bringup azas_cup_uprighting || exit 1; " + "source install/setup.bash; " + "fi && " + "ros2 pkg executables azas_perception | grep -q '^azas_perception hand_eye_static_tf_node$' || " + "{ echo '[Azas] hand_eye_static_tf_node still missing after build'; exit 1; }; " + f"ros2 launch azas_cup_uprighting yolo_cup_uprighting.launch.py " + f"service_prefix:={shlex.quote(service_prefix)} " + "enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION " + "run_yolo:=true publish_hand_eye_tf:=true" + ) if step.key == "voice_input": return f"cd {ROOT} && {ROS_SETUP} && ros2 launch azas_voice azas_voice.launch.py" if step.key == "side_grip": + relay_script_path = ROOT / "src" / "dsr_practice" / "dsr_practice" / "joint_state_relay.py" + side_grip_prefix = ( + "ros2 run tf2_ros static_transform_publisher " + "--x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 " + "--frame-id world --child-frame-id base_link & " + "(sleep 5; " + f"if [ -f {shlex.quote(str(relay_script_path))} ]; then " + "echo '[Azas] starting side_grip RViz /joint_states relay /dsr01/joint_states -> /joint_states'; " + f"python3 {shlex.quote(str(relay_script_path))} --ros-args " + "-p input_topic:=/dsr01/joint_states -p output_topic:=/joint_states; " + "else echo '[WARN] joint_state_relay.py not found; RViz may not mirror real robot joints'; fi" + ") & " + ) return ( f"cd {ROOT} && " "source /opt/ros/humble/setup.bash && " @@ -2225,6 +2593,8 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"source {shlex.quote(str(ROOT / 'install' / 'local_setup.bash'))} && " f"source {shlex.quote(str(ROOT / 'install' / 'dsr_practice' / 'share' / 'dsr_practice' / 'package.bash'))} && " f"export PYTHONPATH={shlex.quote(str(ROOT / 'tools' / 'run' / 'python_compat'))}:${{PYTHONPATH:-}} && " + "(" + f"{side_grip_prefix}" "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " f"ros2 launch {shlex.quote(str(ROOT / 'install' / 'dsr_practice' / 'share' / 'dsr_practice' / 'launch' / 'yolo_cup_pick_node.launch.py'))} " @@ -2233,8 +2603,8 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "imgsz:=640 " "device:=cpu " "target_class:=cup " - "auto_pick:=true " - "auto_pick_interval:=3.0 " + "auto_pick:=false " + "auto_pick_interval:=8.0 " "depth_patch_radius:=7 " "min_depth_valid_ratio:=0.03 " "min_depth_m:=0.15 " @@ -2247,8 +2617,8 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "side_short_stage_backoff_m:=0.08 " "side_grasp_stop_backoff_m:=0.04 " "side_close_underreach_m:=0.03 " - "side_low_retry_lift_m:=0.03 " - "side_low_retry_attempts:=5 " + "side_low_retry_lift_m:=0.0 " + "side_low_retry_attempts:=0 " "side_linear_approach_enabled:=true " "side_final_slide_enabled:=false " "side_fixed_grasp_z_enabled:=true " @@ -2256,7 +2626,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "side_project_bbox_center_to_fixed_z:=true " "side_candidate_plan_check_enabled:=true " "side_move_to_initial_center_before_close:=false " - "verify_motion:=true " + "verify_motion:=false " "move_to_camera_home:=true " "move_joint_home_before_camera_home:=false " "camera_home_mode:=joint " @@ -2264,6 +2634,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "workspace_xy_clamp_enabled:=false " "return_home_after_task:=false " "return_to_camera_home_after_attempt:=true " + "workspace_collision_scene_enabled:=true " "table_collision_enabled:=true " "table_surface_z:=0.0 " "table_thickness:=0.04 " @@ -2271,10 +2642,15 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "table_size_y:=0.65 " "table_center_x:=0.29 " "table_center_y:=0.0 " + "table_collision_expand_to_workspace_walls:=true " + "workspace_boundary_collision_enabled:=true " "dispenser_collision_enabled:=true " + "dispenser_collision_publish_objects:=true " + "dispenser_collision_publish_markers:=true " f"dispenser_collision_config_path:={shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'config' / 'measured_dispenser_collision.yaml'))} " "moveit_controller_name:=/dsr01/dsr_moveit_controller " - f"start_joint_state_relay:={'true' if installed_executable('dsr_practice', 'joint_state_relay') else 'false'}" + "start_joint_state_relay:=false" + ")" ) if step.key == "gripper_soft_grasp": return ( @@ -2406,7 +2782,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "--pregrasp-offset-z-m 0.060 --pregrasp-staging-velocity 12.0 " "--pregrasp-staging-acceleration 20.0 --joint1-clearance-deg 0.0 " "--lift-m 0.100 --lift-velocity 18.0 --lift-acceleration 24.0 " - "--timeout-sec 120 --wait-service-sec 8 --verify-timeout-sec 45 " + "--timeout-sec 120 --wait-service-sec 15 --verify-timeout-sec 45 " "--target-tolerance-mm 15 --gripper-grasp-width-m 0.075 --gripper-force-n 25.0 " "--x-min 0.10 --x-max 0.95 " "--execute --confirm ENABLE_PICK_FROM_MEASURED_DISPENSER_FRONT_HOLD" @@ -2423,6 +2799,10 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/place_side_grip_cup_in_holder.py " f"--service-prefix {service_prefix} " "--config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml " + "--motion-backend moveit " + "--moveit-planning-pipeline ompl --moveit-planner-id RRTConnectkConfigDefault " + "--moveit-planning-time-sec 8.0 --moveit-planning-attempts 5 " + "--moveit-velocity-scaling 0.08 --moveit-acceleration-scaling 0.06 " "--approach-velocity 15.0 --approach-acceleration 20.0 " f"--place-final-z-offset-m {shlex.quote(place_final_z_offset_m)} " "--place-velocity 6.0 --place-acceleration 10.0 " @@ -2682,14 +3062,25 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: time.sleep(1.0) restart_output = "\n".join(cleanup_events) elif step.key == "start_camera": + camera_ready, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=3.0) + if camera_ready: + return { + "key": step.key, + "status": "running", + "output": "이미 RealSense 카메라 토픽이 살아있어 재시작하지 않습니다.\n" + camera_output, + } cleanup_events = cleanup_camera_stack() # Avoid duplicate /camera/camera nodes from previous panel attempts. - time.sleep(1.0) + time.sleep(0.4) restart_output = "\n".join(cleanup_events) elif step.key == "side_grip": # Cleanup and PR #20 preflight already ran above. Do not repeat it here; # repeated cleanup sleeps were making the manual picker feel frozen. restart_output = preflight_output + elif step.key == "start_collision_scene": + cleanup_events = cleanup_collision_scene_stack(grace_sec=2.0) + time.sleep(0.5) + restart_output = "\n".join(cleanup_events) else: old = processes.get(step.key) if old and old.poll() is None: @@ -2818,21 +3209,47 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: } try: - completed = subprocess.run( + timeout_sec = run_timeout_for_step(step) + log_path = background_log_path(step.key) + log_handle = log_path.open("w", encoding="utf-8", buffering=1) + log_handle.write(f"[Azas panel] command: {cmd}\n\n") + proc = subprocess.Popen( ["bash", "-lc", cmd], cwd=str(ROOT), env=env, - input="ENABLE_REAL_ROBOT_MOTION\n", - stdout=subprocess.PIPE, + stdin=subprocess.PIPE, + stdout=log_handle, stderr=subprocess.STDOUT, text=True, - timeout=run_timeout_for_step(step), - check=False, + start_new_session=True, ) - output = completed.stdout + processes[step.key] = proc + process_logs[step.key] = log_path + if proc.stdin is not None: + try: + proc.stdin.write("ENABLE_REAL_ROBOT_MOTION\n") + proc.stdin.close() + except OSError: + pass + deadline = time.monotonic() + timeout_sec + while proc.poll() is None: + if time.monotonic() >= deadline: + terminate_process_tree(proc, label=step.key, grace_sec=3.0) + try: + log_handle.close() + except OSError: + pass + output = tail_file(log_path) + if preflight_output: + output = f"{preflight_output}\n--- command output ---\n{output}" + return {"key": step.key, "status": "timeout", "output": output} + time.sleep(0.25) + log_handle.close() + output = tail_file(log_path, max_chars=50000) + completed_returncode = proc.returncode if preflight_output: output = f"{preflight_output}\n--- command output ---\n{output}" - if completed.returncode == 0: + if completed_returncode == 0: failure = run_output_failure(step, output) if failure is not None: output = f"{output}\n{failure}\n" @@ -2890,16 +3307,42 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: } return { "key": step.key, - "status": "passed" if completed.returncode == 0 else "failed", - "returncode": completed.returncode, + "status": "passed" if completed_returncode == 0 else "failed", + "returncode": completed_returncode, "output": output, } - except subprocess.TimeoutExpired as exc: - output = text_output(exc.stdout) + except OSError as exc: + output = f"[Azas] command launch failed: {exc}" if preflight_output: output = f"{preflight_output}\n--- command output ---\n{output}" - return {"key": step.key, "status": "timeout", "output": output} + return {"key": step.key, "status": "failed", "output": output} + + + +def running_log_snapshot(*, max_chars: int = 10000) -> list[dict[str, Any]]: + """Return live tails for processes launched by this panel. + + Foreground `/api/run` steps also register their temporary log file while the + request is still running, so the browser can poll this endpoint instead of + showing only "실행 중". + """ + + snapshots: list[dict[str, Any]] = [] + for key, proc in list(processes.items()): + log_path = process_logs.get(key) + status = "running" if proc.poll() is None else "exited" + snapshots.append( + { + "key": key, + "pid": proc.pid, + "status": status, + "returncode": proc.returncode, + "log_path": str(log_path) if log_path else "", + "tail": tail_file(log_path, max_chars=max_chars), + } + ) + return snapshots def stop_all() -> dict[str, Any]: stopped: list[dict[str, Any]] = [] @@ -2916,6 +3359,7 @@ def cleanup_all_processes() -> dict[str, Any]: events: list[str] = [] events.extend(cleanup_run_step_stack(grace_sec=3.0)) events.extend(cleanup_side_grip_stack(grace_sec=3.0)) + events.extend(cleanup_collision_scene_stack(grace_sec=3.0)) events.extend(cleanup_camera_stack(grace_sec=3.0)) events.extend(cleanup_doosan_stack(grace_sec=3.0)) events.extend( @@ -2932,11 +3376,16 @@ def cleanup_all_processes() -> dict[str, Any]: class Handler(BaseHTTPRequestHandler): def send_json(self, data: Any, status: int = 200) -> None: body = json.dumps(data, ensure_ascii=False, indent=2).encode("utf-8") - self.send_response(status) - self.send_header("Content-Type", "application/json; charset=utf-8") - self.send_header("Content-Length", str(len(body))) - self.end_headers() - self.wfile.write(body) + try: + self.send_response(status) + self.send_header("Content-Type", "application/json; charset=utf-8") + self.send_header("Content-Length", str(len(body))) + self.end_headers() + self.wfile.write(body) + except BrokenPipeError: + # Browser polling can cancel a request while logs are still being read. + # Do not flood panel logs with tracebacks for harmless client disconnects. + return def do_GET(self) -> None: path = urlparse(self.path).path @@ -2968,6 +3417,9 @@ def do_GET(self) -> None: data.append(item) self.send_json(data) return + if path == "/api/running_logs": + self.send_json({"logs": running_log_snapshot()}) + return if path == "/api/dispenser_color_map": self.send_json(dispenser_color_map_status()) return @@ -2995,8 +3447,12 @@ def do_POST(self) -> None: payload = json.loads(self.rfile.read(length) or b"{}") path = urlparse(self.path).path if path == "/api/run": - selected = with_collision_scene_prereq([str(key) for key in payload.get("selected") or []]) - selected = list(dict.fromkeys(selected)) + raw_selected = [str(key) for key in payload.get("selected") or []] + if payload.get("selected_already_expanded"): + selected = list(dict.fromkeys(raw_selected)) + else: + selected = with_collision_scene_prereq(raw_selected) + selected = list(dict.fromkeys(selected)) steps_by_key = {step.key: step for step in STEPS} results = [ run_step(steps_by_key[key], payload) diff --git a/tools/run/run_cocktail_collision_rviz_preview.sh b/tools/run/run_cocktail_collision_rviz_preview.sh index c3b14b8..1ff245e 100755 --- a/tools/run/run_cocktail_collision_rviz_preview.sh +++ b/tools/run/run_cocktail_collision_rviz_preview.sh @@ -94,7 +94,7 @@ for index in "${!SEQUENCE_GROUPS[@]}"; do PRESS_ONLY=0 \ DISPENSER_COLLISION_ENABLED=1 \ DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS:-1}" \ - REMOVE_COURSE_WORKSPACE_WALLS="${REMOVE_COURSE_WORKSPACE_WALLS:-1}" \ + REMOVE_COURSE_WORKSPACE_WALLS="${REMOVE_COURSE_WORKSPACE_WALLS:-0}" \ DISPENSER_ID="${did}" \ PRESS_COUNT="${count}" \ START_DOOSAN="${start_doosan}" \ diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 9638bb4..62111a0 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -22,20 +22,20 @@ RECIPE_PATH = ROOT / "outputs" / "latest_recipe.json" SEQUENCE_SCRIPT = ROOT / "tools" / "run" / "run_measured_dispenser_recipe_sequence.py" CONFIRM_PHRASE = "ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE" +FALLBACK_DISPENSER_SEQUENCE = ["1", "2", "3", "4"] def load_color_map() -> dict[str, str]: - """dispenser_id → color_name 매핑 로드.""" + """dispenser_id → color_name 매핑 로드. 정상 맵이 없으면 빈 dict로 fallback.""" if not COLOR_MAP_PATH.exists(): - print(f"[run_color_recipe] 색상 맵 없음: {COLOR_MAP_PATH}", file=sys.stderr) - print( - "[run_color_recipe] color_scan 스텝을 먼저 실행하거나, " - "패널 DIRECT DISPENSER INPUT에 1x1,2x2,3x1처럼 물리 디스펜서 번호와 횟수를 입력하세요.", - file=sys.stderr, - ) - sys.exit(1) + print(f"[run_color_recipe] 색상 맵 없음: {COLOR_MAP_PATH}; fallback 1,2,3,4 사용", file=sys.stderr) + return {} data = json.loads(COLOR_MAP_PATH.read_text(encoding="utf-8")) - return {str(k): str(v).lower().strip() for k, v in data.items()} + mapped = {str(k): str(v).lower().strip() for k, v in data.items()} + if not mapped or all(v == "unknown" for v in mapped.values()): + print("[run_color_recipe] 색상 맵이 비어 있거나 전부 unknown; fallback 1,2,3,4 사용", file=sys.stderr) + return {} + return mapped def color_to_dispenser_id(color: str, color_map: dict[str, str]) -> str | None: @@ -106,11 +106,38 @@ def main() -> int: help=f"확인 구문({CONFIRM_PHRASE}) 자동 전달") parser.add_argument("--execute", action="store_true", help="실제 measured dispenser sequence를 실행") + parser.add_argument("--press-min-transit-z-m", default="0.720") + parser.add_argument("--press-line-velocity", default="18.0") + parser.add_argument("--press-line-acceleration", default="25.0") + parser.add_argument("--press-travel-velocity", default="45.0") + parser.add_argument("--press-travel-acceleration", default="60.0") + parser.add_argument("--press-contact-joint-velocity", default="22.0") + parser.add_argument("--press-contact-joint-acceleration", default="30.0") + parser.add_argument("--gripper-open-settle-seconds", default="1.5") + parser.add_argument("--gripper-settle-seconds", default="0.8") + parser.add_argument("--wait-service-sec", default="15.0") + parser.add_argument("--pose-read-retries", default="3") + parser.add_argument("--pose-read-retry-sleep-sec", default="0.5") args = parser.parse_args() if args.execute and not args.confirm: print(f"[BLOCKED] --execute requires --confirm ({CONFIRM_PHRASE})", file=sys.stderr) return 2 + sequence_extra_args = [ + "--press-min-transit-z-m", str(args.press_min_transit_z_m), + "--press-line-velocity", str(args.press_line_velocity), + "--press-line-acceleration", str(args.press_line_acceleration), + "--press-travel-velocity", str(args.press_travel_velocity), + "--press-travel-acceleration", str(args.press_travel_acceleration), + "--press-contact-joint-velocity", str(args.press_contact_joint_velocity), + "--press-contact-joint-acceleration", str(args.press_contact_joint_acceleration), + "--gripper-open-settle-seconds", str(args.gripper_open_settle_seconds), + "--gripper-settle-seconds", str(args.gripper_settle_seconds), + "--wait-service-sec", str(args.wait_service_sec), + "--pose-read-retries", str(args.pose_read_retries), + "--pose-read-retry-sleep-sec", str(args.pose_read_retry_sleep_sec), + ] + direct_dispenser_ids = args.dispenser_ids.strip() if direct_dispenser_ids: try: @@ -123,6 +150,7 @@ def main() -> int: cmd = [ sys.executable, str(SEQUENCE_SCRIPT), "--dispenser-ids", dispenser_ids_str, + *sequence_extra_args, ] if args.execute: cmd += ["--execute"] @@ -133,7 +161,19 @@ def main() -> int: return result.returncode color_map = load_color_map() - print(f"[run_color_recipe] 색상 맵: {color_map}") + print(f"[run_color_recipe] 색상 맵: {color_map if color_map else 'fallback 1,2,3,4'}") + + if not color_map: + dispenser_ids_str = ",".join(FALLBACK_DISPENSER_SEQUENCE) + print(f"[run_color_recipe] fallback 직접 디스펜서 실행 순서: {dispenser_ids_str}") + cmd = [sys.executable, str(SEQUENCE_SCRIPT), "--dispenser-ids", dispenser_ids_str, *sequence_extra_args] + if args.execute: + cmd += ["--execute"] + if args.confirm: + cmd += ["--confirm", CONFIRM_PHRASE] + print(f"[run_color_recipe] 실행: {' '.join(cmd)}") + result = subprocess.run(cmd, check=False) + return result.returncode # 색깔+펌프 수 결정 if args.colors: @@ -170,6 +210,7 @@ def main() -> int: cmd = [ sys.executable, str(SEQUENCE_SCRIPT), "--dispenser-ids", dispenser_ids_str, + *sequence_extra_args, ] if args.execute: cmd += ["--execute"] diff --git a/tools/run/run_course_dispenser_press_cycle_rviz.sh b/tools/run/run_course_dispenser_press_cycle_rviz.sh index e6194c9..dfd9de0 100755 --- a/tools/run/run_course_dispenser_press_cycle_rviz.sh +++ b/tools/run/run_course_dispenser_press_cycle_rviz.sh @@ -41,7 +41,8 @@ DISPENSER_COLLISION_ENABLED="${DISPENSER_COLLISION_ENABLED:-1}" # MoveIt collision checking for the press stroke unless explicitly requested. DISPENSER_COLLISION_OBJECTS="${DISPENSER_COLLISION_OBJECTS:-1}" DISPENSER_COLLISION_EXCLUDE_IDS="${DISPENSER_COLLISION_EXCLUDE_IDS:-dispenser_head_nozzle_merged_horizontal_spout_box}" -REMOVE_COURSE_WORKSPACE_WALLS="${REMOVE_COURSE_WORKSPACE_WALLS:-1}" +REMOVE_COURSE_WORKSPACE_WALLS="${REMOVE_COURSE_WORKSPACE_WALLS:-0}" +WORKSPACE_COLLISION_ENABLED="${WORKSPACE_COLLISION_ENABLED:-1}" SHOW_LINK6_GRIPPER="${SHOW_LINK6_GRIPPER:-1}" DISPENSER_COLLISION_CONFIG="${DISPENSER_COLLISION_CONFIG:-${ROOT_DIR}/install/azas_bringup/share/azas_bringup/config/measured_dispenser_collision.yaml}" if [[ ! -f "${DISPENSER_COLLISION_CONFIG}" ]]; then @@ -188,6 +189,26 @@ else sleep "${CONTROLLER_SETTLE_SEC}" fi +if [[ "${WORKSPACE_COLLISION_ENABLED}" == "1" || "${WORKSPACE_COLLISION_ENABLED}" == "true" ]]; then + ros2 launch azas_bringup workspace_collision_scene.launch.py \ + publish_period_sec:=1.0 \ + publish_collision_objects:=true \ + table_collision_enabled:=true \ + workspace_boundary_collision_enabled:=true \ + dispenser_collision_enabled:=false \ + >"${LOG_DIR}/workspace_collision_scene.log" 2>&1 & + PIDS+=("$!") + echo "[Azas] WORKSPACE_COLLISION_ENABLED=${WORKSPACE_COLLISION_ENABLED}: publishing floor/table + side safety walls on /collision_object and /azas/workspace_collision/markers." + sleep 2 + timeout 8 ros2 topic echo /azas/workspace_collision/markers >"${LOG_DIR}/workspace_collision_markers.txt" 2>/dev/null || true + if grep -q 'side_grip_workspace_.*_wall\|side_grip_table' "${LOG_DIR}/workspace_collision_markers.txt"; then + echo '[Azas] Published workspace safety markers: floor/table + side walls.' + else + echo '[Azas] Warning: workspace safety marker sample did not capture table/walls yet.' >&2 + tail -80 "${LOG_DIR}/workspace_collision_scene.log" >&2 || true + fi +fi + if [[ "${DISPENSER_COLLISION_ENABLED}" == "1" || "${DISPENSER_COLLISION_ENABLED}" == "true" ]]; then if [[ "${DISPENSER_COLLISION_OBJECTS}" == "1" || "${DISPENSER_COLLISION_OBJECTS}" == "true" ]]; then DISPENSER_COLLISION_OBJECTS_BOOL=true @@ -213,7 +234,7 @@ if [[ "${DISPENSER_COLLISION_ENABLED}" == "1" || "${DISPENSER_COLLISION_ENABLED} echo "[Azas] Dispenser combined box represents bottle/body only; press pre/contact is derived from press_contact_joints_deg FK, not from this box." echo "[Azas] DISPENSER_COLLISION_OBJECTS=${DISPENSER_COLLISION_OBJECTS} (1=add to MoveIt collision scene, 0=RViz markers only)." echo "[Azas] DISPENSER_COLLISION_EXCLUDE_IDS=${DISPENSER_COLLISION_EXCLUDE_IDS} (marker-only IDs; not used to block press-contact planning)." - echo "[Azas] REMOVE_COURSE_WORKSPACE_WALLS=${REMOVE_COURSE_WORKSPACE_WALLS} (1=remove stale side_grip_workspace_* walls that can collide with link_2 in this course path)." + echo "[Azas] REMOVE_COURSE_WORKSPACE_WALLS=${REMOVE_COURSE_WORKSPACE_WALLS} (0=keep safety walls visible/active; 1=remove only if a legacy path is blocked)." sleep 2 if [[ "${DISPENSER_COLLISION_OBJECTS}" == "1" || "${DISPENSER_COLLISION_OBJECTS}" == "true" ]]; then timeout 8 ros2 topic echo /collision_object >"${LOG_DIR}/collision_object_samples.txt" 2>/dev/null || true diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index 3aca08d..e2ded84 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -304,32 +304,60 @@ def wait_motion_done(self, label: str, *, timeout_sec: float) -> None: raise RuntimeError(f"MoveWait returned success=false for {label}") def current_posx(self, timeout_sec: float | None = None) -> list[float]: - req = GetCurrentPosx.Request() - req.ref = DR_BASE - response = self._call( - self.get_posx, - req, - timeout_sec=timeout_sec or self.args.wait_service_sec, - label="GetCurrentPosx", - ) - if not response.success or not response.task_pos_info: - raise RuntimeError("GetCurrentPosx returned success=false or empty task_pos_info") - values = list(response.task_pos_info[0].data) - if len(values) < 6: - raise RuntimeError(f"GetCurrentPosx returned too few values: {values}") - return [float(value) for value in values[:6]] + timeout = timeout_sec or self.args.wait_service_sec + last_error = "" + for attempt in range(1, max(int(self.args.pose_read_retries), 1) + 1): + try: + req = GetCurrentPosx.Request() + req.ref = DR_BASE + response = self._call( + self.get_posx, + req, + timeout_sec=timeout, + label="GetCurrentPosx", + ) + if not response.success or not response.task_pos_info: + raise RuntimeError("GetCurrentPosx returned success=false or empty task_pos_info") + values = list(response.task_pos_info[0].data) + if len(values) < 6: + raise RuntimeError(f"GetCurrentPosx returned too few values: {values}") + return [float(value) for value in values[:6]] + except RuntimeError as exc: + last_error = str(exc) + if attempt >= max(int(self.args.pose_read_retries), 1): + break + print( + f"[Azas] GetCurrentPosx retry {attempt}/{int(self.args.pose_read_retries)}: {last_error}", + file=sys.stderr, + ) + time.sleep(max(float(self.args.pose_read_retry_sleep_sec), 0.0)) + raise RuntimeError(last_error or "GetCurrentPosx failed") def current_posj(self, timeout_sec: float | None = None) -> list[float]: - response = self._call( - self.get_posj, - GetCurrentPosj.Request(), - timeout_sec=timeout_sec or self.args.wait_service_sec, - label="GetCurrentPosj", - ) - values = list(response.pos) - if not response.success or len(values) < 6: - raise RuntimeError("GetCurrentPosj returned success=false or too few joint values") - return [float(value) for value in values[:6]] + timeout = timeout_sec or self.args.wait_service_sec + last_error = "" + for attempt in range(1, max(int(self.args.pose_read_retries), 1) + 1): + try: + response = self._call( + self.get_posj, + GetCurrentPosj.Request(), + timeout_sec=timeout, + label="GetCurrentPosj", + ) + values = list(response.pos) + if not response.success or len(values) < 6: + raise RuntimeError("GetCurrentPosj returned success=false or too few joint values") + return [float(value) for value in values[:6]] + except RuntimeError as exc: + last_error = str(exc) + if attempt >= max(int(self.args.pose_read_retries), 1): + break + print( + f"[Azas] GetCurrentPosj retry {attempt}/{int(self.args.pose_read_retries)}: {last_error}", + file=sys.stderr, + ) + time.sleep(max(float(self.args.pose_read_retry_sleep_sec), 0.0)) + raise RuntimeError(last_error or "GetCurrentPosj failed") def current_tcp_pose(self) -> Pose: values = self.current_posx() @@ -363,6 +391,7 @@ def move_front_hold( offset_z_m: float, velocity: float, acceleration: float, + prefer_joint: bool = False, ) -> None: position, quaternion, raw_zyz = load_front_hold_pose(self.args.config, dispenser_id) link6_position = [ @@ -386,6 +415,13 @@ def move_front_hold( response = self._call(self.ikin, req, timeout_sec=self.args.wait_service_sec, label="Ikin") if not response.success: raise RuntimeError(f"Ikin failed for {label}") + if prefer_joint: + print( + f"[Azas] {label}: using IK MoveJoint for transit, not Cartesian MoveLine, " + "to avoid a straight TCP path through dispenser/bottle geometry" + ) + self.move_front_hold_joint_fallback(pos, label=label) + return req = MoveLine.Request() req.pos = pos req.vel = [velocity, velocity] @@ -398,9 +434,61 @@ def move_front_hold( req.sync_type = SYNC response = self._call(self.move_line, req, timeout_sec=self.args.move_timeout_sec, label=f"MoveLine {label}") if not response.success: - raise RuntimeError(f"MoveLine returned success=false for {label}") + if not self.args.front_hold_joint_fallback: + raise RuntimeError(f"MoveLine returned success=false for {label}") + print( + f"[WARN] MoveLine returned success=false for {label}; " + "retrying same measured target with IK MoveJoint fallback" + ) + self.move_front_hold_joint_fallback(pos, label=label) + return self.wait_motion_done(label, timeout_sec=self.args.move_timeout_sec) - self.wait_for_target(pos, label=label) + try: + self.wait_for_target(pos, label=label) + except RuntimeError as exc: + if not self.args.front_hold_joint_fallback: + raise + print( + f"[WARN] MoveLine verification failed for {label}: {exc}; " + "retrying same measured target with IK MoveJoint fallback" + ) + self.move_front_hold_joint_fallback(pos, label=label) + + def move_front_hold_joint_fallback(self, posx_mm_deg: list[float], *, label: str) -> None: + joints_deg = self.ikin_posj(posx_mm_deg, label=f"{label} IK joint fallback") + self.movej( + joints_deg, + label=f"{label} IK MoveJoint fallback", + velocity=self.args.front_hold_joint_fallback_velocity, + acceleration=self.args.front_hold_joint_fallback_acceleration, + ) + self.wait_for_target(posx_mm_deg, label=f"{label} IK MoveJoint fallback posx") + + def safe_lift_current( + self, + *, + label: str, + min_z_m: float, + velocity: float, + acceleration: float, + timeout_sec: float, + ) -> None: + pose = self.current_posx() + target_z_mm = max(pose[2], max(min_z_m, 0.0) * 1000.0) + if target_z_mm <= pose[2] + 1.0: + print( + f"[Azas] {label}: already above safe transit z " + f"current_z={pose[2] / 1000.0:.3f}m min_z={min_z_m:.3f}m" + ) + return + target = [pose[0], pose[1], target_z_mm, pose[3], pose[4], pose[5]] + self.move_posx( + target, + label=label, + velocity=velocity, + acceleration=acceleration, + timeout_sec=timeout_sec, + ) def move_posx( self, @@ -431,6 +519,56 @@ def move_posx( self.wait_motion_done(label, timeout_sec=timeout_sec) self.wait_for_target(pos, label=label) + def move_posx_no_verify( + self, + pos: list[float], + *, + label: str, + velocity: float, + acceleration: float, + timeout_sec: float, + ) -> None: + print( + f"[Azas] {label}: posx=[{pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f}, " + f"{pos[3]:.1f}, {pos[4]:.1f}, {pos[5]:.1f}]" + ) + req = MoveLine.Request() + req.pos = pos + req.vel = [velocity, velocity] + req.acc = [acceleration, acceleration] + req.time = 0.0 + req.radius = 0.0 + req.ref = DR_BASE + req.mode = MOVE_MODE_ABSOLUTE + req.blend_type = BLENDING_SPEED_TYPE_DUPLICATE + req.sync_type = SYNC + response = self._call(self.move_line, req, timeout_sec=timeout_sec, label=f"MoveLine {label}") + if not response.success: + raise RuntimeError(f"MoveLine returned success=false for {label}") + self.wait_motion_done(label, timeout_sec=timeout_sec) + + def movej_no_verify(self, joints_deg: list[float], *, label: str, velocity: float, acceleration: float) -> None: + print( + "[Azas] " + + label + + ": movej_deg=[" + + ", ".join(f"{value:.1f}" for value in joints_deg) + + "]" + ) + req = MoveJoint.Request() + req.pos = [float(value) for value in joints_deg] + req.vel = float(velocity) + req.acc = float(acceleration) + req.time = 0.0 + req.radius = 0.0 + req.mode = MOVE_MODE_ABSOLUTE + req.blend_type = BLENDING_SPEED_TYPE_DUPLICATE + req.sync_type = SYNC + response = self._call(self.move_joint, req, timeout_sec=self.args.press_timeout_sec, label=f"MoveJoint {label}") + if not response.success: + raise RuntimeError(f"MoveJoint returned success=false for {label}") + self.wait_motion_done(label, timeout_sec=self.args.press_timeout_sec) + def movej(self, joints_deg: list[float], *, label: str, velocity: float, acceleration: float) -> None: print( "[Azas] " @@ -507,12 +645,27 @@ def wait_for_joint_target(self, target_joints_deg: list[float], *, label: str) - def wait_for_target(self, target_pos_mm_deg: list[float], *, label: str) -> None: deadline = time.monotonic() + max(self.args.verify_timeout_sec, 0.1) last_distance = 999999.0 + best_distance = last_distance + last_progress_time = time.monotonic() while time.monotonic() < deadline: actual = self.current_posx(timeout_sec=5.0) last_distance = sum((actual[index] - target_pos_mm_deg[index]) ** 2 for index in range(3)) ** 0.5 print(f"[Azas] verify {label}: distance={last_distance:.1f}mm tolerance={self.args.target_tolerance_mm:.1f}mm") if last_distance <= max(self.args.target_tolerance_mm, 0.1): return + if best_distance - last_distance >= max(self.args.target_stall_delta_mm, 0.1): + best_distance = last_distance + last_progress_time = time.monotonic() + elif ( + self.args.target_stall_timeout_sec > 0.0 + and last_distance >= max(self.args.target_stall_min_distance_mm, self.args.target_tolerance_mm) + and time.monotonic() - last_progress_time >= max(self.args.target_stall_timeout_sec, 0.0) + ): + raise RuntimeError( + f"target verification stalled for {label}; " + f"distance={last_distance:.1f}mm best={best_distance:.1f}mm " + f"no_progress_for={time.monotonic() - last_progress_time:.1f}s" + ) time.sleep(max(self.args.verify_poll_seconds, 0.05)) raise RuntimeError(f"target verification timeout for {label}; distance={last_distance:.1f}mm") @@ -570,11 +723,57 @@ def move_and_release(self, dispenser_id: str) -> None: print("[Azas] RG2 full-open release complete; continuing only after open settle wait") def regrasp_and_lift(self, dispenser_id: str) -> None: + self.safe_lift_current( + label="safe vertical lift after press before re-grasp transit", + min_z_m=self.args.regrasp_min_transit_z_m, + velocity=self.args.regrasp_approach_velocity, + acceleration=self.args.regrasp_approach_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) + if abs(self.args.regrasp_retreat_y_m) > 1e-6 or abs(self.args.regrasp_retreat_x_m) > 1e-6: + pose = self.current_posx() + retreat = [ + pose[0] + self.args.regrasp_retreat_x_m * 1000.0, + pose[1] + self.args.regrasp_retreat_y_m * 1000.0, + pose[2], + pose[3], + pose[4], + pose[5], + ] + self.move_posx( + retreat, + label="safe lateral retreat away from dispenser before re-grasp transit", + velocity=self.args.regrasp_approach_velocity, + acceleration=self.args.regrasp_approach_acceleration, + timeout_sec=self.args.move_timeout_sec, + ) + front_hold_position, _, _ = load_front_hold_pose(self.args.config, dispenser_id) + desired_approach_z_m = max( + front_hold_position[2] + max(self.args.regrasp_approach_offset_z_m, 0.0), + max(self.args.regrasp_min_transit_z_m, 0.0), + ) + capped_approach_z_m = min(desired_approach_z_m, max(self.args.regrasp_max_transit_z_m, 0.0)) + if capped_approach_z_m < desired_approach_z_m: + print( + f"[WARN] capping re-grasp high approach z from " + f"{desired_approach_z_m:.3f}m to {capped_approach_z_m:.3f}m" + ) + approach_offset_z_m = max(capped_approach_z_m - front_hold_position[2], 0.0) + self.move_front_hold( + dispenser_id, + label="final re-grasp high transit above front-hold", + offset_x_m=0.0, + offset_y_m=0.0, + offset_z_m=approach_offset_z_m, + velocity=self.args.regrasp_approach_velocity, + acceleration=self.args.regrasp_approach_acceleration, + prefer_joint=self.args.regrasp_high_transit_joint, + ) self.gripper_command( "open", width_m=self.args.gripper_open_width_m, - force_n=self.args.gripper_force_n, - label="RG2 open before re-grasp", + force_n=self.args.gripper_open_force_n, + label="RG2 open at safe high re-grasp approach", ) self.move_front_hold( dispenser_id, @@ -646,12 +845,11 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: # block on this setup, and the measured joint position itself is # the authoritative press contact pose. MoveJoint to the measured # pose first, then read the live TCP as the contact reference. - transit_z = current_pose[2] + max( - self.args.press_transit_height_m, - self.args.press_pre_lift_m, - 0.0, - ) * 1000.0 pre_z = contact_z + max(self.args.press_pre_lift_m, 0.0) * 1000.0 + transit_z = max( + current_pose[2] + max(self.args.press_transit_height_m, self.args.press_pre_lift_m, 0.0) * 1000.0, + min(pre_z, max(self.args.press_min_transit_z_m, 0.0) * 1000.0), + ) pressed_z = contact_z - max(self.args.press_depth_m, 0.0) * 1000.0 print( "[Azas] integrated press: " @@ -740,14 +938,21 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: f"z_descent={contact_z - pressed_z:.1f}mm transit_z={transit_z:.1f} " "source=live TCP after measured MoveJoint" ) - steps.append( - ( - [x_mm, y_mm, pre_z, rx, ry, rz], - "pre pose above dispenser head", - self.args.press_line_velocity, - self.args.press_line_acceleration, + if self.args.press_joint_space_use_high_prepose: + steps.append( + ( + [x_mm, y_mm, pre_z, rx, ry, rz], + "pre pose above dispenser head", + self.args.press_line_velocity, + self.args.press_line_acceleration, + ) ) - ) + else: + print( + "[Azas] joint-space press: skipping high Cartesian pre pose after measured contact; " + "pump will run contact -> pressed_z -> contact to avoid the 300mm pre-pose stall" + ) + pre_z = contact_z else: steps.extend( [ @@ -801,13 +1006,19 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: self.args.press_line_acceleration, ) ) - if self.args.press_post_retreat_after_sequence: + if self.args.press_post_retreat_after_sequence and joint_space_press: + print( + "[Azas] joint-space press: skipping Cartesian post-retreat away from dispenser; " + "measured press joints are already authoritative and the lateral retreat can stall " + "real hardware verification before the re-grasp step" + ) + elif self.args.press_post_retreat_after_sequence: steps.append( ( [ x_mm + self.args.press_post_retreat_dx_m * 1000.0, y_mm + self.args.press_post_retreat_dy_m * 1000.0, - pre_z, + (transit_z if joint_space_press and not self.args.press_joint_space_use_high_prepose else pre_z), rx, ry, rz, @@ -1061,7 +1272,15 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--move-acceleration", type=float, default=90.0) parser.add_argument("--move-prehold-offset-x-m", type=float, default=0.0) parser.add_argument("--move-prehold-offset-y-m", type=float, default=0.0) - parser.add_argument("--move-prehold-offset-z-m", type=float, default=0.0) + parser.add_argument( + "--move-prehold-offset-z-m", + type=float, + default=0.300, + help=( + "Vertical approach offset for initial cup placement at dispenser front-hold. " + "Default 0.300 m keeps lateral travel above bottles/dispensers before descending." + ), + ) parser.add_argument("--move-prehold-velocity", type=float, default=50.0) parser.add_argument("--move-prehold-acceleration", type=float, default=70.0) parser.add_argument("--move-timeout-sec", type=float, default=180.0) @@ -1076,11 +1295,55 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--pick-lift-velocity", type=float, default=35.0) parser.add_argument("--pick-lift-acceleration", type=float, default=50.0) parser.add_argument("--pick-timeout-sec", type=float, default=120.0) + parser.add_argument( + "--regrasp-min-transit-z-m", + type=float, + default=0.720, + help="Minimum absolute TCP Z for the vertical lift immediately after pressing, before returning to the cup.", + ) + parser.add_argument( + "--regrasp-approach-offset-z-m", + type=float, + default=0.650, + help=( + "High front-hold Z offset used before opening the gripper for the post-press re-grasp. " + "The gripper opens only after this high approach is reached." + ), + ) + parser.add_argument( + "--regrasp-max-transit-z-m", + type=float, + default=0.780, + help="Maximum absolute TCP/front-hold high approach Z used for post-press re-grasp transit.", + ) + parser.add_argument("--regrasp-approach-velocity", type=float, default=45.0) + parser.add_argument("--regrasp-approach-acceleration", type=float, default=60.0) + parser.add_argument( + "--regrasp-retreat-x-m", + type=float, + default=0.0, + help="Optional high-Z X retreat immediately after press before returning to cup.", + ) + parser.add_argument( + "--regrasp-retreat-y-m", + type=float, + default=0.0, + help="Optional high-Z Y retreat immediately after press before returning to cup.", + ) + parser.add_argument( + "--regrasp-high-transit-joint", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "Use IK MoveJoint, not Cartesian MoveLine, for the high post-press return-to-cup transit. " + "Default true because a straight TCP line can sweep through dispenser/bottle geometry." + ), + ) parser.add_argument( "--press-depth-m", type=float, - default=0.080, - help="Z descent below the measured dispenser-head contact pose.", + default=0.060, + help="Z descent below the measured dispenser-head contact pose. Default is 0.060 m (6 cm).", ) parser.add_argument( "--press-pre-lift-m", @@ -1090,10 +1353,16 @@ def parse_args() -> argparse.Namespace: ) parser.add_argument("--press-approach-height-m", type=float, default=0.100) parser.add_argument("--press-transit-height-m", type=float, default=0.300) - parser.add_argument("--press-line-velocity", type=float, default=10.0) - parser.add_argument("--press-line-acceleration", type=float, default=15.0) - parser.add_argument("--press-travel-velocity", type=float, default=20.0) - parser.add_argument("--press-travel-acceleration", type=float, default=30.0) + parser.add_argument( + "--press-min-transit-z-m", + type=float, + default=0.720, + help="Minimum absolute TCP Z before moving from cup release toward dispenser press joints.", + ) + parser.add_argument("--press-line-velocity", type=float, default=18.0) + parser.add_argument("--press-line-acceleration", type=float, default=25.0) + parser.add_argument("--press-travel-velocity", type=float, default=45.0) + parser.add_argument("--press-travel-acceleration", type=float, default=60.0) parser.add_argument("--press-timeout-sec", type=float, default=120.0) parser.add_argument("--press-hold-seconds", type=float, default=0.25) parser.add_argument("--press-gripper-close-width-m", type=float, default=0.0) @@ -1106,8 +1375,14 @@ def parse_args() -> argparse.Namespace: ) parser.add_argument("--press-reset-joint-velocity", type=float, default=40.0) parser.add_argument("--press-reset-joint-acceleration", type=float, default=50.0) - parser.add_argument("--press-contact-joint-velocity", type=float, default=12.0) - parser.add_argument("--press-contact-joint-acceleration", type=float, default=18.0) + parser.add_argument("--press-contact-joint-velocity", type=float, default=22.0) + parser.add_argument("--press-contact-joint-acceleration", type=float, default=30.0) + parser.add_argument( + "--press-joint-space-use-high-prepose", + action=argparse.BooleanOptionalAction, + default=False, + help="For measured press_contact_joints_deg, also climb to Cartesian pre_z after contact. Default false avoids the observed pre-pose stall.", + ) parser.add_argument( "--press-move-configured-prepose-before-joint", action=argparse.BooleanOptionalAction, @@ -1118,15 +1393,55 @@ def parse_args() -> argparse.Namespace: "use the measured joints as the authoritative press target." ), ) - parser.add_argument("--press-post-retreat-after-sequence", action=argparse.BooleanOptionalAction, default=True) + parser.add_argument( + "--press-post-retreat-after-sequence", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "After a Cartesian-only press, move laterally away from the dispenser. " + "Default false; joint-space measured press skips this because it caused " + "real-hardware target verification stalls before cup re-grasp." + ), + ) parser.add_argument("--press-post-retreat-dx-m", type=float, default=-0.120) parser.add_argument("--press-post-retreat-dy-m", type=float, default=0.0) parser.add_argument("--press-post-retreat-wait-seconds", type=float, default=0.10) - parser.add_argument("--wait-service-sec", type=float, default=8.0) + parser.add_argument("--wait-service-sec", type=float, default=15.0) + parser.add_argument( + "--pose-read-retries", + type=int, + default=3, + help="Retry count for non-motion pose read services such as GetCurrentPosx/GetCurrentPosj.", + ) + parser.add_argument( + "--pose-read-retry-sleep-sec", + type=float, + default=0.5, + help="Delay between pose read retries.", + ) parser.add_argument("--verify-timeout-sec", type=float, default=70.0) parser.add_argument("--verify-poll-seconds", type=float, default=0.15) parser.add_argument("--target-tolerance-mm", type=float, default=15.0) + parser.add_argument( + "--target-stall-timeout-sec", + type=float, + default=8.0, + help="Fail target verification early when the TCP is far from target and position is not improving.", + ) + parser.add_argument("--target-stall-min-distance-mm", type=float, default=80.0) + parser.add_argument("--target-stall-delta-mm", type=float, default=2.0) parser.add_argument("--joint-target-tolerance-deg", type=float, default=2.0) + parser.add_argument( + "--front-hold-joint-fallback", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "For measured front-hold/pre-hold targets, retry with IK MoveJoint when " + "MoveLine enters a singularity or stalls target verification." + ), + ) + parser.add_argument("--front-hold-joint-fallback-velocity", type=float, default=30.0) + parser.add_argument("--front-hold-joint-fallback-acceleration", type=float, default=40.0) parser.add_argument("--gripper-service", default="/jarvis/rg2/set_width") parser.add_argument("--gripper-open-width-m", type=float, default=0.110) parser.add_argument("--gripper-open-force-n", type=float, default=12.0) @@ -1136,13 +1451,13 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--gripper-settle-seconds", type=float, - default=2.0, + default=0.8, help="Physical wait after every non-open RG2 command before the next robot motion.", ) parser.add_argument( "--gripper-open-settle-seconds", type=float, - default=5.0, + default=1.5, help="Physical wait after every RG2 open command before the next robot motion.", ) parser.add_argument( @@ -1154,6 +1469,16 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--precheck-ikin", action=argparse.BooleanOptionalAction, default=True) parser.add_argument("--ikin-sol-space", type=int, default=2) parser.add_argument("--legacy-subprocess-primitives", action="store_true", help="use the old helper-script-per-step implementation for fallback/debugging") + parser.add_argument( + "--integrated-regrasp-fallback-subprocess", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "If the persistent integrated re-grasp/lift stalls verification, retry once " + "with the legacy pick_from_measured_dispenser_front_hold helper instead of " + "ending the recipe at the first target timeout." + ), + ) parser.add_argument("--execute", action="store_true") parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} when --execute is used") args = parser.parse_args() @@ -1265,8 +1590,17 @@ def main() -> int: try: motion.regrasp_and_lift(dispenser_id) except RuntimeError as exc: - print(f"[FAIL] {label_prefix}: integrated re-grasp/lift failed: {exc}") - return 1 + if not args.integrated_regrasp_fallback_subprocess: + print(f"[FAIL] {label_prefix}: integrated re-grasp/lift failed: {exc}") + return 1 + print( + f"[WARN] {label_prefix}: integrated re-grasp/lift failed: {exc}; " + "retrying once with legacy front-hold pick helper" + ) + rc = run_command(f"{label_prefix}: fallback re-grasp cup from front-hold", pick_cmd(args, dispenser_id)) + if rc != 0: + print(f"[FAIL] {label_prefix}: fallback re-grasp/lift failed after integrated timeout") + return rc if motion is None: rc = run_command( From 47985f29851d39f15724d36a554d0e8036a6fc50 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 16:36:25 +0900 Subject: [PATCH 31/88] Stabilize panel-driven robot logic trials Keep panel execution aligned with the field-tested tmux flow, pin ROS domain setup for non-interactive commands, and add explicit direct launch surfaces for side-grip, cup uprighting, and lid-grip trials. Constraint: Real robot GUI workflows must not depend on interactive shell startup or slow ROS CLI discovery. Rejected: Treating static package presence as enough to enable every panel motion step | it allowed unverified logic to start from the panel. Confidence: medium Scope-risk: broad Directive: Promote Somyeong/Kang panel buttons only after terminal/tmux success logs exist. Tested: python3 -m py_compile on changed Python entry points; bash -n on direct run scripts; panel API smoke check. Not-tested: End-to-end real motion for Somyeong cup uprighting and Kang lid twist is still pending field trial. --- docs/robot_pipeline_control.html | 175 +- .../azas_bringup/joint_state_relay_legacy.py | 6 +- .../lid_sticker_grip_planning.launch.py | 6 +- .../azas_cup_uprighting/_base_node.py | 44 +- .../azas_cup_uprighting/_config.py | 38 +- .../launch/yolo_cup_uprighting.launch.py | 78 +- src/azas_cup_uprighting/package.xml | 1 + .../azas_perception/lid_marker.py | 125 +- .../lid_sticker_detector_node.py | 62 +- .../test/test_depth_and_detection_logic.py | 47 + .../dsr_practice/joint_state_relay.py | 6 +- .../dsr_practice/yolo_cup_pick_node.py | 18 +- .../launch/yolo_cup_pick_node.launch.py | 43 +- ...check_panel_gripper_reconnect_and_force.py | 4 +- .../check_panel_service_discovery_race.py | 43 +- tools/perception/diagnose_aruco_marker.py | 215 +++ tools/run/direct_movej_joints.py | 70 +- tools/run/dispenser_color_scan_ros.sh | 9 +- tools/run/robot_pipeline_control_server.py | 1487 +++++++++++++---- tools/run/run_changhyun_side_grip_direct.sh | 98 ++ tools/run/run_color_recipe_sequence.py | 51 +- tools/run/run_kang_lid_grip_close_direct.sh | 91 + .../run_measured_dispenser_recipe_sequence.py | 63 +- .../run/run_somyeong_cup_uprighting_direct.sh | 59 + tools/run/run_tmux_logic_sequence.sh | 248 +++ tools/run/start_azas_tmux_stack.sh | 44 + 26 files changed, 2634 insertions(+), 497 deletions(-) create mode 100644 tools/perception/diagnose_aruco_marker.py create mode 100755 tools/run/run_changhyun_side_grip_direct.sh create mode 100755 tools/run/run_kang_lid_grip_close_direct.sh create mode 100755 tools/run/run_somyeong_cup_uprighting_direct.sh create mode 100755 tools/run/run_tmux_logic_sequence.sh create mode 100755 tools/run/start_azas_tmux_stack.sh diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index 568e845..6e39576 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -841,21 +841,21 @@

Azas Robot Pipeline Control

-
버튼은 큐에 추가만 합니다. 순서: 준비 → 색상구분/JSON → 레시피 디스펜서 사이클 반복 → 컵홀더 배치 → 선택 순서 실행.
+
실험 순서: 연결 준비1 창현 side-grip → 성공 확인 후 2 소명 누운 컵 → 성공 확인 후 3 강개발자 뚜껑. 로직 버튼은 한 번에 묶지 말고 단독 실행합니다.
- - - - + + + + - - + + - + @@ -905,7 +905,8 @@

Azas Robot Pipeline Control

뚜껑 잡고 닫기 버튼은 강개발자 로직인 lid_grip_close 단계입니다. - 이 로직은 ArUco marker ID 14로 뚜껑 pose를 잡고, RG2로 뚜껑을 파지한 뒤 컵/컵홀더 위치로 이동해서 J6 twist로 뚜껑을 닫습니다. + 이 로직은 IsaacSim ArUcoMarker 가이드와 같은 DICT_6X6_250 marker ID 0으로 뚜껑 pose를 잡고, RG2로 뚜껑을 파지한 뒤 컵/컵홀더 위치로 이동해서 J6 twist로 뚜껑을 닫습니다. + aruco_marker_length_m은 검은 마커 본체 한 변의 실측값입니다. IsaacSim에서 10cm plane에 600px 마커와 60px quiet zone을 쓴 경우에는 0.083333이지만, 실제 인쇄 스티커는 반드시 실측값으로 맞춰야 합니다. 디스펜서 프레스나 컵홀더 배치가 아니라 뚜껑 파지→이동→닫기 전용 실제모션입니다. 실행 전 로봇 연결, 연결 확인, 그리퍼 연결, 카메라 연결이 필요합니다.
@@ -920,8 +921,8 @@

Azas Robot Pipeline Control

require_lid_detection:=false \ allow_aruco_only_after_grip_request:=false \ aruco_only_after_grip_request_sec:=20.0 \ - aruco_dictionary:=DICT_4X4_50 \ - aruco_marker_id:=14 \ + aruco_dictionary:=DICT_6X6_250 \ + aruco_marker_id:=0 \ aruco_marker_length_m:=0.03 \ use_aruco_axis_for_orientation:=true \ use_lid_pose_yaw_for_pick:=true \ @@ -981,70 +982,43 @@

Azas Robot Pipeline Control

- PR #20 side-grip 참고 명령어 + 소명 누운 컵 세우기 참고 명령어
- 패널의 PR #20 RealSense 컵 인식 후 side grip 단계는 아래 dsr_practice side-grip 튜닝 명령으로 실행됩니다. - 실행 때마다 colcon build를 반복하지 않도록 기본 실행에서는 build를 건너뜁니다. 코드 변경 후에만 아래 build 줄을 수동 실행하거나 AZAS_SIDE_GRIP_BUILD=1로 실행하세요. - 카메라 창에서 컵이 보이면 p 키로 집고, Esc로 종료합니다. + 소명 누운 컵 세우기 버튼은 azas_cup_uprighting 단계입니다. + 이 단계의 YOLO 모델은 azas_cup_uprighting/config/best.pt가 아니라 + /home/ssu/Azas/src/azas_perception/config/yolo_cup_uprighting_best.pt를 사용합니다. + 실행 전 로봇 연결, 연결 확인, 카메라 연결이 필요합니다.
cd /home/ssu/Azas
 source /opt/ros/humble/setup.bash
 source /home/ssu/ws_moveit/install/setup.bash
 source /home/ssu/ros2_ws/install/setup.bash
-# 코드 변경 후에만 필요: colcon build --symlink-install --packages-select dsr_practice
 source /home/ssu/Azas/install/setup.bash
-source /home/ssu/Azas/install/dsr_practice/share/dsr_practice/package.bash
-export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-}
-
-ros2 launch /home/ssu/Azas/install/dsr_practice/share/dsr_practice/launch/yolo_cup_pick_node.launch.py \
-  model_path:=/home/ssu/Azas/local_models/best.pt \
-  conf:=0.35 \
-  imgsz:=640 \
-  device:=cpu \
-  target_class:=cup \
-  auto_pick:=false \
-  auto_pick_interval:=8.0 \
-  depth_patch_radius:=7 \
-  min_depth_valid_ratio:=0.03 \
-  min_depth_m:=0.15 \
-  max_depth_m:=1.20 \
-  redetect_on_approach:=false \
-  redetect_settle_sec:=0.5 \
-  grasp_mode:=side \
-  side_far_stage_enabled:=false \
-  side_approach_offset:=0.18 \
-  side_short_stage_backoff_m:=0.08 \
-  side_grasp_stop_backoff_m:=0.04 \
-  side_close_underreach_m:=0.03 \
-  side_low_retry_lift_m:=0.0 \
-  side_low_retry_attempts:=0 \
-  side_linear_approach_enabled:=true \
-  side_final_slide_enabled:=false \
-  side_fixed_grasp_z_enabled:=true \
-  side_fixed_grasp_z:=0.07 \
-  side_project_bbox_center_to_fixed_z:=true \
-  side_candidate_plan_check_enabled:=true \
-  side_move_to_initial_center_before_close:=false \
-  verify_motion:=false \
-  move_to_camera_home:=true \
-  move_joint_home_before_camera_home:=false \
-  camera_home_mode:=joint \
-  min_motion_z:=0.07 \
-  workspace_xy_clamp_enabled:=false \
-  return_home_after_task:=false \
-  return_to_camera_home_after_attempt:=true \
-  table_collision_enabled:=true \
-  table_surface_z:=0.0 \
-  table_thickness:=0.04 \
-  table_size_x:=1.10 \
-  table_size_y:=0.65 \
-  table_center_x:=0.29 \
-  table_center_y:=0.0 \
-  dispenser_collision_enabled:=true \
-  dispenser_collision_config_path:=/home/ssu/Azas/src/azas_bringup/config/measured_dispenser_collision.yaml \
-  moveit_controller_name:=/dsr01/dsr_moveit_controller \
-  start_joint_state_relay:=true
+export AZAS_CUP_UPRIGHTING_MODEL_PATH=/home/ssu/Azas/src/azas_perception/config/yolo_cup_uprighting_best.pt + +ros2 launch /home/ssu/Azas/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py \ + model_path:=/home/ssu/Azas/src/azas_perception/config/yolo_cup_uprighting_best.pt \ + service_prefix:=dsr01 \ + enable_hardware:=true \ + hardware_confirm:=ENABLE_REAL_ROBOT_MOTION \ + run_yolo:=true \ + publish_hand_eye_tf:=true +
+
+ +
+ PR #20 side-grip 참고 명령어 +
+
+ 패널의 PR #20 RealSense 컵 인식 후 side grip 단계는 아래 dsr_practice side-grip 튜닝 명령으로 실행됩니다. + 실행 때마다 colcon build를 반복하지 않도록 기본 실행에서는 build를 건너뜁니다. 코드 변경 후에만 아래 build 줄을 수동 실행하거나 AZAS_SIDE_GRIP_BUILD=1로 실행하세요. + 패널은 이 로직을 별도 tmux 창으로 분리 실행합니다. 카메라 창에서 컵이 보이면 p 키로 집고, Esc로 종료합니다. + 방금 실기 성공한 조건처럼 디스펜서 충돌은 켠 상태로 유지하고, 진입 전 J1 clearance 12.0deg를 적용합니다. +
+
cd /home/ssu/Azas
+SERVICE_PREFIX=dsr01 DISPLAY=${DISPLAY:-:0} XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} \
+  bash tools/run/run_changhyun_side_grip_direct.sh
@@ -1320,8 +1294,6 @@

RealSense 카메라 화면

"side_grip_camera_home", "lid_view_pose", "move_to_color_scan_pose", - "side_grip", - "lid_grip_close", "place_cup_holder", "shake_closed_cup", "run_color_recipe_sequence", @@ -1339,21 +1311,29 @@

RealSense 카메라 화면

for (const prereq of prereqKeys) { const existingIndex = items.findIndex((item) => item.key === prereq); if (existingIndex >= 0) { + if (existingIndex < targetIndex) continue; items.splice(existingIndex, 1); if (existingIndex < targetIndex) targetIndex -= 1; } } targetIndex = items.findIndex((item) => item.key === targetKey); - prereqKeys.forEach((prereq, offset) => { + const missing = prereqKeys.filter((prereq) => !items.slice(0, targetIndex).some((item) => item.key === prereq)); + missing.forEach((prereq, offset) => { items.splice(targetIndex + offset, 0, {id: `q${nextQueueId++}`, key: prereq, injected: true}); }); } - ensureBefore("side_grip", ["start_camera", "side_grip_camera_home"]); + // 창현/소명/강개발자 단독 테스트는 이미 열린 tmux/ROS 세션을 재사용한다. + // 전체 준비 묶음은 각 통합 플로우 버튼에서 명시적으로 넣는다. ensureBefore("color_scan", ["connect_robot", "status_check", "move_to_color_scan_pose", "start_camera"]); - ensureBefore("cup_uprighting", ["connect_robot", "status_check", "start_camera"]); - if (!items.some((item) => needsCollisionScene(item.key))) return items; + const firstCollisionIndex = items.findIndex((item) => needsCollisionScene(item.key)); + if (firstCollisionIndex < 0) return items; + const existingSceneIndex = items.findIndex((item) => item.key === "start_collision_scene"); + if (existingSceneIndex >= 0 && existingSceneIndex <= firstCollisionIndex) return items; const withoutScene = items.filter((item) => item.key !== "start_collision_scene"); - return [{id: `q${nextQueueId++}`, key: "start_collision_scene", injected: true}, ...withoutScene]; + const insertIndex = withoutScene.findIndex((item) => needsCollisionScene(item.key)); + if (insertIndex < 0) return withoutScene; + withoutScene.splice(insertIndex, 0, {id: `q${nextQueueId++}`, key: "start_collision_scene", injected: true}); + return withoutScene; } function renderSummary() { @@ -1446,6 +1426,15 @@

RealSense 카메라 화면

return "info"; } + function shouldHaltAfterResult(result) { + const status = result?.status || ""; + if (["failed", "blocked", "timeout", "starting"].includes(status)) return true; + // Manual OpenCV-node steps: `started` means the window/node is waiting for + // operator input, not that motion has completed. + if (["start_tmux_stack", "cup_uprighting", "side_grip", "lid_grip_close"].includes(result?.key) && status === "started") return true; + return false; + } + function orderedVisibleGroups() { const byGroup = new Map(groupOrder.map((group) => [group, []])); for (const step of visibleSteps()) { @@ -1569,6 +1558,11 @@

RealSense 카메라 화면

} async function runSingleStepNow(key) { + if (isRunning) { + log.textContent = "이미 실행 중입니다. 현재 실행이 끝나거나 정리/중지 후 다시 누르세요."; + focusLog(); + return; + } const step = stepByKey(key); const itemId = `direct-${key}-${Date.now()}`; resetResultBadges(); @@ -1850,7 +1844,7 @@

RealSense 카메라 화면

async function resolveRecipeDispenserIdsFromJsonOrFallback() { const input = document.getElementById("recipeDispenserIds"); const directOrder = input?.value.trim() || ""; - if (directOrder) return {order: directOrder, source: "direct", fallback: false, fallbackReason: ""}; + if (directOrder) return {order: directOrder, source: /[A-Za-z가-힣]/.test(directOrder) ? "direct colors" : "direct dispenser ids", fallback: false, fallbackReason: ""}; const res = await fetch("/api/dispenser_color_map"); const data = await res.json(); const order = String(data.sequence_compact || data.sequence_csv || "1x1,2x1,3x1,4x1").trim(); @@ -1885,17 +1879,21 @@

RealSense 카메라 화면

document.getElementById("oneClickReadyBtn")?.addEventListener("click", () => { queueOnly(["check_one_click_cocktail_ready"], "준비확인을 큐에 추가했습니다."); }); + const PREP_STEPS = ["start_tmux_stack"]; + function queuePrepBundle(message) { + queueOnly(PREP_STEPS, message || "검증된 tmux 연결 스택 시작을 큐에 추가했습니다. status_check/개별 연결 큐를 반복하지 않습니다."); + } document.getElementById("startPrepBtn")?.addEventListener("click", () => { - queueOnly(["connect_robot", "status_check", "connect_gripper", "start_camera"], "시작 준비 묶음을 큐에 추가했습니다."); + queuePrepBundle(); }); document.getElementById("connectRobotBtn")?.addEventListener("click", () => { - queueOnly(["connect_robot", "status_check"], "로봇 연결 + 상태 확인을 큐에 추가했습니다."); + queuePrepBundle("tmux 연결 스택 시작을 큐에 추가했습니다. 로봇/그리퍼/카메라를 azas-logic 세션에 분리 실행합니다."); }); document.getElementById("connectGripperBtn")?.addEventListener("click", () => { - queueOnly(["connect_gripper"], "그리퍼 연결을 큐에 추가했습니다."); + queuePrepBundle("tmux 연결 스택 시작을 큐에 추가했습니다. 그리퍼도 같은 azas-logic 세션에서 시작합니다."); }); document.getElementById("connectCameraBtn")?.addEventListener("click", () => { - queueOnly(["start_camera"], "RealSense 카메라 연결을 큐에 추가했습니다. 640x480x30 저부하 프로파일로 시작합니다."); + queuePrepBundle("tmux 연결 스택 시작을 큐에 추가했습니다. RealSense는 검증된 640x480x30 설정으로 시작합니다."); }); document.getElementById("cameraViewBtn")?.addEventListener("click", () => { queueOnly(["start_camera_view"], "rqt_image_view 카메라 화면 보기를 큐에 추가했습니다."); @@ -1904,16 +1902,16 @@

RealSense 카메라 화면

queueOnly(["detect_cup_lid"], "YOLO 컵/뚜껑 인식 토픽 시작을 큐에 추가했습니다. 이 스텝은 화면 창을 띄우지 않습니다."); }); document.getElementById("cupUprightingBtn")?.addEventListener("click", () => { - queueOnly(["connect_robot", "status_check", "start_camera", "cup_uprighting"], "소명/누운 컵 직립화 로직을 큐에 추가했습니다. 실제 모션 허용 체크가 필요합니다."); + queueOnly(["cup_uprighting"], "2단계 소명/누운 컵 직립화 로직만 단독으로 큐에 추가했습니다. 1단계 성공 확인 후 실행하세요. OpenCV 창에서 컵을 확인한 뒤 p 키로 실행합니다."); }); document.getElementById("sideGripBtn")?.addEventListener("click", () => { - queueOnly(["start_camera", "side_grip_camera_home", "side_grip"], "창현/PR #20 RealSense side-grip 로직을 큐에 추가했습니다. 로봇 연결은 현재 세션을 사용하고, 카메라 홈 자세 이동 후 실행합니다."); + queueOnly(["side_grip"], "1단계 창현/PR #20 RealSense side-grip만 단독으로 큐에 추가했습니다. 카메라 화면에서 컵을 확인한 뒤 p 키로 직접 잡습니다. 실패하거나 로봇이 안 움직이면 이 단계만 다시 실행하세요."); }); document.getElementById("pickLidBtn")?.addEventListener("click", () => { queueOnly(["start_camera", "pick_lid"], "뚜껑 grip pose 계획 로직을 큐에 추가했습니다. 실제 로봇 모션은 실행하지 않습니다."); }); document.getElementById("lidGripCloseBtn")?.addEventListener("click", () => { - queueOnly(["connect_robot", "status_check", "connect_gripper", "start_camera", "lid_view_pose", "lid_grip_close"], "강개발자 lid_grip_close를 큐에 추가했습니다. 뚜껑 보기 자세→ArUco 14 인식→RG2 파지→컵 위치 이동→J6 twist 닫기 실제모션입니다."); + queueOnly(["lid_grip_close"], "3단계 강개발자 lid_grip_close만 단독으로 큐에 추가했습니다. 1/2단계 성공 확인 후 실행하세요. ArUco 확인 후 p 키 흐름으로 실행합니다."); }); document.getElementById("colorScanJsonBtn")?.addEventListener("click", () => { @@ -1932,14 +1930,20 @@

RealSense 카메라 화면

document.getElementById("fullCocktailRealBtn")?.addEventListener("click", async () => { try { await resolveRecipeDispenserIdsFromJsonOrFallback(); } catch (err) { log.textContent = String(err); focusLog(); return; } - queueOnly(["connect_robot", "status_check", "connect_gripper", "start_camera", "side_grip_camera_home", "side_grip", "move_to_color_scan_pose", "color_scan", "run_color_recipe_sequence", "place_cup_holder"], "전체 플로우를 큐에 추가했습니다. (카메라 홈→컵 side-grip → 검증자세 색상 핸들 JSON → 레시피 사이클 → MoveIt 컵홀더 배치)", {clear: true}); + queueOnly([...PREP_STEPS, "side_grip_camera_home", "side_grip", "move_to_color_scan_pose", "color_scan", "run_color_recipe_sequence", "place_cup_holder"], "전체 플로우를 큐에 추가했습니다. (tmux 연결 준비 → 카메라 홈→컵 side-grip → 검증자세 색상 핸들 JSON → 레시피 사이클 → MoveIt 컵홀더 배치)", {clear: true}); }); document.getElementById("oneClickResultBtn")?.addEventListener("click", () => { queueOnly(["check_one_click_cocktail_result"], "결과확인을 큐에 추가했습니다."); }); document.getElementById("run").addEventListener("click", async () => { + if (isRunning) { + log.textContent = "이미 실행 중입니다. 준비 중인 로봇/그리퍼/카메라 단계가 끝날 때까지 다음 실행을 막습니다."; + focusLog(); + return; + } const selected = withCollisionScenePrereq(selectedQueue); + selectedQueue = selected; resetResultBadges(); isRunning = Boolean(selected.length); activeLogKeys = new Set(selected.map((item) => item.key)); @@ -1957,6 +1961,7 @@

RealSense 카메라 화면

refreshRunningLogs(false); const body = payload(); body.selected = [item.key]; + body.selected_already_expanded = true; const res = await fetch("/api/run", { method: "POST", headers: {"Content-Type": "application/json"}, @@ -1979,7 +1984,7 @@

RealSense 카메라 화면

results.push(returned); const statusItemId = returned.key === item.key ? item.id : ""; setStepStatus(returned.key || item.key, returned.status, statusItemId); - if (returned.status === "failed" || returned.status === "blocked" || returned.status === "timeout") { + if (shouldHaltAfterResult(returned)) { shouldStop = true; break; } diff --git a/src/azas_bringup/azas_bringup/joint_state_relay_legacy.py b/src/azas_bringup/azas_bringup/joint_state_relay_legacy.py index c727f8c..4cc4de7 100644 --- a/src/azas_bringup/azas_bringup/joint_state_relay_legacy.py +++ b/src/azas_bringup/azas_bringup/joint_state_relay_legacy.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from sensor_msgs.msg import JointState @@ -32,9 +33,12 @@ def main(args=None): node = JointStateRelay() try: rclpy.spin(node) + except (ExternalShutdownException, KeyboardInterrupt): + pass finally: node.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() if __name__ == "__main__": diff --git a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py index f62218b..ce5f856 100644 --- a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py +++ b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py @@ -85,8 +85,9 @@ def generate_launch_description(): DeclareLaunchArgument("red_min_area_px", default_value="80.0"), DeclareLaunchArgument("red_min_radius_px", default_value="4.0"), DeclareLaunchArgument("red_min_circularity", default_value="0.65"), - DeclareLaunchArgument("aruco_dictionary", default_value="DICT_4X4_50"), - DeclareLaunchArgument("aruco_marker_id", default_value="-1"), + DeclareLaunchArgument("aruco_dictionary", default_value="DICT_6X6_250"), + DeclareLaunchArgument("aruco_marker_id", default_value="0"), + DeclareLaunchArgument("aruco_fallback_markers", default_value="DICT_4X4_50:14"), DeclareLaunchArgument("aruco_marker_length_m", default_value="0.03"), DeclareLaunchArgument("use_aruco_axis_for_orientation", default_value="true"), DeclareLaunchArgument("aruco_finger_axis_quarter_turns", default_value="1"), @@ -277,6 +278,7 @@ def generate_launch_description(): LaunchConfiguration("aruco_marker_id"), value_type=int, ), + "aruco_fallback_markers": LaunchConfiguration("aruco_fallback_markers"), "aruco_marker_length_m": ParameterValue( LaunchConfiguration("aruco_marker_length_m"), value_type=float, diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py index 5ed8869..fe4b0f2 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -18,6 +18,7 @@ - _handle_key_extra(key) — 추가 키 (e.g. 's' for box scan) """ +import os import threading import time @@ -46,6 +47,14 @@ raise ImportError("pip install ultralytics") from e +def _parse_bool(value) -> bool: + if isinstance(value, bool): + return value + if value is None: + return False + return str(value).strip().lower() in {"1", "true", "yes", "y", "on"} + + class BaseMoveItPickNode(Node): """MoveIt + RealSense + YOLO + RG2 그리퍼 통합 베이스.""" @@ -64,10 +73,15 @@ def __init__(self): self.intrinsics = None # ── 픽 상태 ── + self.declare_parameter("auto_pick", False) + self.declare_parameter("skip_initial_home_move", False) self.picking = False self.home_xyz = None # (x, y, z) [m] — initialize_home 에서 설정 self.home_ori = None # quat dict {x, y, z, w} - self._auto_mode = False + self._auto_mode = _parse_bool(self.get_parameter("auto_pick").value) + self._skip_initial_home_move = _parse_bool( + self.get_parameter("skip_initial_home_move").value + ) self._last_pick_time = 0.0 self._detections: list[dict] = [] self._frozen_frame = None @@ -92,8 +106,16 @@ def __init__(self): "pilz_industrial_motion_planner", "PTP", vel=0.15, acc=0.1, time=2.0) # ── YOLO ── - log.info(f"YOLO 모델 로드: {cfg.YOLO_MODEL_PATH}") - self.yolo = YOLO(cfg.YOLO_MODEL_PATH) + self.declare_parameter("model_path", cfg.YOLO_MODEL_PATH) + self.model_path = str(self.get_parameter("model_path").value).strip() + self.model_path = os.path.expanduser(self.model_path) + log.info(f"YOLO 모델 로드: {self.model_path}") + if not os.path.isfile(self.model_path): + raise FileNotFoundError( + f"YOLO model_path does not exist: {self.model_path}. " + "Set model_path:=... or AZAS_CUP_UPRIGHTING_MODEL_PATH." + ) + self.yolo = YOLO(self.model_path) log.info("YOLO 모델 로드 완료") # ── 카메라 구독 ── @@ -287,6 +309,22 @@ def _handle_key_extra(self, key: int): # ════════════════════════════════════════════ def initialize_home(self) -> bool: log = self.get_logger() + if self._skip_initial_home_move: + log.info("[Init] Home 이동 생략: 현재 로봇 자세를 관찰 시작 자세로 사용") + T = get_ee_matrix(self.robot) + self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) + qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() + self.home_ori = { + "x": float(qx), + "y": float(qy), + "z": float(qz), + "w": float(qw), + } + log.info(f"[Init] Current = ({T[0,3]:.3f}, {T[1,3]:.3f}, {T[2,3]:.3f}) m") + self.gripper.open_gripper() + time.sleep(1.0) + return True + log.info("[Init] Home 이동") if not self.go_home_pose(): log.error("Home 실패") diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_config.py b/src/azas_cup_uprighting/azas_cup_uprighting/_config.py index 4079399..e18b4a0 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_config.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_config.py @@ -2,7 +2,7 @@ import math import os import yaml -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import PackageNotFoundError, get_package_share_directory PKG_SHARE = get_package_share_directory('azas_cup_uprighting') @@ -57,7 +57,41 @@ def load_yaml(file_name): TOOLCHARGER_PORT = 502 # ── YOLO ──────────────────────────────────────────── -YOLO_MODEL_PATH = os.path.join(PKG_SHARE, 'config', 'best.pt') +def _default_yolo_model_path() -> str: + """Return a real model path without copying weights into install/. + + Historical cup-uprighting code looked for config/best.pt inside this + package. The current trained model is owned by azas_perception, so prefer + an explicit operator/env override and then the perception package asset. + """ + for env_name in ("AZAS_CUP_UPRIGHTING_MODEL_PATH", "AZAS_YOLO_MODEL_PATH", "MODEL_PATH"): + env_path = os.environ.get(env_name) + if env_path and os.path.isfile(os.path.expanduser(env_path)): + return os.path.expanduser(env_path) + + candidates = [ + os.path.join(PKG_SHARE, 'config', 'best.pt'), + ] + try: + perception_share = get_package_share_directory('azas_perception') + candidates.append( + os.path.join(perception_share, 'config', 'yolo_cup_uprighting_best.pt') + ) + except PackageNotFoundError: + pass + + repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), '..', '..', '..')) + candidates.extend([ + os.path.join(repo_root, 'src', 'azas_perception', 'config', 'yolo_cup_uprighting_best.pt'), + os.path.join(repo_root, 'local_models', 'best.pt'), + ]) + for candidate in candidates: + if os.path.isfile(candidate): + return candidate + return candidates[0] + + +YOLO_MODEL_PATH = _default_yolo_model_path() YOLO_CONF_THRESH = 0.5 AUTO_PICK_INTERVAL = 3.0 # 자동 모드 픽 간격 [s] diff --git a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py index c2b90ac..6648f31 100644 --- a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py +++ b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py @@ -1,8 +1,10 @@ from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare from moveit_configs_utils import MoveItConfigsBuilder @@ -27,6 +29,30 @@ def generate_launch_description(): moveit_py_params = PathJoinSubstitution( [FindPackageShare("azas_cup_uprighting"), "config", "moveit_py.yaml"] ) + model_path_arg = DeclareLaunchArgument( + "model_path", + default_value=PathJoinSubstitution([ + FindPackageShare("azas_perception"), + "config", + "yolo_cup_uprighting_best.pt", + ]), + description="YOLO weights for cup uprighting.", + ) + publish_hand_eye_tf_arg = DeclareLaunchArgument( + "publish_hand_eye_tf", + default_value="true", + description="Publish measured base_link -> camera_color_optical_frame TF.", + ) + auto_pick_arg = DeclareLaunchArgument( + "auto_pick", + default_value="false", + description="Automatically run the first detected fallen-cup upright sequence. false keeps manual p-key confirmation.", + ) + skip_initial_home_move_arg = DeclareLaunchArgument( + "skip_initial_home_move", + default_value="false", + description="Use the current robot pose as the camera observation pose without commanding Home first.", + ) # 3. 공통 안전/충돌 장면: side-grip, dispenser, cup-uprighting이 같은 바닥/벽/디스펜서 기준을 보도록 통일 workspace_collision_scene = IncludeLaunchDescription( @@ -48,6 +74,35 @@ def generate_launch_description(): }.items(), ) + world_base_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="cup_uprighting_world_base_tf", + output="screen", + arguments=[ + "--x", "0", + "--y", "0", + "--z", "0", + "--yaw", "0", + "--pitch", "0", + "--roll", "0", + "--frame-id", "world", + "--child-frame-id", "base_link", + ], + ) + + hand_eye_tf = Node( + package="azas_perception", + executable="hand_eye_static_tf_node", + name="cup_uprighting_hand_eye_static_tf_node", + output="screen", + condition=IfCondition(LaunchConfiguration("publish_hand_eye_tf")), + parameters=[{ + "compose_timeout_sec": 30.0, + "allow_direct_fallback": False, + }], + ) + # 4. 컵 직립화(Uprighting) 노드 실행 및 파라미터 주입 yolo_cup_uprighting_node = Node( package="azas_cup_uprighting", @@ -57,7 +112,24 @@ def generate_launch_description(): parameters=[ moveit_config.to_dict(), moveit_py_params, + { + "model_path": ParameterValue( + LaunchConfiguration("model_path"), + value_type=str, + ), + "auto_pick": LaunchConfiguration("auto_pick"), + "skip_initial_home_move": LaunchConfiguration("skip_initial_home_move"), + }, ], ) - return LaunchDescription([workspace_collision_scene, yolo_cup_uprighting_node]) \ No newline at end of file + return LaunchDescription([ + model_path_arg, + publish_hand_eye_tf_arg, + auto_pick_arg, + skip_initial_home_move_arg, + workspace_collision_scene, + world_base_tf, + hand_eye_tf, + yolo_cup_uprighting_node, + ]) diff --git a/src/azas_cup_uprighting/package.xml b/src/azas_cup_uprighting/package.xml index 59c6c88..a7efc79 100644 --- a/src/azas_cup_uprighting/package.xml +++ b/src/azas_cup_uprighting/package.xml @@ -15,6 +15,7 @@ moveit_py moveit_configs_utils dsr_moveit_config_m0609 + azas_perception python3-pymodbus python3-opencv diff --git a/src/azas_perception/azas_perception/lid_marker.py b/src/azas_perception/azas_perception/lid_marker.py index d801c8a..c7e9b24 100644 --- a/src/azas_perception/azas_perception/lid_marker.py +++ b/src/azas_perception/azas_perception/lid_marker.py @@ -143,21 +143,125 @@ def detect_aruco_marker( return None gray = cv2.cvtColor(patch, cv2.COLOR_BGR2GRAY) - dictionary = cv2.aruco.getPredefinedDictionary(dictionary_id) - parameters = cv2.aruco.DetectorParameters() - detector = cv2.aruco.ArucoDetector(dictionary, parameters) - corners_list, ids, _rejected = detector.detectMarkers(gray) + dictionary = _create_aruco_dictionary(dictionary_id) + parameters = _create_aruco_detector_parameters() + + # The lid marker appears small and oblique in the wrist-camera view. Try + # conservative contrast/scale variants, but keep the dictionary/id filter + # strict so a noisy table feature cannot become a false lid marker. + best: ArucoMarker | None = None + best_score = -1.0 + for candidate_gray, scale in _aruco_detection_images(gray): + corners_list, ids, _rejected = _detect_aruco_markers(candidate_gray, dictionary, parameters) + candidate = _select_aruco_marker_from_detections( + corners_list, + ids, + roi=roi, + desired_id=int(marker_id), + scale=scale, + ) + if candidate is not None and candidate.side_px > best_score: + best = candidate + best_score = candidate.side_px + return best + + +def _aruco_dictionary_id(dictionary_name: str) -> int | None: + name = str(dictionary_name).strip().upper() + if not name: + return None + if not name.startswith("DICT_"): + name = f"DICT_{name}" + return getattr(cv2.aruco, name, None) + + +def _create_aruco_dictionary(dictionary_id: int): + if hasattr(cv2.aruco, "getPredefinedDictionary"): + return cv2.aruco.getPredefinedDictionary(dictionary_id) + return cv2.aruco.Dictionary_get(dictionary_id) + + +def _create_aruco_detector_parameters(): + if hasattr(cv2.aruco, "DetectorParameters"): + parameters = cv2.aruco.DetectorParameters() + else: + parameters = cv2.aruco.DetectorParameters_create() + return _tune_lid_aruco_detector_parameters(parameters) + + +def _tune_lid_aruco_detector_parameters(parameters): + # The lid marker is small in the wrist-camera overview image and often seen + # at an angle. Keep the expected marker-id filter strict, but make candidate + # extraction and perspective sampling tolerant enough for the measured setup. + tuned_values = { + "adaptiveThreshWinSizeMin": 3, + "adaptiveThreshWinSizeMax": 53, + "adaptiveThreshWinSizeStep": 4, + "minMarkerPerimeterRate": 0.01, + "polygonalApproxAccuracyRate": 0.05, + "minCornerDistanceRate": 0.02, + "minDistanceToBorder": 1, + "perspectiveRemovePixelPerCell": 8, + "perspectiveRemoveIgnoredMarginPerCell": 0.20, + "errorCorrectionRate": 0.75, + "cornerRefinementMethod": getattr(cv2.aruco, "CORNER_REFINE_SUBPIX", 1), + "cornerRefinementWinSize": 3, + } + for name, value in tuned_values.items(): + if hasattr(parameters, name): + setattr(parameters, name, value) + return parameters + + +def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: + """Return grayscale variants for small/low-contrast lid ArUco detection. + + OpenCV returns corners in the coordinate system of the image it receives, + so each variant carries the scale needed to map corners back to the source + ROI. Variants are intentionally limited to deterministic contrast/scale + transforms; no dictionary or marker-id relaxation is performed. + """ + variants: list[tuple[np.ndarray, float]] = [(gray, 1.0)] + equalized = cv2.equalizeHist(gray) + variants.append((equalized, 1.0)) + + blur = cv2.GaussianBlur(gray, (0, 0), 1.0) + sharpened = cv2.addWeighted(gray, 1.6, blur, -0.6, 0) + variants.append((sharpened, 1.0)) + + # Upscaling materially helps when the marker body is only a few tens of + # pixels wide in the RealSense overview frame. + for source in (gray, equalized, sharpened): + variants.append((cv2.resize(source, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_CUBIC), 2.0)) + return variants + + +def _detect_aruco_markers(gray: np.ndarray, dictionary, parameters): + if hasattr(cv2.aruco, "ArucoDetector"): + detector = cv2.aruco.ArucoDetector(dictionary, parameters) + return detector.detectMarkers(gray) + return cv2.aruco.detectMarkers(gray, dictionary, parameters=parameters) + + +def _select_aruco_marker_from_detections( + corners_list, + ids, + *, + roi: ImageRoi, + desired_id: int, + scale: float, +) -> ArucoMarker | None: if ids is None or len(ids) == 0: return None - desired_id = int(marker_id) best: ArucoMarker | None = None best_score = -1.0 + inverse_scale = 1.0 / max(float(scale), 1e-9) for corners, marker_id_array in zip(corners_list, ids): detected_id = int(marker_id_array[0]) if desired_id >= 0 and detected_id != desired_id: continue - local_corners = np.asarray(corners, dtype=float).reshape(4, 2) + local_corners = np.asarray(corners, dtype=float).reshape(4, 2) * inverse_scale global_corners = local_corners + np.array([roi.x_min, roi.y_min], dtype=float) side_px = _aruco_side_px(global_corners) if side_px <= 0.0: @@ -176,15 +280,6 @@ def detect_aruco_marker( return best -def _aruco_dictionary_id(dictionary_name: str) -> int | None: - name = str(dictionary_name).strip().upper() - if not name: - return None - if not name.startswith("DICT_"): - name = f"DICT_{name}" - return getattr(cv2.aruco, name, None) - - def _aruco_side_px(corners: np.ndarray) -> float: if corners.shape != (4, 2): return 0.0 diff --git a/src/azas_perception/azas_perception/lid_sticker_detector_node.py b/src/azas_perception/azas_perception/lid_sticker_detector_node.py index e8dcd44..044061b 100644 --- a/src/azas_perception/azas_perception/lid_sticker_detector_node.py +++ b/src/azas_perception/azas_perception/lid_sticker_detector_node.py @@ -112,8 +112,9 @@ def __init__(self): self.declare_parameter("red_min_saturation", 80) self.declare_parameter("red_min_value", 40) self.declare_parameter("red_morph_kernel_px", 3) - self.declare_parameter("aruco_dictionary", "DICT_4X4_50") - self.declare_parameter("aruco_marker_id", -1) + self.declare_parameter("aruco_dictionary", "DICT_6X6_250") + self.declare_parameter("aruco_marker_id", 0) + self.declare_parameter("aruco_fallback_markers", "DICT_4X4_50:14") self.declare_parameter("aruco_marker_length_m", 0.03) self.declare_parameter("use_aruco_axis_for_orientation", True) self.declare_parameter("aruco_finger_axis_quarter_turns", 1) @@ -383,12 +384,23 @@ def _marker_roi(self, image: np.ndarray, lid: LidDetection2D | bool | None) -> I def _detect_marker(self, image: np.ndarray, marker_roi: ImageRoi) -> ArucoMarker | RedCircle | None: marker_type = self._marker_type() if marker_type == "aruco": - return detect_aruco_marker( - image, - marker_roi, - dictionary_name=str(self.get_parameter("aruco_dictionary").value), - marker_id=int(self.get_parameter("aruco_marker_id").value), - ) + for dictionary_name, marker_id in self._aruco_marker_candidates(): + marker = detect_aruco_marker( + image, + marker_roi, + dictionary_name=dictionary_name, + marker_id=marker_id, + ) + if marker is not None: + if ( + dictionary_name != str(self.get_parameter("aruco_dictionary").value) + or marker_id != int(self.get_parameter("aruco_marker_id").value) + ): + self.get_logger().info( + f"Detected fallback ArUco marker dictionary={dictionary_name} id={marker_id}" + ) + return marker + return None if marker_type == "red": marker = detect_red_circle_marker(image, marker_roi, self._red_circle_config()) if marker is not None: @@ -403,6 +415,40 @@ def _detect_marker(self, image: np.ndarray, marker_roi: ImageRoi) -> ArucoMarker ) return None + def _aruco_marker_candidates(self) -> list[tuple[str, int]]: + """Primary configured ArUco marker followed by explicit fallbacks. + + The lab has used both IsaacSim-style DICT_6X6_250/id0 and the earlier + lid-closing setup DICT_4X4_50/id14. Try only configured pairs so the + detector remains strict and does not accept arbitrary table markers. + """ + primary = ( + str(self.get_parameter("aruco_dictionary").value).strip() or "DICT_6X6_250", + int(self.get_parameter("aruco_marker_id").value), + ) + candidates: list[tuple[str, int]] = [primary] + raw = str(self.get_parameter("aruco_fallback_markers").value or "").strip() + for item in raw.replace(";", ",").split(","): + token = item.strip() + if not token: + continue + if ":" in token: + dictionary_name, marker_id_text = token.rsplit(":", 1) + elif "=" in token: + dictionary_name, marker_id_text = token.rsplit("=", 1) + else: + continue + dictionary_name = dictionary_name.strip() + try: + marker_id = int(marker_id_text.strip()) + except ValueError: + self.get_logger().warn(f"Ignoring invalid aruco_fallback_markers entry: {token!r}") + continue + candidate = (dictionary_name, marker_id) + if candidate not in candidates: + candidates.append(candidate) + return candidates + def _finger_axis_hint( self, marker: ArucoMarker | RedCircle, diff --git a/src/azas_perception/test/test_depth_and_detection_logic.py b/src/azas_perception/test/test_depth_and_detection_logic.py index f46922a..6bbff1d 100644 --- a/src/azas_perception/test/test_depth_and_detection_logic.py +++ b/src/azas_perception/test/test_depth_and_detection_logic.py @@ -91,6 +91,53 @@ def test_detect_aruco_marker_uses_configured_dictionary_and_roi(): assert marker.side_px == pytest.approx(59, abs=2) +def test_detect_aruco_marker_handles_small_6x6_lid_marker(): + image = np.full((240, 320, 3), 205, dtype=np.uint8) + dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250) + marker_image = cv2.aruco.generateImageMarker(dictionary, 0, 28) + + # Simulate the real wrist-camera view: a small marker body with a quiet-zone + # margin on a bright lid/table background. + marker_with_quiet_zone = np.full((36, 36), 255, dtype=np.uint8) + marker_with_quiet_zone[4:32, 4:32] = marker_image + marker_bgr = cv2.cvtColor(marker_with_quiet_zone, cv2.COLOR_GRAY2BGR) + image[40:76, 230:266] = marker_bgr + + marker = detect_aruco_marker( + image, + ImageRoi(0, 0, image.shape[1], image.shape[0]), + dictionary_name="DICT_6X6_250", + marker_id=0, + ) + + assert marker is not None + assert marker.marker_id == 0 + assert marker.center_u == pytest.approx(248, abs=2) + assert marker.center_v == pytest.approx(58, abs=2) + assert marker.side_px == pytest.approx(27, abs=3) + + +def test_detect_aruco_marker_handles_legacy_lid_marker_4x4_id14(): + image = np.full((240, 320, 3), 220, dtype=np.uint8) + dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + marker_image = cv2.aruco.generateImageMarker(dictionary, 14, 34) + marker_with_quiet_zone = np.full((44, 44), 255, dtype=np.uint8) + marker_with_quiet_zone[5:39, 5:39] = marker_image + image[35:79, 220:264] = cv2.cvtColor(marker_with_quiet_zone, cv2.COLOR_GRAY2BGR) + + marker = detect_aruco_marker( + image, + ImageRoi(0, 0, image.shape[1], image.shape[0]), + dictionary_name="DICT_4X4_50", + marker_id=14, + ) + + assert marker is not None + assert marker.marker_id == 14 + assert marker.center_u == pytest.approx(242, abs=2) + assert marker.center_v == pytest.approx(57, abs=2) + + def test_lid_normal_quaternion_points_local_z_to_normal(): qx, qy, qz, qw = quaternion_from_lid_normal(np.array([0.0, 0.0, -1.0])) local_z = np.array([ diff --git a/src/dsr_practice/dsr_practice/joint_state_relay.py b/src/dsr_practice/dsr_practice/joint_state_relay.py index c727f8c..4cc4de7 100644 --- a/src/dsr_practice/dsr_practice/joint_state_relay.py +++ b/src/dsr_practice/dsr_practice/joint_state_relay.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from sensor_msgs.msg import JointState @@ -32,9 +33,12 @@ def main(args=None): node = JointStateRelay() try: rclpy.spin(node) + except (ExternalShutdownException, KeyboardInterrupt): + pass finally: node.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() if __name__ == "__main__": diff --git a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py index 5f6f8de..f323995 100644 --- a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -209,6 +209,7 @@ def __init__(self): self.declare_parameter("target_class", "cup") self.declare_parameter("auto_pick", False) self.declare_parameter("auto_pick_interval", 3.0) + self.declare_parameter("exit_after_pick", False) self.declare_parameter("depth_patch_radius", 7) self.declare_parameter("min_depth_valid_ratio", 0.03) self.declare_parameter("min_depth_m", 0.15) @@ -347,6 +348,7 @@ def __init__(self): self.declare_parameter("motion_verify_tolerance", 0.01) self.declare_parameter("joint_goal_tolerance_rad", 0.02) self.declare_parameter("min_motion_delta_m", 0.005) + self.declare_parameter("skip_initial_home_move", False) self.declare_parameter("move_to_camera_home", True) self.declare_parameter("move_joint_home_before_camera_home", False) self.declare_parameter("camera_home_mode", "joint") @@ -374,6 +376,7 @@ def __init__(self): self.target_class = self.get_parameter("target_class").value self.auto_pick = parse_bool(self.get_parameter("auto_pick").value) self.auto_pick_interval = float(self.get_parameter("auto_pick_interval").value) + self.exit_after_pick = parse_bool(self.get_parameter("exit_after_pick").value) self.depth_patch_radius = int(self.get_parameter("depth_patch_radius").value) self.min_depth_valid_ratio = float( self.get_parameter("min_depth_valid_ratio").value @@ -551,6 +554,9 @@ def __init__(self): self.min_motion_delta_m = max( 0.0, float(self.get_parameter("min_motion_delta_m").value) ) + self.skip_initial_home_move = parse_bool( + self.get_parameter("skip_initial_home_move").value + ) self.move_to_camera_home = parse_bool( self.get_parameter("move_to_camera_home").value ) @@ -2236,7 +2242,14 @@ def draw_hud(self, image, detections): def run(self): log = self.get_logger() - if self.move_to_camera_home: + if self.skip_initial_home_move: + log.info("Skip initial home move; using current robot pose as camera home") + try: + transform = get_ee_matrix(self.robot) + self.update_home_orientation_from_matrix(transform) + except Exception as exc: + log.warning(f"Could not read current EE orientation before scan: {exc}") + elif self.move_to_camera_home: if self.move_joint_home_before_camera_home: log.info("Move JOINT HOME before high camera home") if not self.move_joint_home(): @@ -2259,6 +2272,9 @@ def run(self): while rclpy.ok(): rclpy.spin_once(self, timeout_sec=0.01) + if self.exit_after_pick and self.has_picked_once and not self.picking: + log.info("exit_after_pick=true and one pick completed; closing side_grip node") + break if self.color_image is None: continue diff --git a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py index cb5ab52..9401151 100644 --- a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py +++ b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py @@ -1,7 +1,9 @@ from copy import deepcopy from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue @@ -151,6 +153,20 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params ) ) + nodes.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("azas_bringup"), "launch", "rg2_link6_tcp.launch.py"] + ) + ), + launch_arguments={ + "publish_gripper_collision": LaunchConfiguration("link6_gripper_collision_enabled"), + }.items(), + condition=IfCondition(LaunchConfiguration("link6_gripper_collision_enabled")), + ) + ) + nodes.append( Node( package="dsr_practice", @@ -176,6 +192,7 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params value_type=str, ), "auto_pick_interval": LaunchConfiguration("auto_pick_interval"), + "exit_after_pick": LaunchConfiguration("exit_after_pick"), "depth_patch_radius": LaunchConfiguration("depth_patch_radius"), "min_depth_valid_ratio": LaunchConfiguration("min_depth_valid_ratio"), "min_depth_m": LaunchConfiguration("min_depth_m"), @@ -296,6 +313,9 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params "joint_goal_tolerance_rad": LaunchConfiguration( "joint_goal_tolerance_rad" ), + "skip_initial_home_move": LaunchConfiguration( + "skip_initial_home_move" + ), "move_to_camera_home": LaunchConfiguration("move_to_camera_home"), "move_joint_home_before_camera_home": LaunchConfiguration( "move_joint_home_before_camera_home" @@ -393,6 +413,11 @@ def generate_launch_description(): auto_pick_interval_arg = DeclareLaunchArgument( "auto_pick_interval", default_value="3.0" ) + exit_after_pick_arg = DeclareLaunchArgument( + "exit_after_pick", + default_value="false", + description="Exit the side-grip process after one successful pick so queued panel flows can continue.", + ) depth_patch_radius_arg = DeclareLaunchArgument( "depth_patch_radius", default_value="7" ) @@ -544,6 +569,14 @@ def generate_launch_description(): default_value="true", description="Publish RViz markers for dispenser collision boxes.", ) + link6_gripper_collision_enabled_arg = DeclareLaunchArgument( + "link6_gripper_collision_enabled", + default_value="true", + description=( + "Publish the RG2/link_6 attached collision envelope so MoveIt plans " + "with the mounted gripper, not only the bare robot flange." + ), + ) table_collision_enabled_arg = DeclareLaunchArgument( "table_collision_enabled", default_value="true", @@ -695,6 +728,11 @@ def generate_launch_description(): joint_goal_tolerance_rad_arg = DeclareLaunchArgument( "joint_goal_tolerance_rad", default_value="0.02" ) + skip_initial_home_move_arg = DeclareLaunchArgument( + "skip_initial_home_move", + default_value="false", + description="Start scanning from the current robot pose without commanding joint/camera home first.", + ) move_to_camera_home_arg = DeclareLaunchArgument( "move_to_camera_home", default_value="true" ) @@ -787,6 +825,7 @@ def generate_launch_description(): device_arg, target_class_arg, auto_pick_interval_arg, + exit_after_pick_arg, depth_patch_radius_arg, min_depth_valid_ratio_arg, min_depth_m_arg, @@ -820,6 +859,7 @@ def generate_launch_description(): dispenser_collision_publish_period_sec_arg, dispenser_collision_publish_objects_arg, dispenser_collision_publish_markers_arg, + link6_gripper_collision_enabled_arg, workspace_collision_scene_enabled_arg, workspace_collision_publish_period_sec_arg, table_collision_enabled_arg, @@ -853,6 +893,7 @@ def generate_launch_description(): verify_motion_arg, motion_verify_tolerance_arg, joint_goal_tolerance_rad_arg, + skip_initial_home_move_arg, move_to_camera_home_arg, move_joint_home_before_camera_home_arg, camera_home_mode_arg, diff --git a/tools/checks/check_panel_gripper_reconnect_and_force.py b/tools/checks/check_panel_gripper_reconnect_and_force.py index 5634684..9802390 100755 --- a/tools/checks/check_panel_gripper_reconnect_and_force.py +++ b/tools/checks/check_panel_gripper_reconnect_and_force.py @@ -22,8 +22,8 @@ def main() -> int: require(PANEL, "RG2_STACK_PATTERNS") require(PANEL, "elif step.key == \"connect_gripper\":") require(PANEL, "cleanup_rg2_stack()") - require(PANEL, "ros2 run azas_gripper rg2_gripper_node") - require(PANEL, "-p default_force_n:=30.0") + require(PANEL, "rg2_trigger.launch.py") + require(PANEL, "force:=300") require(PANEL, "{command: 'set_width', width_m: 0.075, force_n: 25.0}") require(PANEL, "-p gripper_close_force:=30.0") require(PANEL, "--gripper-force-n 25.0") diff --git a/tools/checks/check_panel_service_discovery_race.py b/tools/checks/check_panel_service_discovery_race.py index f038b13..dcc8c8c 100755 --- a/tools/checks/check_panel_service_discovery_race.py +++ b/tools/checks/check_panel_service_discovery_race.py @@ -47,6 +47,7 @@ def main() -> int: expected_color_scan_order = [ "connect_robot", "status_check", + "start_collision_scene", "move_to_color_scan_pose", "start_camera", "color_scan", @@ -80,12 +81,50 @@ def main() -> int: side_grip = next(step for step in panel.STEPS if step.key == "side_grip") side_grip_command = panel.command_for(side_grip, {"service_prefix": "dsr01"}) - expected_package_source = "install/dsr_practice/share/dsr_practice/package.bash" + expected_package_source = "tools/run/run_changhyun_side_grip_direct.sh" if expected_package_source not in side_grip_command: - print("[FAIL] side_grip command does not force the Azas dsr_practice overlay") + print("[FAIL] side_grip command does not use the field-tested direct runner") print(side_grip_command) return 1 + cup_uprighting = next(step for step in panel.STEPS if step.key == "cup_uprighting") + cup_uprighting_command = panel.command_for(cup_uprighting, {"service_prefix": "dsr01"}) + if "tools/run/run_somyeong_cup_uprighting_direct.sh" not in cup_uprighting_command: + print("[FAIL] cup_uprighting command does not use the direct runner") + print(cup_uprighting_command) + return 1 + if "colcon build" in cup_uprighting_command: + print("[FAIL] cup_uprighting command must not run colcon build from the panel") + print(cup_uprighting_command) + return 1 + + lid_grip_close = next(step for step in panel.STEPS if step.key == "lid_grip_close") + lid_grip_close_command = panel.command_for(lid_grip_close, {"service_prefix": "dsr01"}) + if "tools/run/run_kang_lid_grip_close_direct.sh" not in lid_grip_close_command: + print("[FAIL] lid_grip_close command does not use the direct runner") + print(lid_grip_close_command) + return 1 + + non_tmux_background = [ + step.key + for step in panel.STEPS + if step.kind == "background" and step.implemented and step.key not in panel.PANEL_TMUX_STEPS + ] + if non_tmux_background: + print("[FAIL] background steps must run through tmux, not panel-owned Popen") + print(non_tmux_background) + return 1 + + auto_build_steps = [ + step.key + for step in panel.STEPS + if step.implemented and "colcon build" in panel.command_for(step, {"service_prefix": "dsr01"}) + ] + if auto_build_steps: + print("[FAIL] panel commands must not run colcon build") + print(auto_build_steps) + return 1 + shake = next(step for step in panel.STEPS if step.key == "shake_closed_cup") required = panel.required_services_for_step(shake, "dsr01") calls = {"ros_service_names": 0} diff --git a/tools/perception/diagnose_aruco_marker.py b/tools/perception/diagnose_aruco_marker.py new file mode 100644 index 0000000..adaa834 --- /dev/null +++ b/tools/perception/diagnose_aruco_marker.py @@ -0,0 +1,215 @@ +#!/usr/bin/env python3 +"""Sample a ROS color image and report visible ArUco marker dictionaries/IDs.""" + +from __future__ import annotations + +import argparse +import json +import time +from pathlib import Path +from typing import Any + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image + + +DEFAULT_DICTIONARIES = [ + "DICT_4X4_50", + "DICT_4X4_100", + "DICT_4X4_250", + "DICT_5X5_50", + "DICT_5X5_100", + "DICT_5X5_250", + "DICT_6X6_50", + "DICT_6X6_100", + "DICT_6X6_250", + "DICT_7X7_50", + "DICT_7X7_100", + "DICT_7X7_250", +] + + +def image_to_bgr(msg: Image) -> np.ndarray: + encoding = msg.encoding.lower() + channels = 3 if encoding in {"rgb8", "bgr8"} else 1 + array = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, channels) + if encoding == "rgb8": + return cv2.cvtColor(array, cv2.COLOR_RGB2BGR) + if encoding == "bgr8": + return array.copy() + if encoding in {"mono8", "8uc1"}: + return cv2.cvtColor(array.reshape(msg.height, msg.width), cv2.COLOR_GRAY2BGR) + raise ValueError(f"unsupported image encoding: {msg.encoding}") + + +def aruco_dictionary(name: str): + dictionary_id = getattr(cv2.aruco, name, None) + if dictionary_id is None: + return None + if hasattr(cv2.aruco, "getPredefinedDictionary"): + return cv2.aruco.getPredefinedDictionary(dictionary_id) + return cv2.aruco.Dictionary_get(dictionary_id) + + +def aruco_parameters(): + if hasattr(cv2.aruco, "DetectorParameters"): + parameters = cv2.aruco.DetectorParameters() + else: + parameters = cv2.aruco.DetectorParameters_create() + tuned_values = { + "adaptiveThreshWinSizeMax": 53, + "perspectiveRemovePixelPerCell": 8, + } + for name, value in tuned_values.items(): + if hasattr(parameters, name): + setattr(parameters, name, value) + return parameters + + +def detect_markers(gray: np.ndarray, dictionary, parameters): + if hasattr(cv2.aruco, "ArucoDetector"): + detector = cv2.aruco.ArucoDetector(dictionary, parameters) + return detector.detectMarkers(gray) + return cv2.aruco.detectMarkers(gray, dictionary, parameters=parameters) + + +class ImageSampler(Node): + def __init__(self, topic: str): + super().__init__("azas_aruco_marker_diagnostic") + self.msg: Image | None = None + self.create_subscription(Image, topic, self._on_image, 10) + + def _on_image(self, msg: Image) -> None: + if self.msg is None: + self.msg = msg + + +def sample_image(topic: str, timeout_sec: float) -> Image: + node = ImageSampler(topic) + deadline = time.monotonic() + max(timeout_sec, 0.1) + try: + while rclpy.ok() and node.msg is None and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if node.msg is None: + raise RuntimeError(f"timed out waiting for image on {topic}") + return node.msg + finally: + node.destroy_node() + + +def marker_summary(corners: np.ndarray) -> dict[str, Any]: + points = np.asarray(corners, dtype=float).reshape(4, 2) + center = points.mean(axis=0) + side_lengths = [ + float(np.linalg.norm(points[(index + 1) % 4] - points[index])) + for index in range(4) + ] + return { + "center_u": round(float(center[0]), 2), + "center_v": round(float(center[1]), 2), + "side_px": round(float(np.mean(side_lengths)), 2), + "corners": [[round(float(x), 2), round(float(y), 2)] for x, y in points], + } + + +def diagnose(image_bgr: np.ndarray, dictionaries: list[str], expected_id: int) -> tuple[list[dict[str, Any]], np.ndarray]: + gray = cv2.cvtColor(image_bgr, cv2.COLOR_BGR2GRAY) + overlay = image_bgr.copy() + results: list[dict[str, Any]] = [] + for name in dictionaries: + dictionary = aruco_dictionary(name) + if dictionary is None: + results.append({"dictionary": name, "available": False, "markers": [], "rejected": 0}) + continue + corners_list, ids, rejected = detect_markers(gray, dictionary, aruco_parameters()) + markers = [] + if ids is not None: + for corners, marker_id_array in zip(corners_list, ids): + marker_id = int(marker_id_array[0]) + summary = marker_summary(corners) + summary["id"] = marker_id + summary["matches_expected_id"] = expected_id < 0 or marker_id == expected_id + markers.append(summary) + if markers: + cv2.aruco.drawDetectedMarkers(overlay, corners_list, ids) + results.append( + { + "dictionary": name, + "available": True, + "markers": markers, + "rejected": len(rejected) if rejected is not None else 0, + } + ) + return results, overlay + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--topic", default="/camera/camera/color/image_raw") + parser.add_argument("--timeout-sec", type=float, default=5.0) + parser.add_argument("--expected-dictionary", default="DICT_4X4_50") + parser.add_argument("--expected-id", type=int, default=14) + parser.add_argument("--all-dictionaries", action="store_true") + parser.add_argument("--debug-image", default="outputs/aruco_marker_diagnostic.jpg") + parser.add_argument("--json-output", default="outputs/aruco_marker_diagnostic.json") + return parser.parse_args() + + +def main() -> None: + args = parse_args() + rclpy.init() + try: + msg = sample_image(args.topic, args.timeout_sec) + finally: + if rclpy.ok(): + rclpy.shutdown() + + image_bgr = image_to_bgr(msg) + dictionaries = DEFAULT_DICTIONARIES if args.all_dictionaries else [args.expected_dictionary] + results, overlay = diagnose(image_bgr, dictionaries, args.expected_id) + payload = { + "topic": args.topic, + "encoding": msg.encoding, + "width": msg.width, + "height": msg.height, + "expected_dictionary": args.expected_dictionary, + "expected_id": args.expected_id, + "opencv_version": cv2.__version__, + "results": results, + } + + debug_path = Path(args.debug_image) + debug_path.parent.mkdir(parents=True, exist_ok=True) + cv2.imwrite(str(debug_path), overlay) + json_path = Path(args.json_output) + json_path.parent.mkdir(parents=True, exist_ok=True) + json_path.write_text(json.dumps(payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") + + expected_hits = [ + marker + for result in results + if result.get("dictionary") == args.expected_dictionary + for marker in result.get("markers", []) + if marker.get("matches_expected_id") + ] + any_hits = [ + (result.get("dictionary"), marker) + for result in results + for marker in result.get("markers", []) + ] + print(json.dumps(payload, ensure_ascii=False, indent=2)) + print(f"[Azas] debug_image={debug_path}") + print(f"[Azas] json_output={json_path}") + if expected_hits: + print(f"[PASS] expected marker visible: {args.expected_dictionary} id={args.expected_id}") + elif any_hits: + print("[WARN] ArUco marker(s) visible, but expected dictionary/id did not match") + else: + print("[FAIL] no ArUco marker detected in sampled color frame") + + +if __name__ == "__main__": + main() diff --git a/tools/run/direct_movej_joints.py b/tools/run/direct_movej_joints.py index 72a3449..b4f5996 100755 --- a/tools/run/direct_movej_joints.py +++ b/tools/run/direct_movej_joints.py @@ -11,7 +11,7 @@ from dataclasses import dataclass import rclpy -from dsr_msgs2.srv import MoveJoint +from dsr_msgs2.srv import CheckMotion, MoveJoint, MoveWait MOVE_MODE_ABSOLUTE = 0 @@ -65,6 +65,17 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--j5-max-deg", type=float, default=135.0, help="safe upper limit for joint 5") parser.add_argument("--timeout-sec", type=float, default=20.0, help="service response timeout") parser.add_argument("--wait-service-sec", type=float, default=5.0, help="service availability timeout") + parser.add_argument( + "--motion-timeout-sec", + type=float, + default=90.0, + help="time to wait until the robot reports motion complete after MoveJoint is accepted", + ) + parser.add_argument( + "--no-wait-motion", + action="store_true", + help="return after MoveJoint is accepted; unsafe for sequenced panel steps", + ) parser.add_argument( "--execute", action="store_true", @@ -78,6 +89,54 @@ def parse_args() -> argparse.Namespace: return parser.parse_args() +def namespaced_service(prefix: str, suffix: str) -> str: + clean = prefix.strip("/") + return f"/{clean}/{suffix}" if clean else f"/{suffix}" + + +def wait_until_motion_done(node, prefix: str, timeout_sec: float) -> tuple[bool, str]: + """Wait until the Doosan controller finishes the accepted command. + + Prefer the controller's MoveWait service because it blocks until motion + completion. If MoveWait is not exposed by a particular stack, fall back to + CheckMotion and require status=0. + """ + timeout_sec = max(timeout_sec, 0.1) + move_wait_name = namespaced_service(prefix, "motion/move_wait") + move_wait_client = node.create_client(MoveWait, move_wait_name) + if move_wait_client.wait_for_service(timeout_sec=1.0): + future = move_wait_client.call_async(MoveWait.Request()) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec) + if not future.done(): + return False, f"MoveWait timeout after {timeout_sec:.1f}s" + if future.exception() is not None: + return False, f"MoveWait exception: {future.exception()}" + response = future.result() + if response is None: + return False, "MoveWait returned no response" + if not bool(getattr(response, "success", True)): + return False, f"MoveWait returned success=false: {response}" + return True, "MoveWait completed" + + check_name = namespaced_service(prefix, "motion/check_motion") + check_client = node.create_client(CheckMotion, check_name) + if not check_client.wait_for_service(timeout_sec=1.0): + return False, f"neither MoveWait nor CheckMotion is available: {move_wait_name}, {check_name}" + + future = check_client.call_async(CheckMotion.Request()) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec) + if not future.done(): + return False, f"CheckMotion timeout after {timeout_sec:.1f}s" + if future.exception() is not None: + return False, f"CheckMotion exception: {future.exception()}" + response = future.result() + status = int(getattr(response, "status", -1)) + success = bool(getattr(response, "success", True)) + if success and status == 0: + return True, "CheckMotion status=0" + return False, f"CheckMotion not complete: status={status} success={success}" + + def main() -> int: args = parse_args() joints_deg = [float(getattr(args, f"j{index}")) for index in range(1, 7)] @@ -136,6 +195,15 @@ def main() -> int: print("[FAIL] MoveJoint returned success=false") return 1 print("[PASS] MoveJoint accepted by service") + if not args.no_wait_motion: + done, wait_output = wait_until_motion_done( + node, + args.service_prefix, + timeout_sec=float(args.motion_timeout_sec), + ) + print(f"[Azas] motion completion wait: {wait_output}") + if not done: + return 1 return 0 finally: node.destroy_node() diff --git a/tools/run/dispenser_color_scan_ros.sh b/tools/run/dispenser_color_scan_ros.sh index a768ae2..338133e 100755 --- a/tools/run/dispenser_color_scan_ros.sh +++ b/tools/run/dispenser_color_scan_ros.sh @@ -21,8 +21,13 @@ source_setup() { source_setup /opt/ros/humble/setup.bash source_setup "$ROOT/install/local_setup.bash" +mkdir -p "$ROOT/outputs" +# Fail closed against stale UI results: a new scan must create a new JSON. +rm -f "$ROOT/outputs/dispenser_color_map.json" "$ROOT/outputs/dispenser_color_map.json.failed" + +PYTHONUNBUFFERED=1 timeout "${AZAS_COLOR_SCAN_TIMEOUT_SEC:-18s}" \ python3 "$ROOT/tools/perception/dispenser_color_scan.py" --ros \ - --settle-sec "${AZAS_COLOR_SCAN_SETTLE_SEC:-1.5}" \ - --sample-frames "${AZAS_COLOR_SCAN_SAMPLE_FRAMES:-5}" \ + --settle-sec "${AZAS_COLOR_SCAN_SETTLE_SEC:-0.6}" \ + --sample-frames "${AZAS_COLOR_SCAN_SAMPLE_FRAMES:-3}" \ --debug-image "$ROOT/outputs/dispenser_color_scan_debug.jpg" \ --output "$ROOT/outputs/dispenser_color_map.json" diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 5908afb..60a55e2 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -8,8 +8,10 @@ import os import re import shlex +import shutil import signal import subprocess +import threading import time from dataclasses import asdict, dataclass from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer @@ -33,6 +35,8 @@ ROS_SETUP = ( "source /opt/ros/humble/setup.bash && " "mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && " + "export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && " + "export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && " "if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then " "source /home/ssu/ws_moveit/install/setup.bash; " "fi && " @@ -50,10 +54,15 @@ DEFAULT_RT_HOST = "0.0.0.0" DEFAULT_ROS_DOMAIN_ID = "9" DEFAULT_YOLO_MODEL_PATH = ROOT / "local_models" / "best.pt" +CUP_UPRIGHTING_YOLO_MODEL_PATH = ( + ROOT / "src" / "azas_perception" / "config" / "yolo_cup_uprighting_best.pt" +) PR20_YOLO_MODEL_PATH = DEFAULT_YOLO_MODEL_PATH DEFAULT_DISPENSER_TCP_NAME = "GripperDA_v1_jarvis" DEFAULT_LINK6_TCP_NAME = "azas_link6_tcp" CALIBRATION_CONFIG_PATH = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +HAND_EYE_TF_TARGET_FRAME = "base_link" +HAND_EYE_TF_SOURCE_FRAME = "camera_color_optical_frame" FAST_MOVE_VELOCITY = "30" FAST_MOVE_ACCELERATION = "30" RVIZ_PREVIEW_ROS_DOMAIN_ID = "79" @@ -174,6 +183,17 @@ def _normalize_color_map(raw: Any) -> dict[str, str]: return {key: normalized.get(key, "") for key in ("1", "2", "3", "4")} +def _file_timestamp(path: Path) -> dict[str, Any]: + if not path.exists(): + return {"exists": False, "mtime": None, "age_sec": None} + stat = path.stat() + return { + "exists": True, + "mtime": time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(stat.st_mtime)), + "age_sec": round(max(time.time() - stat.st_mtime, 0.0), 3), + } + + def _compact_dispenser_sequence(sequence: list[str]) -> str: groups: list[str] = [] index = 0 @@ -188,6 +208,15 @@ def _compact_dispenser_sequence(sequence: list[str]) -> str: return ",".join(groups) +def hand_eye_static_tf_command(*, compose_timeout_sec: float = 30.0) -> str: + """Start the measured hand-eye TF publisher without inventing camera poses.""" + return ( + "ros2 run azas_perception hand_eye_static_tf_node --ros-args " + f"-p compose_timeout_sec:={compose_timeout_sec:.1f} " + "-p allow_direct_fallback:=false" + ) + + def dispenser_color_map_status() -> dict[str, Any]: """Read outputs/dispenser_color_map.json and derive physical dispenser order. @@ -198,6 +227,8 @@ def dispenser_color_map_status() -> dict[str, Any]: """ issues: list[str] = [] + output_file = _file_timestamp(DISPENSER_COLOR_MAP_PATH) + failed_file = _file_timestamp(DISPENSER_COLOR_MAP_FAILED_PATH) failed_map: dict[str, str] | None = None if DISPENSER_COLOR_MAP_FAILED_PATH.exists(): try: @@ -222,6 +253,8 @@ def dispenser_color_map_status() -> dict[str, Any]: "sequence_compact": _compact_dispenser_sequence(fallback_sequence), "source": str(DISPENSER_COLOR_MAP_PATH), "failed_source": str(DISPENSER_COLOR_MAP_FAILED_PATH), + "output_file": output_file, + "failed_file": failed_file, "issues": issues, } @@ -242,6 +275,8 @@ def dispenser_color_map_status() -> dict[str, Any]: "sequence_compact": _compact_dispenser_sequence(fallback_sequence), "source": str(DISPENSER_COLOR_MAP_PATH), "failed_source": str(DISPENSER_COLOR_MAP_FAILED_PATH), + "output_file": output_file, + "failed_file": failed_file, "issues": issues, } @@ -307,6 +342,8 @@ def dispenser_color_map_status() -> dict[str, Any]: "sequence_compact": _compact_dispenser_sequence(sequence), "source": str(DISPENSER_COLOR_MAP_PATH), "failed_source": str(DISPENSER_COLOR_MAP_FAILED_PATH), + "output_file": output_file, + "failed_file": failed_file, "recipe_source": str(LATEST_RECIPE_PATH), "issues": issues, } @@ -368,6 +405,15 @@ class Step: False, "실제 로봇 real-mode bringup. 준비됨/시작중이면 유지하고, stale 상태일 때만 정리 후 시작", ), + Step( + "start_tmux_stack", + "tmux 연결 스택 시작", + "background", + "tools/run/start_azas_tmux_stack.sh", + True, + False, + "검증된 tmux 방식으로 azas-logic 세션에 로봇, RG2 그리퍼, RealSense, joint relay를 분리 시작. status_check/개별 연결 큐를 반복하지 않음", + ), Step("status_check", "연결 확인", "run", "ros2 service list | grep /dsr01/motion", True, False, "명령 후보만 있음: /dsr01/motion 서비스가 보여야 통과"), Step("connect_gripper", "그리퍼 연결", "background", "ros2 launch azas_gripper rg2_trigger.launch.py", True, False, "RG2 Trigger 서비스(/jarvis/rg2/open, close, set_width) 시작"), Step("start_camera", "RealSense 카메라 시작", "background", "ros2 launch realsense2_camera rs_launch.py", True, False, "RealSense 드라이버와 color/aligned-depth 토픽 시작; 화면 창은 별도 버튼 사용"), @@ -499,25 +545,25 @@ class Step: "tools/run/run_color_recipe_sequence.py --execute --confirm", True, True, - "latest_recipe.json + dispenser_color_map.json → 컵 놓기→dispenser_amounts 횟수 프레스→컵 다시 잡기/다음 디스펜서 이동을 한 단계로 실행", + "latest_recipe.json + dispenser_color_map.json → 컵 놓기→프레스→컵 다시 잡기/다음 디스펜서 이동. 재집기 Z 상승은 특이점 시 IK MoveJoint로 우회하고 legacy 저자세 직행 fallback은 사용하지 않음", ), Step( "side_grip", "PR #20 RealSense 컵 인식 후 side grip", "background", - "ros2 launch dsr_practice yolo_cup_pick_node.launch.py auto_pick:=false grasp_mode:=side moveit_controller_name:=/dsr01/dsr_moveit_controller", + "ros2 launch dsr_practice yolo_cup_pick_node.launch.py auto_pick:=false exit_after_pick:=false grasp_mode:=side moveit_controller_name:=/dsr01/dsr_moveit_controller", True, True, - "auto_pick=true: 컵 감지 즉시 자동 side-grip. 서버 command_for()가 실제 파라미터를 오버라이드함. 패널에서 OpenCV 창에서 확인 후 ESC 종료", + "OpenCV 창에서 컵 확인 후 p 키로 side-grip 실행. 장기 실행 GUI 노드라 패널에서는 tmux 창으로 분리 실행", ), Step( "cup_uprighting", "소명 누운 컵 세우기 / cup uprighting", - "run", + "background", "ros2 launch azas_cup_uprighting yolo_cup_uprighting.launch.py", True, True, - "RealSense + YOLO 기반 누운 컵 직립화. 실제 로봇 모션이며 side-grip 전에 선택 실행", + "RealSense + YOLO 기반 누운 컵 직립화. OpenCV 창에서 컵 확인 후 p 키로 실행, Esc/q로 종료하는 수동 실제모션 단계", ), Step("gripper_soft_grasp", "그리퍼 살짝 잡기", "run", "ros2 service call /jarvis/rg2/set_width azas_interfaces/srv/SetGripper", True, True, "큰 컵용: 완전 close 대신 폭 75mm/약한 힘으로 살짝 오므림"), Step( @@ -663,7 +709,7 @@ class Step: "", True, True, - "강개발자 로직: ArUco 14 뚜껑 pose를 p키로 확정한 뒤 RG2 파지→lift→teach point 이동→J6 단계 회전으로 뚜껑을 닫음", + "강개발자 로직: ArUco DICT_6X6_250 id0 뚜껑 pose를 p키로 확정한 뒤 RG2 파지→lift→teach point 이동→J6 단계 회전으로 뚜껑을 닫음", ), Step( "place_cup_holder", @@ -688,6 +734,44 @@ class Step: processes: dict[str, subprocess.Popen[str]] = {} process_logs: dict[str, Path] = {} +tmux_jobs: dict[str, dict[str, str]] = {} +RUN_LOCK = threading.Lock() +ROS_ENV_LOCK = threading.Lock() +ROS_ENV_CACHE: dict[str, str] | None = None +# Use the same tmux session as the field-tested manual workflow. Keeping the +# panel and terminal commands in one session avoids split ownership where the +# panel launches a second robot/camera stack while the operator is watching a +# different one. +PANEL_TMUX_SESSION = "azas-logic" +PANEL_TMUX_STEPS = { + "connect_robot", + "connect_gripper", + "start_camera", + "start_camera_view", + "detect_cup_lid", + "start_collision_scene", + "rviz_color_scan_pose_preview", + "voice_input", + "pick_lid", + "side_grip", + "cup_uprighting", + "lid_grip_close", + "shake_rviz_preview", +} +PANEL_DIRECT_TMUX_STEPS = { + # Match the successful field workflow: the panel opens the same tmux launch + # command and does not pre-block on slow ROS graph/service introspection. + # The launched node/MoveIt stack still performs the actual motion checks. + "side_grip", + "cup_uprighting", + "lid_grip_close", +} +PANEL_FIELD_VERIFIED_DIRECT_TMUX_STEPS = { + # Only commands that have been observed working from the same terminal/tmux + # mechanism are allowed to start from the panel. Static package/launch checks + # are not enough for real-motion GUI workflows. + "side_grip", +} DOOSAN_STACK_PATTERNS = ( "run_doosan_real_m0609.sh", @@ -703,6 +787,7 @@ class Step: "joint_state_relay_legacy", "dsr_practice/joint_state_relay", "joint_state_relay --ros-args", + "azas_joint_state_relay", "robot_state_publisher", "virtual_node", "move_group", @@ -715,6 +800,10 @@ class Step: "yolo_perception.launch.py", "azas_voice.launch.py", "rqt_image_view", + "lid_sticker_grip_planning.launch.py", + "lid_grip_planner_node", + "lid_sticker_detector_node", + "yolo_cup_uprighting.launch.py", "run_rule_based_shake_real.sh", "run_cup_target_then_shake_rviz.sh", "cup_target_then_shake_rviz.launch.py", @@ -725,12 +814,17 @@ class Step: "tumbler_shake_sequence_node", "shake_visualizer_node", "m0609_shake_joint_state_node", + "robot_connection_control.launch.py", + "yolo_to_floor_place.launch.py", + "tumbler_floor_place.launch.py", + "tumbler_floor_place_node", + "cup_detection_pose_bridge_node", + "hand_eye_static_tf_node", "measured_dispenser_collision_scene_node", "tumbler_collision_scene_node", "link6_gripper_collision_node", "rg2_link6_tcp.launch.py", "azas_rg2_link6_tcp_state_publisher", - "static_transform_publisher", "--frame-id world --child-frame-id base_link", ) @@ -742,7 +836,6 @@ class Step: "link6_gripper_collision_node", "rg2_link6_tcp.launch.py", "azas_rg2_link6_tcp_state_publisher", - "static_transform_publisher", "--frame-id world --child-frame-id base_link", ) @@ -762,20 +855,39 @@ class Step: "dsr_practice/yolo_cup_pick_node", "yolo_cup_pick_node --ros-args", "yolo_cup_pick_moveit_py", - "joint_state_relay_legacy", - "dsr_practice/joint_state_relay", - "joint_state_relay --ros-args", +) + +CUP_UPRIGHTING_STACK_PATTERNS = ( + "yolo_cup_uprighting.launch.py", + "azas_cup_uprighting/yolo_cup_uprighting", + "yolo_cup_uprighting --ros-args", + "yolo_cup_uprighting_py", +) + +LID_GRIP_STACK_PATTERNS = ( + "lid_grip_close", + "lid_sticker_detector_node", + "lid_grip_planner_node", + "lid_detection_pose_bridge_node", ) RUN_STEP_STACK_PATTERNS = ( "dispenser_press_node", "direct_movej_joints.py", + "dispenser_color_scan_ros.sh", + "run_one_click_cocktail_real.sh", + "run_cocktail_now_real.sh", + "run_cocktail_collision_rviz_preview.sh", + "stop_cocktail_motion_preview.sh", "rg2_full_open_verify.sh", "move_to_measured_dispenser_front_hold.py", "pick_from_measured_dispenser_front_hold.py", "run_measured_dispenser_recipe_sequence.py", + "run_color_recipe_sequence.py", "place_side_grip_cup_in_holder.py", "pick_from_cup_holder_side_grip.py", + "teach_measured_dispenser_front_hold.py", + "direct_movel_xyz.py", "ros2 service call /dsr01/", "ros2 control list_controllers", ) @@ -785,6 +897,16 @@ class Step: "run_robot_pipeline_control_panel.sh", ) +AGENT_PROTECTED_PATTERNS = ( + "codex", + "codex-linux-sandbox", + ".codex", + "omx", + "oh-my-codex", + "tmux", + "bwrap", +) + def command_line(proc: Any) -> str: try: @@ -820,17 +942,252 @@ def tail_file(path: Path | None, *, max_chars: int = 8000) -> str: return data.decode("utf-8", errors="replace") +def ros_command_env() -> dict[str, str]: + """Return a cached environment with ROS overlays already sourced. + + Panel status probes can call ros2 many times. Re-sourcing every workspace + for each probe adds about a second before the actual DDS/service operation + starts. Cache the sourced environment once per panel process and run short + ros2 commands inside that environment. + """ + + global ROS_ENV_CACHE + with ROS_ENV_LOCK: + if ROS_ENV_CACHE is not None: + return dict(ROS_ENV_CACHE) + script = ( + f"{ROS_SETUP} && " + "python3 - <<'PY'\n" + "import json, os\n" + "print(json.dumps(dict(os.environ)))\n" + "PY" + ) + completed = subprocess.run( + ["bash", "-lc", script], + cwd=str(ROOT), + env=os.environ.copy(), + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + timeout=8.0, + check=False, + ) + if completed.returncode != 0: + raise RuntimeError("failed to source ROS environment:\n" + completed.stdout[-4000:]) + try: + ROS_ENV_CACHE = {str(k): str(v) for k, v in json.loads(completed.stdout).items()} + except json.JSONDecodeError as exc: + raise RuntimeError("failed to parse sourced ROS environment:\n" + completed.stdout[-4000:]) from exc + return dict(ROS_ENV_CACHE) + + def background_log_path(step_key: str) -> Path: BACKGROUND_LOG_DIR.mkdir(parents=True, exist_ok=True) stamp = time.strftime("%Y%m%d-%H%M%S") return BACKGROUND_LOG_DIR / f"{step_key}-{stamp}.log" +def tmux_window_name(step_key: str) -> str: + return re.sub(r"[^A-Za-z0-9_.-]+", "-", step_key).strip("-")[:48] or "step" + + +def tmux_available() -> bool: + return shutil.which("tmux") is not None + + +def ensure_panel_tmux_session(env: dict[str, str]) -> None: + if not tmux_available(): + raise RuntimeError("tmux 명령을 찾을 수 없습니다.") + has_session = subprocess.run( + ["tmux", "has-session", "-t", PANEL_TMUX_SESSION], + cwd=str(ROOT), + env=env, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + check=False, + ) + if has_session.returncode != 0: + subprocess.run( + [ + "tmux", + "new-session", + "-d", + "-s", + PANEL_TMUX_SESSION, + "-n", + "monitor", + "bash -lc 'echo \"[Azas panel tmux] monitor\"; exec bash'", + ], + cwd=str(ROOT), + env=env, + check=True, + ) + subprocess.run( + ["tmux", "set-option", "-t", PANEL_TMUX_SESSION, "remain-on-exit", "on"], + cwd=str(ROOT), + env=env, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + check=False, + ) + + +def kill_panel_tmux_window(step_key: str, env: dict[str, str]) -> None: + if not tmux_available(): + return + subprocess.run( + ["tmux", "kill-window", "-t", f"{PANEL_TMUX_SESSION}:{tmux_window_name(step_key)}"], + cwd=str(ROOT), + env=env, + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + check=False, + ) + + +def capture_panel_tmux_window(step_key: str, env: dict[str, str], *, max_chars: int = 6000) -> str: + if not tmux_available(): + return "" + result = subprocess.run( + ["tmux", "capture-pane", "-t", f"{PANEL_TMUX_SESSION}:{tmux_window_name(step_key)}", "-p", "-S", "-220"], + cwd=str(ROOT), + env=env, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + check=False, + ) + return result.stdout[-max_chars:] + + +def start_panel_tmux_window(step_key: str, cmd: str, env: dict[str, str]) -> Path: + ensure_panel_tmux_session(env) + kill_panel_tmux_window(step_key, env) + log_path = background_log_path(f"tmux_{step_key}") + log_path.write_text(f"[Azas panel tmux] command: {cmd}\n\n", encoding="utf-8") + exports = " ".join( + f"export {name}={shlex.quote(str(env.get(name, '')))};" + for name in ("ROS_DOMAIN_ID", "ROS_LOCALHOST_ONLY", "ROBOT_HOST", "ROBOT_NAME", "SERVICE_PREFIX", "DISPLAY", "XAUTHORITY") + ) + shell_cmd = ( + f"{exports} cd {shlex.quote(str(ROOT))}; " + "set -o pipefail; " + f"({cmd}) 2>&1 | tee -a {shlex.quote(str(log_path))}; " + "rc=${PIPESTATUS[0]}; " + "echo; echo \"[Azas panel tmux] command exited rc=${rc}\" | tee -a " + f"{shlex.quote(str(log_path))}; " + "exec bash" + ) + subprocess.run( + [ + "tmux", + "new-window", + "-t", + PANEL_TMUX_SESSION, + "-n", + tmux_window_name(step_key), + "bash -lc " + shlex.quote(shell_cmd), + ], + cwd=str(ROOT), + env=env, + check=True, + ) + tmux_jobs[step_key] = { + "session": PANEL_TMUX_SESSION, + "window": tmux_window_name(step_key), + "log": str(log_path), + } + process_logs[step_key] = log_path + return log_path + + +def run_background_step_in_tmux( + step: Step, + cmd: str, + env: dict[str, str], + *, + restart_output: str = "", +) -> dict[str, Any]: + try: + log_path = start_panel_tmux_window(step.key, cmd, env) + except Exception as exc: + return { + "key": step.key, + "status": "failed", + "output": f"tmux 창 실행을 시작하지 못했습니다: {exc}\n--- command ---\n{cmd}", + } + + base_output = f"{cmd}\n--- tmux ---\nsession={PANEL_TMUX_SESSION} window={tmux_window_name(step.key)}\n--- log ---\n{log_path}" + if restart_output: + base_output = f"{restart_output}\n--- start command ---\n{base_output}" + + if step.key == "connect_robot": + output = ( + f"{base_output}\n" + "--- tmux tail ---\n" + f"{capture_panel_tmux_window(step.key, env, max_chars=4000)}\n" + "[Azas] robot tmux window started. 패널은 여기서 ROS graph/service 조회로 블로킹하지 않습니다. " + "준비 확인은 몇 초 뒤 '연결 확인'을 누르거나 tmux 로그를 보세요." + ) + return {"key": step.key, "status": "started", "output": output} + + if step.key == "connect_gripper": + output = ( + f"{base_output}\n" + "--- tmux tail ---\n" + f"{capture_panel_tmux_window(step.key, env, max_chars=4000)}\n" + "[Azas] gripper tmux window started. 서비스 확인은 status_check/로그에서 분리해서 봅니다." + ) + return {"key": step.key, "status": "started", "output": output} + + if step.key == "start_camera": + # RealSense launch is long-running and can publish normally while the + # local ros2cli graph daemon is stale or blocked. Keep the panel + # behavior aligned with the working terminal/tmux workflow: start the + # camera in its own window and let downstream vision nodes consume it. + output = ( + f"{base_output}\n" + "--- readiness ---\n" + "camera tmux window started; topic sampling is advisory and is not used as a panel blocking gate.\n" + "--- tmux tail ---\n" + f"{capture_panel_tmux_window(step.key, env, max_chars=4000)}" + ) + return {"key": step.key, "status": "started", "output": output} + + if step.key == "start_collision_scene": + output = ( + f"{base_output}\n" + "--- tmux tail ---\n" + f"{capture_panel_tmux_window(step.key, env, max_chars=4000)}\n" + "[Azas] collision scene tmux window started. TF/collision topic echo를 패널 실행 경로에서 블로킹하지 않습니다." + ) + return {"key": step.key, "status": "started", "output": output} + + output = base_output + "\n--- tmux tail ---\n" + capture_panel_tmux_window(step.key, env, max_chars=4000) + if step.key == "side_grip": + output += ( + "\n[Azas] side_grip은 tmux 창에서 OpenCV 화면을 띄워 대기합니다. " + "컵을 확인한 뒤 p 키를 누르면 잡기 동작이 실행됩니다. " + "디스펜서 collision은 켠 상태이며 pre_pick_joint1_clearance_deg=12.0으로 보정했습니다." + ) + return {"key": step.key, "status": "started", "output": output} + + def terminate_process_tree(proc: subprocess.Popen[str], *, label: str, grace_sec: float = 3.0) -> list[str]: """Terminate a Popen process and its children without killing the panel server.""" events: list[str] = [] if proc.poll() is not None: return events + try: + # Panel-spawned commands use start_new_session=True. Signal the whole + # process group first so ros2 launch children do not keep executing after + # the wrapper shell exits. + os.killpg(proc.pid, signal.SIGINT) + events.append(f"{label}: SIGINT process group pgid={proc.pid}") + except ProcessLookupError: + return events + except OSError as exc: + events.append(f"{label}: process-group SIGINT failed: {exc}") if psutil is not None: try: root = psutil.Process(proc.pid) @@ -850,17 +1207,82 @@ def terminate_process_tree(proc: subprocess.Popen[str], *, label: str, grace_sec except psutil.Error as exc: events.append(f"{label}: psutil tree cleanup failed: {exc}") - proc.send_signal(signal.SIGINT) try: proc.wait(timeout=grace_sec) except subprocess.TimeoutExpired: - proc.kill() - events.append(f"{label}: killed pid={proc.pid}") + try: + os.killpg(proc.pid, signal.SIGKILL) + events.append(f"{label}: SIGKILL process group pgid={proc.pid}") + except OSError: + proc.kill() + events.append(f"{label}: killed pid={proc.pid}") else: events.append(f"{label}: stopped pid={proc.pid}") return events +def protected_pids() -> set[int]: + """Return the panel process and its ancestors, which cleanup must not kill.""" + pids = {os.getpid()} + if psutil is None: + return pids + try: + current = psutil.Process(os.getpid()) + pids.update(parent.pid for parent in current.parents()) + except psutil.Error: + pass + return pids + + +def is_protected_process(proc: Any, protected: set[int] | None = None) -> bool: + """Protect the panel plus Codex/OMX/tmux agent processes from cleanup scans.""" + protected = protected or protected_pids() + if proc.pid in protected: + return True + cmd = command_line(proc) + if any(pattern in cmd for pattern in PANEL_PROTECTED_PATTERNS): + return True + lowered = cmd.lower() + return any(pattern in lowered for pattern in AGENT_PROTECTED_PATTERNS) + + +def terminate_psutil_tree(proc: Any, *, label: str, grace_sec: float = 3.0) -> list[str]: + """Terminate a matched stale process and its descendants, with agent guards.""" + events: list[str] = [] + protected = protected_pids() + if is_protected_process(proc, protected): + events.append(f"{label}: skip protected pid={proc.pid} cmd={command_line(proc)[:160]}") + return events + try: + targets = proc.children(recursive=True) + [proc] + except psutil.Error as exc: + events.append(f"{label}: inspect failed pid={proc.pid}: {exc}") + return events + + killable = [target for target in targets if not is_protected_process(target, protected)] + skipped = [target for target in targets if target not in killable] + for target in skipped: + events.append(f"{label}: skip protected pid={target.pid} cmd={command_line(target)[:160]}") + for target in killable: + try: + events.append(f"{label}: terminate pid={target.pid} cmd={command_line(target)[:160]}") + target.terminate() + except psutil.Error as exc: + events.append(f"{label}: terminate failed pid={target.pid}: {exc}") + + _, alive = psutil.wait_procs(killable, timeout=grace_sec) + for target in alive: + if is_protected_process(target, protected): + events.append(f"{label}: skip protected alive pid={target.pid} cmd={command_line(target)[:160]}") + continue + try: + events.append(f"{label}: kill pid={target.pid} cmd={command_line(target)[:160]}") + target.kill() + except psutil.Error as exc: + events.append(f"{label}: kill failed pid={target.pid}: {exc}") + return events + + def cleanup_doosan_stack(*, grace_sec: float = 3.0) -> list[str]: """Best-effort cleanup of stale Doosan/MoveIt graph processes before reconnect.""" events: list[str] = [] @@ -873,16 +1295,14 @@ def cleanup_doosan_stack(*, grace_sec: float = 3.0) -> list[str]: if psutil is None: return events - current_pid = os.getpid() + protected = protected_pids() candidates: list[Any] = [] for proc in psutil.process_iter(["pid", "cmdline", "name"]): - if proc.pid == current_pid: + if is_protected_process(proc, protected): continue cmd = command_line(proc) if not cmd: continue - if any(protected in cmd for protected in PANEL_PROTECTED_PATTERNS): - continue if any(pattern in cmd for pattern in DOOSAN_STACK_PATTERNS): candidates.append(proc) @@ -891,19 +1311,7 @@ def cleanup_doosan_stack(*, grace_sec: float = 3.0) -> list[str]: return events for proc in candidates: - events.append(f"cleanup: terminate pid={proc.pid} cmd={command_line(proc)[:160]}") - try: - proc.terminate() - except psutil.Error as exc: - events.append(f"cleanup: terminate failed pid={proc.pid}: {exc}") - - _, alive = psutil.wait_procs(candidates, timeout=grace_sec) - for proc in alive: - try: - events.append(f"cleanup: kill pid={proc.pid} cmd={command_line(proc)[:160]}") - proc.kill() - except psutil.Error as exc: - events.append(f"cleanup: kill failed pid={proc.pid}: {exc}") + events.extend(terminate_psutil_tree(proc, label="cleanup", grace_sec=grace_sec)) return events @@ -920,17 +1328,15 @@ def cleanup_matching_processes( events.append(f"{label}: psutil unavailable; only tracked panel processes can be stopped") return events - current_pid = os.getpid() + protected = protected_pids() candidates: list[Any] = [] seen: set[int] = set() for proc in psutil.process_iter(["pid", "cmdline", "name"]): - if proc.pid == current_pid or proc.pid in seen: + if proc.pid in seen or is_protected_process(proc, protected): continue cmd = command_line(proc) if not cmd: continue - if any(protected in cmd for protected in PANEL_PROTECTED_PATTERNS): - continue if any(pattern in cmd for pattern in patterns): candidates.append(proc) seen.add(proc.pid) @@ -940,19 +1346,7 @@ def cleanup_matching_processes( return events for proc in candidates: - try: - events.append(f"{label}: terminate pid={proc.pid} cmd={command_line(proc)[:160]}") - proc.terminate() - except psutil.Error as exc: - events.append(f"{label}: terminate failed pid={proc.pid}: {exc}") - - _, alive = psutil.wait_procs(candidates, timeout=grace_sec) - for proc in alive: - try: - events.append(f"{label}: kill pid={proc.pid} cmd={command_line(proc)[:160]}") - proc.kill() - except psutil.Error as exc: - events.append(f"{label}: kill failed pid={proc.pid}: {exc}") + events.extend(terminate_psutil_tree(proc, label=label, grace_sec=grace_sec)) return events @@ -991,10 +1385,29 @@ def cleanup_side_grip_stack(*, grace_sec: float = 2.0) -> list[str]: old = processes.pop("side_grip", None) if old is not None: events.extend(terminate_process_tree(old, label="stored side_grip", grace_sec=grace_sec)) + hand_eye = processes.pop("hand_eye_static_tf", None) + if hand_eye is not None: + events.extend(terminate_process_tree(hand_eye, label="stored hand_eye_static_tf", grace_sec=grace_sec)) events.extend(cleanup_matching_processes(SIDE_GRIP_STACK_PATTERNS, label="side_grip cleanup", grace_sec=grace_sec)) return events +def cleanup_cup_uprighting_stack(*, grace_sec: float = 2.0) -> list[str]: + """Best-effort cleanup of stale cup-uprighting nodes without killing TF/scene.""" + events: list[str] = [] + old = processes.pop("cup_uprighting", None) + if old is not None: + events.extend(terminate_process_tree(old, label="stored cup_uprighting", grace_sec=grace_sec)) + events.extend( + cleanup_matching_processes( + CUP_UPRIGHTING_STACK_PATTERNS, + label="cup_uprighting cleanup", + grace_sec=grace_sec, + ) + ) + return events + + def cleanup_collision_scene_stack(*, grace_sec: float = 2.0) -> list[str]: """Replace stale PlanningScene publishers before starting a shared scene. @@ -1128,10 +1541,11 @@ def robot_graph_ready(service_prefix: str) -> bool: def ros2_call(command: str, timeout_sec: float = 8.0) -> tuple[int, str]: - cmd = f"{ROS_SETUP} && timeout {max(timeout_sec, 0.1):.1f}s {command}" + cmd = f"timeout {max(timeout_sec, 0.1):.1f}s {command}" completed = subprocess.run( ["bash", "-lc", cmd], cwd=str(ROOT), + env=ros_command_env(), stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, @@ -1368,6 +1782,7 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: if step.key in {"home_robot", "lift_robot", "side_grip_camera_home", "move_to_color_scan_pose"}: return [ f"/{clean}/motion/move_joint", + f"/{clean}/motion/move_wait", f"/{clean}/motion/check_motion", f"/{clean}/system/get_robot_state", ] @@ -1650,6 +2065,159 @@ def wait_for_camera_topic_samples( ) +def wait_for_tf_transform( + *, + env: dict[str, str], + target_frame: str, + source_frame: str, + timeout_sec: float = 10.0, + proc: subprocess.Popen[str] | None = None, +) -> tuple[bool, str]: + """Wait until tf2 can transform source_frame into target_frame.""" + deadline = time.monotonic() + max(timeout_sec, 0.1) + last_output = "" + attempt = 0 + while time.monotonic() < deadline: + if proc is not None and proc.poll() is not None: + return False, "TF provider process exited while waiting\n" + tail_file(process_logs.get("hand_eye_static_tf")) + attempt += 1 + result = subprocess.run( + [ + "bash", + "-lc", + "timeout 2s ros2 run tf2_ros tf2_echo " + f"{shlex.quote(target_frame)} {shlex.quote(source_frame)}", + ], + cwd=str(ROOT), + env=env, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + timeout=3.0, + check=False, + ) + last_output = result.stdout[-2000:] + if "Translation:" in result.stdout and "Rotation:" in result.stdout: + return ( + True, + f"TF ready after {attempt} check(s): {target_frame} <- {source_frame}\n" + + last_output, + ) + time.sleep(0.5) + return ( + False, + f"TF not ready within {timeout_sec:.1f}s: {target_frame} <- {source_frame}\n" + f"--- last tf2_echo output ---\n{last_output}", + ) + + +def ensure_hand_eye_tf(env: dict[str, str], *, timeout_sec: float = 20.0) -> tuple[bool, str]: + """Ensure the measured hand-eye publisher connects the RealSense tree to base_link.""" + ready, output = wait_for_tf_transform( + env=env, + target_frame=HAND_EYE_TF_TARGET_FRAME, + source_frame=HAND_EYE_TF_SOURCE_FRAME, + timeout_sec=8.0, + ) + if ready: + return True, output + + events = [ + "hand-eye TF not currently available; starting measured hand_eye_static_tf_node", + output, + ] + proc = processes.get("hand_eye_static_tf") + if proc is None or proc.poll() is not None: + cmd = f"cd {ROOT} && {ROS_SETUP} && {hand_eye_static_tf_command(compose_timeout_sec=30.0)}" + log_path = background_log_path("hand_eye_static_tf") + log_handle = log_path.open("w", encoding="utf-8", buffering=1) + log_handle.write(f"[Azas panel] auto command: {cmd}\n\n") + proc = subprocess.Popen( + ["bash", "-lc", cmd], + cwd=str(ROOT), + env=env, + stdout=log_handle, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + log_handle.close() + processes["hand_eye_static_tf"] = proc + process_logs["hand_eye_static_tf"] = log_path + events.append(f"auto-started hand_eye_static_tf pid={proc.pid} log={log_path}") + else: + events.append(f"hand_eye_static_tf already running pid={proc.pid}") + + ready, wait_output = wait_for_tf_transform( + env=env, + target_frame=HAND_EYE_TF_TARGET_FRAME, + source_frame=HAND_EYE_TF_SOURCE_FRAME, + timeout_sec=timeout_sec, + proc=proc, + ) + events.append(wait_output) + if not ready: + events.append("--- hand_eye_static_tf log tail ---") + events.append(tail_file(process_logs.get("hand_eye_static_tf"))) + return ready, "\n".join(events) + + +def ensure_world_base_tf(env: dict[str, str], *, timeout_sec: float = 5.0) -> tuple[bool, str]: + """Ensure the MoveIt planning frame can reach the robot base frame.""" + ready, output = wait_for_tf_transform( + env=env, + target_frame="world", + source_frame="base_link", + timeout_sec=5.0, + ) + if ready: + return True, output + + events = [ + "world -> base_link TF not currently available; starting identity static TF", + output, + ] + proc = processes.get("world_base_static_tf") + if proc is None or proc.poll() is not None: + cmd = ( + f"cd {ROOT} && {ROS_SETUP} && " + "ros2 run tf2_ros static_transform_publisher " + "--x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 " + "--frame-id world --child-frame-id base_link" + ) + log_path = background_log_path("world_base_static_tf") + log_handle = log_path.open("w", encoding="utf-8", buffering=1) + log_handle.write(f"[Azas panel] auto command: {cmd}\n\n") + proc = subprocess.Popen( + ["bash", "-lc", cmd], + cwd=str(ROOT), + env=env, + stdout=log_handle, + stderr=subprocess.STDOUT, + text=True, + start_new_session=True, + ) + log_handle.close() + processes["world_base_static_tf"] = proc + process_logs["world_base_static_tf"] = log_path + events.append(f"auto-started world_base_static_tf pid={proc.pid} log={log_path}") + else: + events.append(f"world_base_static_tf already running pid={proc.pid}") + + ready, wait_output = wait_for_tf_transform( + env=env, + target_frame="world", + source_frame="base_link", + timeout_sec=timeout_sec, + proc=proc, + ) + events.append(wait_output) + if not ready: + events.append("--- world_base_static_tf log tail ---") + events.append(tail_file(process_logs.get("world_base_static_tf"))) + return ready, "\n".join(events) + + def wait_for_cup_detection_sample( *, env: dict[str, str], @@ -1795,7 +2363,10 @@ def side_grip_preflight(env: dict[str, str], service_prefix: str) -> tuple[bool, f"cd {ROOT} && {ROS_SETUP} && " "ros2 launch realsense2_camera rs_launch.py " "camera_name:=camera " - "enable_color:=true enable_depth:=true align_depth.enable:=true" + "initial_reset:=true reconnect_timeout:=5.0 " + "enable_color:=true enable_depth:=true align_depth.enable:=true " + "rgb_camera.color_profile:=640x480x30 " + "depth_module.depth_profile:=640x480x30" ) log_path = background_log_path("start_camera") log_handle = log_path.open("w", encoding="utf-8", buffering=1) @@ -1823,6 +2394,116 @@ def side_grip_preflight(env: dict[str, str], service_prefix: str) -> tuple[bool, action_ready, action_output = wait_for_action_server(action_name, timeout_sec=5.0) checks.append("--- MoveIt action ---\n" + action_output) if not action_ready: + checks.append( + "[WARN] MoveIt action introspection timed out. Continuing because " + "field runs can execute through the side-grip node even when ros2 action info is slow." + ) + + world_base_ready, world_base_output = ensure_world_base_tf(env, timeout_sec=5.0) + checks.append("--- world/base TF ---\n" + world_base_output) + if not world_base_ready: + ok = False + + tf_ready, tf_output = ensure_hand_eye_tf(env, timeout_sec=20.0) + checks.append("--- hand-eye TF ---\n" + tf_output) + if not tf_ready: + ok = False + + return ok, "\n".join(checks) + + +def cup_uprighting_preflight(env: dict[str, str], service_prefix: str) -> tuple[bool, str]: + """Fail closed before the cup-uprighting MoveItPy node can command motion.""" + checks: list[str] = [] + ok = True + + if CUP_UPRIGHTING_YOLO_MODEL_PATH.exists(): + checks.append(f"[OK] cup_uprighting YOLO model: {CUP_UPRIGHTING_YOLO_MODEL_PATH}") + else: + ok = False + checks.append(f"[FAIL] cup_uprighting YOLO model missing: {CUP_UPRIGHTING_YOLO_MODEL_PATH}") + + camera_ready, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=5.0) + checks.append("--- camera topics ---\n" + camera_output) + if not camera_ready: + ok = False + + clean = service_prefix.strip("/") or "dsr01" + action_name = f"/{clean}/dsr_moveit_controller/follow_joint_trajectory" + action_ready, action_output = wait_for_action_server(action_name, timeout_sec=5.0) + checks.append("--- MoveIt action ---\n" + action_output) + if not action_ready: + ok = False + + world_base_ready, world_base_output = ensure_world_base_tf(env, timeout_sec=5.0) + checks.append("--- world/base TF ---\n" + world_base_output) + if not world_base_ready: + ok = False + + tf_ready, tf_output = ensure_hand_eye_tf(env, timeout_sec=20.0) + checks.append("--- hand-eye TF ---\n" + tf_output) + if not tf_ready: + ok = False + + world_tf_ready, world_tf_output = wait_for_tf_transform( + env=env, + target_frame="world", + source_frame=HAND_EYE_TF_SOURCE_FRAME, + timeout_sec=5.0, + ) + checks.append("--- MoveIt planning-frame TF ---\n" + world_tf_output) + if not world_tf_ready: + ok = False + + return ok, "\n".join(checks) + + +def lid_grip_preflight(env: dict[str, str], service_prefix: str) -> tuple[bool, str]: + checks: list[str] = [] + ok = True + + for package, executable in ( + ("azas_perception", "lid_sticker_detector_node"), + ("azas_perception", "cup_detection_pose_bridge_node"), + ("azas_perception", "hand_eye_static_tf_node"), + ("azas_motion", "lid_grip_planner_node"), + ): + rc, output = ros2_call( + f"ros2 pkg executables {shlex.quote(package)}", + timeout_sec=3.0, + ) + line = f"{package} {executable}" + if rc == 0 and line in output: + checks.append(f"[OK] executable: {line}") + else: + ok = False + checks.append(f"[FAIL] executable missing: {line}\n{output}") + + clean = service_prefix.strip("/") or "dsr01" + required = [ + "/jarvis/rg2/set_width", + f"/{clean}/motion/move_line", + f"/{clean}/motion/move_joint", + f"/{clean}/motion/move_periodic", + f"/{clean}/motion/ikin", + f"/{clean}/motion/check_motion", + f"/{clean}/system/get_robot_state", + f"/{clean}/aux_control/get_current_posj", + f"/{clean}/aux_control/get_current_posx", + ] + services_ok, services_output = wait_for_required_services(required, timeout_sec=12.0) + checks.append("--- lid required services ---\n" + services_output) + if not services_ok: + ok = False + + world_base_ready, world_base_output = ensure_world_base_tf(env, timeout_sec=5.0) + checks.append("--- world/base TF ---\n" + world_base_output) + if not world_base_ready: + ok = False + + tf_ready, tf_output = ensure_hand_eye_tf(env, timeout_sec=20.0) + checks.append("--- hand-eye TF ---\n" + tf_output) + if not tf_ready: ok = False return ok, "\n".join(checks) @@ -1895,6 +2576,7 @@ def requires_doosan_motion(step: Step) -> bool: or step.key.startswith("move_to_dispenser_") or step.key.startswith("press_dispenser_") or step.key.startswith("pick_from_dispenser_") + or step.key == "run_color_recipe_sequence" or step.key == "place_cup_holder" ) @@ -1965,6 +2647,73 @@ def doosan_robot_ready(service_prefix: str) -> tuple[bool, str]: return True, "--- get_robot_state ---\n" + state_output + "\n--- check_motion ---\n" + motion_output +def real_motion_readiness_gate( + service_prefix: str, + *, + motion_timeout_sec: float = 35.0, +) -> tuple[bool, str]: + clean = service_prefix.strip("/") or "dsr01" + motion_ready, motion_output = wait_for_motion_services_ready( + clean, + timeout_sec=motion_timeout_sec, + ) + if not motion_ready: + return False, "--- motion services ---\n" + motion_output + robot_ready, robot_output = doosan_robot_ready(clean) + output = "--- motion services ---\n" + motion_output + "\n" + robot_output + if not robot_ready: + return False, output + action_name = f"/{clean}/dsr_moveit_controller/follow_joint_trajectory" + action_ready, action_output = wait_for_action_server(action_name, timeout_sec=8.0) + output += "\n--- MoveIt action ---\n" + action_output + if not action_ready: + return False, output + return True, output + + +def manual_logic_preflight(step: Step, env: dict[str, str], service_prefix: str) -> tuple[bool, str]: + checks: list[str] = [] + motion_ok, motion_output = real_motion_readiness_gate( + service_prefix, + motion_timeout_sec=25.0, + ) + checks.append(motion_output) + if not motion_ok: + return False, "\n".join(checks) + + required_gripper = ["/jarvis/rg2/open", "/jarvis/rg2/close", "/jarvis/rg2/set_width"] + gripper_ok, gripper_output = wait_for_required_services( + required_gripper, + timeout_sec=8.0, + ) + checks.append("--- gripper services ---\n" + gripper_output) + if not gripper_ok: + return False, "\n".join(checks) + + camera_ok, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=10.0) + checks.append("--- camera topics ---\n" + camera_output) + if not camera_ok: + return False, "\n".join(checks) + + if step.key == "side_grip": + side_ok, side_output = side_grip_preflight(env, service_prefix) + checks.append("--- side_grip preflight ---\n" + side_output) + if not side_ok: + return False, "\n".join(checks) + elif step.key == "cup_uprighting": + cup_ok, cup_output = cup_uprighting_preflight(env, service_prefix) + checks.append("--- cup_uprighting preflight ---\n" + cup_output) + if not cup_ok: + return False, "\n".join(checks) + elif step.key == "lid_grip_close": + lid_ok, lid_output = lid_grip_preflight(env, service_prefix) + checks.append("--- lid_grip_close preflight ---\n" + lid_output) + if not lid_ok: + return False, "\n".join(checks) + + return True, "\n".join(checks) + + def parse_numeric_array(text: str) -> list[float]: match = re.search(r"(?:data|pos)[:=]\s*(?:array\()?\[([^\]]+)\]", text, re.S) if not match: @@ -2059,8 +2808,6 @@ def requires_collision_scene_step(key: str) -> bool: "side_grip_camera_home", "lid_view_pose", "move_to_color_scan_pose", - "side_grip", - "lid_grip_close", "place_cup_holder", "shake_closed_cup", "run_color_recipe_sequence", @@ -2078,35 +2825,27 @@ def with_collision_scene_prereq(selected: list[str]) -> list[str]: def ensure_before(target: str, prerequisites: list[str]) -> None: if target not in ordered: return - # Keep prerequisites exactly once and immediately before the target. - # This prevents stale UI/manual selections from producing orders like - # start_camera -> connect_robot -> ... -> start_camera -> target. target_index = ordered.index(target) for prereq in prerequisites: if prereq in ordered: prereq_index = ordered.index(prereq) + if prereq_index < target_index: + continue ordered.pop(prereq_index) if prereq_index < target_index: target_index -= 1 target_index = ordered.index(target) - for offset, prereq in enumerate(prerequisites): + missing = [ + prereq + for prereq in prerequisites + if prereq not in ordered[:target_index] + ] + for offset, prereq in enumerate(missing): ordered.insert(target_index + offset, prereq) - # PR #20 side-grip is the cup acquisition step. Make the real-motion and - # perception prerequisites explicit, but do not move already-queued steps; - # this preserves the operator's full-flow order. - ensure_before( - "side_grip", - ["start_camera", "side_grip_camera_home"], - ) - ensure_before( - "cup_uprighting", - ["connect_robot", "status_check", "start_camera"], - ) - ensure_before( - "lid_grip_close", - ["connect_robot", "status_check", "connect_gripper", "start_camera", "lid_view_pose"], - ) + # 창현/소명/강개발자 수동 OpenCV 로직은 검증된 tmux stack을 사용한 뒤 + # 단독 버튼으로 실행한다. 서버가 connect/status/camera 단계를 다시 끼워 + # 넣으면 패널 실행 경로가 수동 tmux 경로와 달라지고 느려진다. # Color classification must aim the robot at the measured color-scan pose # before sampling the dispenser image. If the camera is already running from @@ -2124,22 +2863,34 @@ def ensure_before(target: str, prerequisites: list[str]) -> None: ["connect_robot", "status_check", "connect_gripper"], ) - if any(requires_collision_scene_step(key) for key in ordered): - ordered = [key for key in ordered if key != "start_collision_scene"] - first_collision_index = next( - ( - index - for index, key in enumerate(ordered) - if requires_collision_scene_step(key) - ), - 0, - ) - ordered.insert(first_collision_index, "start_collision_scene") + first_collision_index = next( + ( + index + for index, key in enumerate(ordered) + if requires_collision_scene_step(key) + ), + -1, + ) + if first_collision_index >= 0: + existing_scene_index = ( + ordered.index("start_collision_scene") if "start_collision_scene" in ordered else -1 + ) + if existing_scene_index < 0 or existing_scene_index > first_collision_index: + ordered = [key for key in ordered if key != "start_collision_scene"] + first_collision_index = next( + ( + index + for index, key in enumerate(ordered) + if requires_collision_scene_step(key) + ), + 0, + ) + ordered.insert(first_collision_index, "start_collision_scene") return list(dict.fromkeys(ordered)) def run_timeout_for_step(step: Step) -> float: if step.key == "side_grip": - return 900.0 + return 300.0 if step.key == "cup_uprighting": return 900.0 if step.key == "side_grip_camera_home": @@ -2235,6 +2986,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "echo '[Azas] starting RViz /joint_states relay " f"{relay_input} -> /joint_states'; " f"python3 {shlex.quote(str(relay_script_path))} --ros-args " + "-r __node:=azas_joint_state_relay " f"-p input_topic:={shlex.quote(relay_input)} " "-p output_topic:=/joint_states; " "else echo '[WARN] joint_state_relay.py not found; RViz may not mirror real robot joints'; fi" @@ -2250,46 +3002,71 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "wait $bringup_pid" ) return f"bash -lc {shlex.quote(bringup_script)}" + if step.key == "start_tmux_stack": + robot_host = str(payload.get("robot_host") or os.environ.get("ROBOT_HOST") or DEFAULT_ROBOT_HOST) + robot_name = str(payload.get("robot_name") or os.environ.get("ROBOT_NAME") or "dsr01") + rt_host = str(payload.get("rt_host") or os.environ.get("RT_HOST") or DEFAULT_RT_HOST) + rg2_ip = str(payload.get("rg2_ip") or os.environ.get("RG2_IP") or "192.168.1.1") + ros_domain_id = str(payload.get("ros_domain_id") or os.environ.get("AZAS_PANEL_ROS_DOMAIN_ID") or os.environ.get("ROS_DOMAIN_ID") or DEFAULT_ROS_DOMAIN_ID) + ros_localhost_only = str(os.environ.get("ROS_LOCALHOST_ONLY") or "0") + return ( + f"cd {ROOT} && " + f"SESSION={shlex.quote(PANEL_TMUX_SESSION)} " + f"ROS_DOMAIN_ID={shlex.quote(ros_domain_id)} " + f"ROS_LOCALHOST_ONLY={shlex.quote(ros_localhost_only)} " + f"ROBOT_HOST={shlex.quote(robot_host)} " + f"ROBOT_NAME={shlex.quote(robot_name)} " + f"RT_HOST={shlex.quote(rt_host)} " + f"RG2_IP={shlex.quote(rg2_ip)} " + f"bash {shlex.quote(str(ROOT / 'tools' / 'run' / 'start_azas_tmux_stack.sh'))}" + ) if step.key == "status_check": clean = service_prefix.strip("/") or "dsr01" return ( f"cd {ROOT} && {ROS_SETUP} && " - "echo '--- nodes ---' && ros2 node list && " + "echo '--- nodes ---' && " + "(timeout 0.5s ros2 node list || echo '[WARN] ros2 node list timed out; continuing with direct service checks') && " "echo '--- required motion service types ---' && " - f"ros2 service type /{clean}/motion/move_line && " - f"ros2 service type /{clean}/motion/move_joint && " + f"(timeout 0.5s ros2 service type /{clean}/motion/move_line || echo '[WARN] move_line service type lookup timed out') && " + f"(timeout 0.5s ros2 service type /{clean}/motion/move_joint || echo '[WARN] move_joint service type lookup timed out') && " "echo '--- robot state ---' && " - f"timeout 9s python3 {shlex.quote(str(ROOT / 'tools' / 'run' / 'ros_call_empty_service.py'))} " - f"/{clean}/system/get_robot_state dsr_msgs2/srv/GetRobotState --timeout 8.0 && " + f"timeout 4s python3 {shlex.quote(str(ROOT / 'tools' / 'run' / 'ros_call_empty_service.py'))} " + f"/{clean}/system/get_robot_state dsr_msgs2/srv/GetRobotState --timeout 3.0 && " "echo '--- check motion ---' && " - f"timeout 9s python3 {shlex.quote(str(ROOT / 'tools' / 'run' / 'ros_call_empty_service.py'))} " - f"/{clean}/motion/check_motion dsr_msgs2/srv/CheckMotion --timeout 8.0 && " + f"timeout 4s python3 {shlex.quote(str(ROOT / 'tools' / 'run' / 'ros_call_empty_service.py'))} " + f"/{clean}/motion/check_motion dsr_msgs2/srv/CheckMotion --timeout 3.0 && " "echo '--- trajectory action ---' && " - f"ros2 action info /{clean}/dsr_moveit_controller/follow_joint_trajectory && " + f"(timeout 0.5s ros2 action info /{clean}/dsr_moveit_controller/follow_joint_trajectory || " + "echo '[WARN] trajectory action info timed out') && " "echo '--- rviz joint_states relay sample ---' && " - "(timeout 3s ros2 topic echo /joint_states --once || " + "(timeout 0.5s ros2 topic echo /joint_states --once || " "echo '[WARN] no /joint_states sample; RViz robot model may stay frozen even while /dsr01/joint_states moves') && " "echo '--- lid/ArUco package executables ---' && " - "(ros2 pkg executables azas_perception | grep -E 'lid_sticker_detector_node|hand_eye_static_tf_node' || " + "(timeout 1s ros2 pkg executables azas_perception | grep -E 'lid_sticker_detector_node|hand_eye_static_tf_node' || " "echo '[WARN] azas_perception lid/hand-eye executables not visible') && " - "(ros2 pkg executables azas_motion | grep -E 'lid_grip_planner_node' || " + "(timeout 1s ros2 pkg executables azas_motion | grep -E 'lid_grip_planner_node' || " "echo '[WARN] azas_motion lid_grip_planner_node executable not visible') && " - "echo '--- lid/ArUco runtime nodes ---' && " - "(ros2 node list | grep -E '/lid_sticker_detector_node|/lid_detection_pose_bridge_node|/lid_grip_planner_node' || " - "echo '[INFO] lid_grip_close nodes are not running yet') && " - "echo '--- lid/ArUco topic samples ---' && " - "(timeout 2s ros2 topic echo /jarvis/lid_gripper/status --once || echo '[INFO] no /jarvis/lid_gripper/status sample yet') && " - "(timeout 2s ros2 topic echo /jarvis/lid_gripper/lid_pose --once || echo '[INFO] no /jarvis/lid_gripper/lid_pose sample yet')" + "echo '--- vision TF note ---' && " + "echo '[INFO] camera/hand-eye TF is checked after RealSense + MoveIt collision scene startup, not during core robot status_check.'" ) if step.key == "run_color_recipe_sequence": - recipe_dispenser_ids = str(payload.get("recipe_dispenser_ids") or "").strip() - direct_ids_arg = "" - if recipe_dispenser_ids: - direct_ids_arg = f" --dispenser-ids {shlex.quote(recipe_dispenser_ids)}" + recipe_override = str(payload.get("recipe_dispenser_ids") or "").strip() + direct_arg = "" + if recipe_override: + # Operators now enter color pump counts in the direct field, e.g. + # red1,blue3. Numeric diagnostics such as 1x2,3x2 still mean + # physical dispenser IDs; route only non-digit-leading tokens + # through --colors so x in 1x2 is not mistaken for a color name. + tokens = [token.strip() for token in re.split(r"[,;]+", recipe_override) if token.strip()] + numeric_dispenser_override = bool(tokens) and all(re.match(r"^[1-4](?:\s*(?:x|:)\s*\d+)?$", token.lower()) for token in tokens) + if numeric_dispenser_override: + direct_arg = f" --dispenser-ids {shlex.quote(recipe_override)}" + else: + direct_arg = f" --colors {shlex.quote(recipe_override)}" return ( f"cd {ROOT} && {ROS_SETUP} && " "python3 tools/run/run_color_recipe_sequence.py --execute --confirm" - f"{direct_ids_arg}" + f"{direct_arg}" ) if step.key == "rviz_cocktail_collision_preview": recipe_dispenser_ids = str(payload.get("recipe_dispenser_ids") or "").strip() @@ -2361,7 +3138,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"--j3 {shlex.quote(joints['j3'])} --j4 {shlex.quote(joints['j4'])} " f"--j5 {shlex.quote(joints['j5'])} --j6 {shlex.quote(joints['j6'])} " f"--velocity {FAST_MOVE_VELOCITY} --acceleration {FAST_MOVE_ACCELERATION} " - "--j5-min-deg -135 --j5-max-deg 135 --timeout-sec 60 " + "--j5-min-deg -135 --j5-max-deg 135 --timeout-sec 60 --motion-timeout-sec 120 " "--execute --confirm ENABLE_DIRECT_MOVEJ" ) if step.key == "side_grip_camera_home": @@ -2370,7 +3147,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"--service-prefix {service_prefix} " "--j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 " "--velocity 20 --acceleration 20 " - "--j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 " + "--j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 " "--execute --confirm ENABLE_DIRECT_MOVEJ" ) if step.key == "lid_view_pose": @@ -2379,7 +3156,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"--service-prefix {service_prefix} " "--j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 " "--velocity 15 --acceleration 15 " - "--j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 " + "--j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 " "--execute --confirm ENABLE_DIRECT_MOVEJ" ) if step.key == "home_robot": @@ -2387,7 +3164,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/direct_movej_joints.py " f"--service-prefix {service_prefix} --j1 0 --j2 0 --j3 90 " f"--j4 0 --j5 90 --j6 0 --velocity {FAST_MOVE_VELOCITY} --acceleration {FAST_MOVE_ACCELERATION} " - "--execute --confirm ENABLE_DIRECT_MOVEJ" + "--motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ" ) if step.key == "move_to_color_scan_pose": joints = measured_color_scan_joints() @@ -2397,7 +3174,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"--j1 {shlex.quote(joints['j1'])} --j2 {shlex.quote(joints['j2'])} " f"--j3 {shlex.quote(joints['j3'])} --j4 {shlex.quote(joints['j4'])} " f"--j5 {shlex.quote(joints['j5'])} --j6 {shlex.quote(joints['j6'])} " - "--velocity 30 --acceleration 30 --timeout-sec 60 " + "--velocity 30 --acceleration 30 --timeout-sec 60 --motion-timeout-sec 120 " "--execute --confirm ENABLE_DIRECT_MOVEJ" ) if step.key == "connect_gripper": @@ -2427,6 +3204,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "ros2 run tf2_ros static_transform_publisher " "--x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 " "--frame-id world --child-frame-id base_link & " + f"{hand_eye_static_tf_command(compose_timeout_sec=30.0)} & " "python3 -m azas_motion.tumbler_collision_scene_node --ros-args " "-p action:=publish_detected " "-p object_id:=detected_tumbler " @@ -2438,6 +3216,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"cd {ROOT} && {ROS_SETUP} && " "ros2 launch realsense2_camera rs_launch.py " "camera_name:=camera " + "initial_reset:=true reconnect_timeout:=5.0 " "enable_color:=true enable_depth:=true align_depth.enable:=true " "rgb_camera.color_profile:=640x480x30 " "depth_module.depth_profile:=640x480x30" @@ -2457,200 +3236,34 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "ros2 launch azas_bringup lid_sticker_grip_planning.launch.py" ) if step.key == "lid_grip_close": - clean_prefix = service_prefix if str(service_prefix).startswith("/") else f"/{service_prefix}" - model_path = DEFAULT_YOLO_MODEL_PATH if DEFAULT_YOLO_MODEL_PATH.is_file() else Path("/home/ssu/Downloads/best.pt") + direct_script = ROOT / "tools" / "run" / "run_kang_lid_grip_close_direct.sh" return ( - f"cd {ROOT} && {ROS_SETUP} && " - "ros2 pkg executables azas_perception | grep -q '^azas_perception lid_sticker_detector_node$' && " - "ros2 pkg executables azas_motion | grep -q '^azas_motion lid_grip_planner_node$' || " - "{ echo '[FAIL] lid/ArUco executables missing; build azas_perception azas_motion azas_bringup first'; exit 1; }; " + f"cd {ROOT} && " + f"SERVICE_PREFIX={shlex.quote(service_prefix)} " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " - "ros2 launch azas_bringup lid_sticker_grip_planning.launch.py " - f"model_path:={shlex.quote(str(model_path))} " - "marker_type:=aruco " - "require_lid_detection:=false " - "allow_aruco_only_after_grip_request:=false " - "aruco_only_after_grip_request_sec:=20.0 " - "aruco_dictionary:=DICT_4X4_50 " - "aruco_marker_id:=14 " - "aruco_marker_length_m:=0.03 " - "use_aruco_axis_for_orientation:=true " - "aruco_finger_axis_quarter_turns:=0 " - "use_lid_pose_yaw_for_pick:=true " - "lid_pose_yaw_axis:=y " - "lid_pose_yaw_offset_deg:=0.0 " - "lid_pose_yaw_equivalence_deg:=180.0 " - "visual_refine_before_grasp:=true " - "visual_refine_sample_count:=5 " - "visual_refine_timeout_sec:=3.0 " - "visual_refine_max_yaw_std_deg:=3.0 " - "visual_refine_max_position_std_m:=0.005 " - "visual_refine_apply_xy:=true " - "visual_refine_apply_yaw:=true " - "visual_refine_fallback_to_initial_plan:=true " - "enable_hardware:=true " - "hardware_confirm:=ENABLE_REAL_ROBOT_MOTION " - "allow_service_control_without_moveit:=true " - f"service_prefix:={shlex.quote(clean_prefix)} " - "rx:=108.41 " - "ry:=-176.32 " - "rz:=175.98 " - "offset_axis:=base_z " - "surface_offset_m:=0.0 " - "tcp_grasp_offset_x_m:=0.0 " - "tcp_grasp_offset_y_m:=0.0 " - "tcp_grasp_offset_z_m:=-0.040 " - "min_grasp_z_m:=0.025 " - "approach_offset_m:=0.08 " - "lift_offset_m:=0.10 " - "settle_seconds_before_grasp:=0.5 " - "hold_seconds_after_grasp:=3.0 " - "line_velocity:=30.0 " - "line_acceleration:=10.0 " - "move_timeout_sec:=90.0 " - "enable_gripper_service_calls:=true " - "gripper_set_service:=/jarvis/rg2/set_width " - "gripper_preopen_width_m:=0.110 " - "gripper_grasp_width_m:=0.020 " - "gripper_force_n:=12.0 " - "continue_after_gripper_grasp_failure:=true " - "gripper_grasp_failure_wait_sec:=2.0 " - "enable_lid_twist_after_grasp:=true " - "lid_twist_target_x_m:=0.422959106 " - "lid_twist_target_y_m:=0.223224869 " - "lid_twist_target_z_m:=0.166827988 " - "lid_twist_rx:=73.901489 " - "lid_twist_ry:=-178.542740 " - "lid_twist_rz:=117.385612 " - "lid_twist_transfer_clearance_m:=0.12 " - "lid_twist_transfer_max_z_m:=0.60 " - "lid_twist_use_force_control:=false " - "lid_twist_force_rotation_mode:=j6 " - "lid_twist_preseat_periodic_before_turn:=true " - "lid_twist_preseat_periodic_x_amp_mm:=0.0 " - "lid_twist_preseat_periodic_y_amp_mm:=0.0 " - "lid_twist_preseat_periodic_z_amp_mm:=1.0 " - "lid_twist_preseat_periodic_rx_amp_deg:=0.0 " - "lid_twist_preseat_periodic_ry_amp_deg:=0.0 " - "lid_twist_preseat_periodic_rz_amp_deg:=10.0 " - "lid_twist_preseat_periodic_period_sec:=3.6 " - "lid_twist_preseat_periodic_acc_time_sec:=1.0 " - "lid_twist_preseat_periodic_repeat:=2 " - "lid_twist_preseat_periodic_ref:=tool " - "lid_twist_rz_delta_deg:=300.0 " - "lid_twist_turn_step_deg:=50.0 " - "lid_twist_release_lift_m:=0.03 " - "lid_twist_min_z_m:=0.140 " - "lid_twist_max_z_m:=0.220 " - "lid_twist_transfer_velocity:=25.0 " - "lid_twist_press_velocity:=5.0 " - "lid_twist_turn_velocity:=30.0 " - "lid_twist_acceleration:=15.0 " - "lid_twist_hold_seconds_before_turn:=0.0 " - "lid_twist_hold_seconds_after_turn:=0.5" + f"bash {shlex.quote(str(direct_script))}" ) if step.key == "cup_uprighting": + direct_script = ROOT / "tools" / "run" / "run_somyeong_cup_uprighting_direct.sh" return ( - f"cd {ROOT} && {ROS_SETUP} && " - "if ! ros2 pkg prefix azas_cup_uprighting >/dev/null 2>&1 || " - "! ros2 pkg executables azas_perception | grep -q '^azas_perception hand_eye_static_tf_node$'; then " - "echo '[Azas] cup_uprighting/hand_eye 실행파일이 없어 필요한 패키지를 빌드합니다.'; " - "colcon build --symlink-install --packages-select azas_perception azas_bringup azas_cup_uprighting || exit 1; " - "source install/setup.bash; " - "fi && " - "ros2 pkg executables azas_perception | grep -q '^azas_perception hand_eye_static_tf_node$' || " - "{ echo '[Azas] hand_eye_static_tf_node still missing after build'; exit 1; }; " - f"ros2 launch azas_cup_uprighting yolo_cup_uprighting.launch.py " - f"service_prefix:={shlex.quote(service_prefix)} " - "enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION " - "run_yolo:=true publish_hand_eye_tf:=true" + f"cd {ROOT} && " + f"SERVICE_PREFIX={shlex.quote(service_prefix)} " + "DISPLAY=${DISPLAY:-:0} " + "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " + f"MODEL_PATH={shlex.quote(str(CUP_UPRIGHTING_YOLO_MODEL_PATH))} " + f"bash {shlex.quote(str(direct_script))}" ) if step.key == "voice_input": return f"cd {ROOT} && {ROS_SETUP} && ros2 launch azas_voice azas_voice.launch.py" if step.key == "side_grip": - relay_script_path = ROOT / "src" / "dsr_practice" / "dsr_practice" / "joint_state_relay.py" - side_grip_prefix = ( - "ros2 run tf2_ros static_transform_publisher " - "--x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 " - "--frame-id world --child-frame-id base_link & " - "(sleep 5; " - f"if [ -f {shlex.quote(str(relay_script_path))} ]; then " - "echo '[Azas] starting side_grip RViz /joint_states relay /dsr01/joint_states -> /joint_states'; " - f"python3 {shlex.quote(str(relay_script_path))} --ros-args " - "-p input_topic:=/dsr01/joint_states -p output_topic:=/joint_states; " - "else echo '[WARN] joint_state_relay.py not found; RViz may not mirror real robot joints'; fi" - ") & " - ) + direct_script = ROOT / "tools" / "run" / "run_changhyun_side_grip_direct.sh" return ( f"cd {ROOT} && " - "source /opt/ros/humble/setup.bash && " - "source /home/ssu/ws_moveit/install/setup.bash && " - "source /home/ssu/ros2_ws/install/setup.bash && " - "if [ \"${AZAS_SIDE_GRIP_BUILD:-0}\" = \"1\" ]; then " - "colcon build --symlink-install --packages-select dsr_practice; " - "fi && " - f"source {shlex.quote(str(ROOT / 'install' / 'local_setup.bash'))} && " - f"source {shlex.quote(str(ROOT / 'install' / 'dsr_practice' / 'share' / 'dsr_practice' / 'package.bash'))} && " - f"export PYTHONPATH={shlex.quote(str(ROOT / 'tools' / 'run' / 'python_compat'))}:${{PYTHONPATH:-}} && " - "(" - f"{side_grip_prefix}" + f"SERVICE_PREFIX={shlex.quote(service_prefix)} " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " - f"ros2 launch {shlex.quote(str(ROOT / 'install' / 'dsr_practice' / 'share' / 'dsr_practice' / 'launch' / 'yolo_cup_pick_node.launch.py'))} " - f"model_path:={shlex.quote(str(DEFAULT_YOLO_MODEL_PATH))} " - "conf:=0.35 " - "imgsz:=640 " - "device:=cpu " - "target_class:=cup " - "auto_pick:=false " - "auto_pick_interval:=8.0 " - "depth_patch_radius:=7 " - "min_depth_valid_ratio:=0.03 " - "min_depth_m:=0.15 " - "max_depth_m:=1.20 " - "redetect_on_approach:=false " - "redetect_settle_sec:=0.5 " - "grasp_mode:=side " - "side_far_stage_enabled:=false " - "side_approach_offset:=0.18 " - "side_short_stage_backoff_m:=0.08 " - "side_grasp_stop_backoff_m:=0.04 " - "side_close_underreach_m:=0.03 " - "side_low_retry_lift_m:=0.0 " - "side_low_retry_attempts:=0 " - "side_linear_approach_enabled:=true " - "side_final_slide_enabled:=false " - "side_fixed_grasp_z_enabled:=true " - "side_fixed_grasp_z:=0.07 " - "side_project_bbox_center_to_fixed_z:=true " - "side_candidate_plan_check_enabled:=true " - "side_move_to_initial_center_before_close:=false " - "verify_motion:=false " - "move_to_camera_home:=true " - "move_joint_home_before_camera_home:=false " - "camera_home_mode:=joint " - "min_motion_z:=0.07 " - "workspace_xy_clamp_enabled:=false " - "return_home_after_task:=false " - "return_to_camera_home_after_attempt:=true " - "workspace_collision_scene_enabled:=true " - "table_collision_enabled:=true " - "table_surface_z:=0.0 " - "table_thickness:=0.04 " - "table_size_x:=1.10 " - "table_size_y:=0.65 " - "table_center_x:=0.29 " - "table_center_y:=0.0 " - "table_collision_expand_to_workspace_walls:=true " - "workspace_boundary_collision_enabled:=true " - "dispenser_collision_enabled:=true " - "dispenser_collision_publish_objects:=true " - "dispenser_collision_publish_markers:=true " - f"dispenser_collision_config_path:={shlex.quote(str(ROOT / 'src' / 'azas_bringup' / 'config' / 'measured_dispenser_collision.yaml'))} " - "moveit_controller_name:=/dsr01/dsr_moveit_controller " - "start_joint_state_relay:=false" - ")" + f"bash {shlex.quote(str(direct_script))}" ) if step.key == "gripper_soft_grasp": return ( @@ -2876,6 +3489,66 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: return {"key": step.key, "status": "blocked", "output": step.note} if step.real_motion and not payload.get("armed"): return {"key": step.key, "status": "blocked", "output": "실제 모션 허용 체크가 꺼져 있습니다."} + if step.key in PANEL_DIRECT_TMUX_STEPS: + env = shell_env(payload) + service_prefix = str(payload.get("service_prefix") or "dsr01") + if step.key not in PANEL_FIELD_VERIFIED_DIRECT_TMUX_STEPS: + cmd = command_for(step, payload) + return { + "key": step.key, + "status": "blocked", + "output": ( + "이 단계는 패널에 저장된 명령 후보는 있지만, 아직 동일한 terminal/tmux 방식으로 " + "실제 성공 검증이 끝나지 않아 패널에서 실행하지 않았습니다.\n" + "먼저 터미널/tmux에서 성공 로그를 확인한 뒤 패널 허용 목록에 올려야 합니다.\n" + "--- command candidate ---\n" + f"{cmd}\n" + ), + } + cleanup_output = "" + if step.key == "side_grip": + cleanup_output = "\n".join(cleanup_side_grip_stack(grace_sec=3.0)) + time.sleep(1.0) + cmd = command_for(step, payload) + restart_output = "\n".join( + part + for part in ( + "[Azas] field-verified tmux mode: 창현 side_grip은 ROS CLI discovery preflight로 막지 않고 검증된 tmux 명령을 직접 실행합니다.", + "[Azas] 전제: 먼저 'tmux 연결 스택 시작'으로 robot/gripper/camera/joint_relay 창이 떠 있어야 합니다.", + cleanup_output, + ) + if part + ) + return run_background_step_in_tmux(step, cmd, env, restart_output=restart_output) + elif step.key == "cup_uprighting": + cleanup_output = "\n".join(cleanup_cup_uprighting_stack(grace_sec=3.0)) + time.sleep(1.0) + elif step.key == "lid_grip_close": + cleanup_output = "\n".join( + cleanup_matching_processes(LID_GRIP_STACK_PATTERNS, label="lid_grip cleanup", grace_sec=3.0) + ) + time.sleep(1.0) + preflight_ok, preflight_output = manual_logic_preflight(step, env, service_prefix) + if not preflight_ok: + return { + "key": step.key, + "status": "blocked", + "output": ( + "패널 수동 로직 실행 전 준비 조건이 충족되지 않아 시작하지 않았습니다.\n" + + (cleanup_output + "\n" if cleanup_output else "") + + preflight_output + ), + } + cmd = command_for(step, payload) + restart_output = "\n".join( + [ + "[Azas] direct tmux mode: 최소 준비 게이트 통과 후 현장 tmux launch 명령을 실행합니다.", + "[Azas] 확인됨: motion services, robot_state=STANDBY, check_motion, MoveIt action, gripper services, camera topics.", + cleanup_output, + preflight_output, + ] + ) + return run_background_step_in_tmux(step, cmd, env, restart_output=restart_output) if step.real_motion and step.key not in {"run_one_click_cocktail_real", "run_cocktail_now_real"}: service_prefix = str(payload.get("service_prefix") or "dsr01") gripper_ready, gripper_output = ensure_gripper_services(step, payload, service_prefix) @@ -2917,17 +3590,17 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: if step.key == "side_grip": clean = service_prefix.strip("/") or "dsr01" action_name = f"/{clean}/dsr_moveit_controller/follow_joint_trajectory" - action_ready, action_output = wait_for_action_server(action_name, timeout_sec=15.0) + action_ready, action_output = wait_for_action_server(action_name, timeout_sec=3.0) if not action_ready: - return { - "key": step.key, - "status": "blocked", - "output": ( - "MoveIt trajectory action server가 없어 side_grip 실제 동작을 막았습니다.\n" - "로봇 연결을 다시 시작해서 dsr_moveit_controller action server가 뜨는지 확인하세요.\n" - f"{action_output}" - ), - } + # ROS action graph introspection can stall on the field setup + # even when MoveIt execution works. The side-grip node still + # performs its own MoveIt planning/execution checks, so this + # panel gate is advisory only. + print( + "[Azas panel] warning: side_grip action introspection timed out; continuing\n" + + action_output, + flush=True, + ) if step.key == "connect_robot" and not ( payload.get("robot_host") or os.environ.get("ROBOT_HOST") or DEFAULT_ROBOT_HOST ): @@ -2957,6 +3630,27 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: f"{preflight_output}" ), } + if step.key == "cup_uprighting": + cleanup_events = cleanup_cup_uprighting_stack(grace_sec=3.0) + time.sleep(1.0) + service_prefix = str(payload.get("service_prefix") or "dsr01") + preflight_ok, preflight_details = cup_uprighting_preflight(env, service_prefix) + preflight_output = "\n".join( + cleanup_events + + [ + "--- cup_uprighting preflight ---", + preflight_details, + ] + ).strip() + if not preflight_ok: + return { + "key": step.key, + "status": "blocked", + "output": ( + "cup_uprighting 실행 전 조건이 충족되지 않아 시작하지 않았습니다.\n" + f"{preflight_output}" + ), + } cmd = command_for(step, payload) if step.kind == "background": @@ -2964,7 +3658,26 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: if step.key == "connect_robot": ready, ready_output, _svc = motion_services_ready(env["SERVICE_PREFIX"]) if ready: + robot_ready, robot_ready_output = doosan_robot_ready(env["SERVICE_PREFIX"]) virtual_present, virtual_output = doosan_virtual_nodes_present(env["SERVICE_PREFIX"]) + if robot_ready: + note = "" + if virtual_present: + note = ( + "\n[INFO] /virtual_node 이름이 보이지만 robot_state=STATE_STANDBY(1) " + "및 check_motion success=True라서 real bringup을 유지합니다. " + "Doosan real launch에서도 이 노드명이 보일 수 있어 이름만으로 재시작하지 않습니다.\n" + f"{virtual_output}" + ) + return { + "key": step.key, + "status": "running", + "output": ( + "이미 Doosan motion 서비스가 보이고 로봇이 STATE_STANDBY(1)입니다. " + "재시작하지 않습니다.\n" + f"{ready_output}\n{robot_ready_output}{note}" + ), + } if virtual_present: cleanup_events = cleanup_doosan_stack(grace_sec=3.0) restart_output = ( @@ -2975,27 +3688,16 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: + "\n" ) else: - robot_ready, robot_ready_output = doosan_robot_ready(env["SERVICE_PREFIX"]) - if not robot_ready: - return { - "key": step.key, - "status": "blocked", - "output": ( - "Doosan motion 서비스는 보이지만 로봇이 motion-ready 상태가 아닙니다. " - "재시작하지 않습니다.\n" - f"{ready_output}\n" - "티치펜던트/컨트롤러에서 빨간 상태(SAFE_OFF/보호정지/서보 상태)를 해제해 " - "STATE_STANDBY(1)로 만든 뒤 다시 확인하세요.\n" - f"{robot_ready_output}" - ), - } return { "key": step.key, - "status": "running", + "status": "blocked", "output": ( - "이미 실제 Doosan motion 서비스가 보이고 로봇이 STATE_STANDBY(1)입니다. " + "Doosan motion 서비스는 보이지만 로봇이 motion-ready 상태가 아닙니다. " "재시작하지 않습니다.\n" - f"{ready_output}\n{robot_ready_output}" + f"{ready_output}\n" + "티치펜던트/컨트롤러에서 빨간 상태(SAFE_OFF/보호정지/서보 상태)를 해제해 " + "STATE_STANDBY(1)로 만든 뒤 다시 확인하세요.\n" + f"{robot_ready_output}" ), } old = processes.get(step.key) @@ -3062,15 +3764,11 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: time.sleep(1.0) restart_output = "\n".join(cleanup_events) elif step.key == "start_camera": - camera_ready, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=3.0) - if camera_ready: - return { - "key": step.key, - "status": "running", - "output": "이미 RealSense 카메라 토픽이 살아있어 재시작하지 않습니다.\n" + camera_output, - } cleanup_events = cleanup_camera_stack() # Avoid duplicate /camera/camera nodes from previous panel attempts. + # Do not probe camera topics here: ros2cli graph/topic calls can + # wedge in the field and leave stale daemon/query processes. The + # RealSense tmux window is the source of truth for startup logs. time.sleep(0.4) restart_output = "\n".join(cleanup_events) elif step.key == "side_grip": @@ -3090,6 +3788,8 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: "output": "이미 실행 중입니다.\n" + tail_file(process_logs.get(step.key)), "pid": old.pid, } + if step.key in PANEL_TMUX_STEPS: + return run_background_step_in_tmux(step, cmd, env, restart_output=restart_output) log_path = background_log_path(step.key) log_handle = log_path.open("w", encoding="utf-8", buffering=1) log_handle.write(f"[Azas panel] command: {cmd}\n\n") @@ -3105,6 +3805,60 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: log_handle.close() processes[step.key] = proc process_logs[step.key] = log_path + if step.key == "start_tmux_stack": + try: + proc.wait(timeout=90.0) + except subprocess.TimeoutExpired: + output = ( + f"{cmd}\n--- log ---\n{log_path}\n" + "tmux 연결 스택 스크립트가 90초 안에 종료되지 않았습니다.\n" + + tail_file(log_path, max_chars=8000) + ) + return {"key": step.key, "status": "starting", "pid": proc.pid, "output": output} + output = f"{cmd}\n--- log ---\n{log_path}\n--- start output ---\n{tail_file(log_path, max_chars=10000)}" + if proc.returncode != 0: + return {"key": step.key, "status": "failed", "returncode": proc.returncode, "output": output} + return { + "key": step.key, + "status": "passed", + "pid": proc.pid, + "output": ( + output + + "\n[Azas] tmux 창 생성 성공. robot/gripper/camera/joint_relay는 각 tmux 창 로그를 기준으로 확인합니다. " + "ROS CLI daemon/discovery 오류 때문에 이 단계에서 후속 조회로 차단하지 않습니다." + ), + } + + service_prefix = env["SERVICE_PREFIX"] + motion_ok, motion_output = real_motion_readiness_gate( + service_prefix, + motion_timeout_sec=45.0, + ) + gripper_ok, gripper_output = wait_for_required_services( + ["/jarvis/rg2/open", "/jarvis/rg2/close", "/jarvis/rg2/set_width"], + timeout_sec=10.0, + ) + camera_ok, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=12.0) + output += ( + "\n--- robot readiness ---\n" + + motion_output + + "\n--- gripper readiness ---\n" + + gripper_output + + "\n--- camera readiness ---\n" + + camera_output + ) + if motion_ok and gripper_ok and camera_ok: + return {"key": step.key, "status": "passed", "pid": proc.pid, "output": output} + return { + "key": step.key, + "status": "blocked", + "pid": proc.pid, + "output": ( + "tmux 창은 시작했지만 로봇/그리퍼/카메라 준비 조건이 완성되지 않았습니다. " + "side-grip을 시작하지 않습니다.\n" + + output + ), + } if step.key == "connect_robot": ready, waited_output = wait_for_motion_services_ready( env["SERVICE_PREFIX"], @@ -3119,7 +3873,7 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: if robot_ready: return { "key": step.key, - "status": "started", + "status": "passed", "pid": proc.pid, "output": output, } @@ -3147,10 +3901,7 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: "output": output, } else: - # PR #20 side_grip is a manual OpenCV-window node. It should stay - # alive waiting for the operator's `p`/Esc key, so the panel must - # return quickly instead of blocking until that node exits. - time.sleep(3.0 if step.key == "side_grip" else 2.0) + time.sleep(2.0) if proc.poll() is not None: output = tail_file(log_path) if restart_output: @@ -3167,28 +3918,40 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: "\n[Azas] RG2 ROS services are ready. Note: azas_gripper RG2 wrapper has no physical " "finger-position feedback, so movement still must be visually confirmed." ) - return {"key": step.key, "status": "started", "pid": proc.pid, "output": output} + return {"key": step.key, "status": "passed", "pid": proc.pid, "output": output} output += "\n[Azas] RG2 bridge is still starting; retry gripper connection if services stay absent." return {"key": step.key, "status": "starting", "pid": proc.pid, "output": output} if step.key == "start_collision_scene": ready, waited_output = wait_for_collision_object_sample(env=env, timeout_sec=10.0, proc=proc) + tf_ready = False + tf_output = "" + if ready: + tf_ready, tf_output = wait_for_tf_transform( + env=env, + target_frame=HAND_EYE_TF_TARGET_FRAME, + source_frame=HAND_EYE_TF_SOURCE_FRAME, + timeout_sec=12.0, + proc=proc, + ) output = f"{cmd}\n--- log ---\n{log_path}\n--- readiness ---\n{waited_output}" + if ready: + output += f"\n--- hand-eye TF readiness ---\n{tf_output}" if restart_output: output = f"{restart_output}\n--- start command ---\n{output}" - if ready: - return {"key": step.key, "status": "started", "pid": proc.pid, "output": output} + if ready and tf_ready: + return {"key": step.key, "status": "passed", "pid": proc.pid, "output": output} return {"key": step.key, "status": "starting", "pid": proc.pid, "output": output} if step.key == "start_camera": ready, waited_output = wait_for_camera_topic_samples(env=env, timeout_sec=15.0, proc=proc) output = f"{cmd}\n--- log ---\n{log_path}\n--- readiness ---\n{waited_output}" if ready: - return {"key": step.key, "status": "started", "pid": proc.pid, "output": output} + return {"key": step.key, "status": "passed", "pid": proc.pid, "output": output} return {"key": step.key, "status": "starting", "pid": proc.pid, "output": output} if step.key == "detect_cup_lid": ready, waited_output = wait_for_cup_detection_sample(env=env, timeout_sec=10.0, proc=proc) output = f"{cmd}\n--- log ---\n{log_path}\n--- readiness ---\n{waited_output}" if ready: - return {"key": step.key, "status": "started", "pid": proc.pid, "output": output} + return {"key": step.key, "status": "passed", "pid": proc.pid, "output": output} return {"key": step.key, "status": "starting", "pid": proc.pid, "output": output} output = f"{cmd}\n--- log ---\n{log_path}" if restart_output: @@ -3350,6 +4113,7 @@ def stop_all() -> dict[str, Any]: if proc.poll() is None: events = terminate_process_tree(proc, label=key, grace_sec=5.0) stopped.append({"key": key, "pid": proc.pid, "events": events}) + processes.pop(key, None) return {"stopped": stopped} @@ -3361,6 +4125,7 @@ def cleanup_all_processes() -> dict[str, Any]: events.extend(cleanup_side_grip_stack(grace_sec=3.0)) events.extend(cleanup_collision_scene_stack(grace_sec=3.0)) events.extend(cleanup_camera_stack(grace_sec=3.0)) + events.extend(cleanup_rg2_stack(grace_sec=3.0)) events.extend(cleanup_doosan_stack(grace_sec=3.0)) events.extend( cleanup_matching_processes( @@ -3370,6 +4135,7 @@ def cleanup_all_processes() -> dict[str, Any]: ) ) events.extend(stop_ros2_daemon()) + process_logs.clear() return {"stopped": stopped.get("stopped", []), "cleanup": events} @@ -3447,19 +4213,46 @@ def do_POST(self) -> None: payload = json.loads(self.rfile.read(length) or b"{}") path = urlparse(self.path).path if path == "/api/run": - raw_selected = [str(key) for key in payload.get("selected") or []] - if payload.get("selected_already_expanded"): - selected = list(dict.fromkeys(raw_selected)) - else: - selected = with_collision_scene_prereq(raw_selected) - selected = list(dict.fromkeys(selected)) - steps_by_key = {step.key: step for step in STEPS} - results = [ - run_step(steps_by_key[key], payload) - for key in selected - if key in steps_by_key - ] - self.send_json({"execution_order": selected, "results": results}) + if not RUN_LOCK.acquire(blocking=False): + self.send_json( + { + "error": "another pipeline step is already running", + "results": [ + { + "key": "pipeline", + "status": "blocked", + "output": "이미 다른 실행 요청이 처리 중입니다. 현재 단계가 끝난 뒤 다시 실행하세요.", + } + ], + }, + 409, + ) + return + try: + raw_selected = [str(key) for key in payload.get("selected") or []] + if payload.get("selected_already_expanded"): + selected = list(dict.fromkeys(raw_selected)) + else: + selected = with_collision_scene_prereq(raw_selected) + selected = list(dict.fromkeys(selected)) + steps_by_key = {step.key: step for step in STEPS} + results = [] + for key in selected: + step = steps_by_key.get(key) + if step is None: + continue + result = run_step(step, payload) + results.append(result) + status = str(result.get("status") or "") + # Fail closed for server-side multi-step requests too. + # This prevents a queued motion step from running while a + # prerequisite is still starting, failed, timed out, or + # waiting for a prerequisite or failed motion. + if status in {"failed", "blocked", "timeout", "starting"}: + break + self.send_json({"execution_order": selected, "results": results}) + finally: + RUN_LOCK.release() return if path == "/api/dispenser_color_map": new_map = payload.get("map") diff --git a/tools/run/run_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh new file mode 100755 index 0000000..27cd06d --- /dev/null +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -0,0 +1,98 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT="${ROOT:-/home/ssu/Azas}" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +DISPLAY="${DISPLAY:-:0}" +XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" +ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" + +cd "${ROOT}" + +set +u +source /opt/ros/humble/setup.bash +if [[ -f /home/ssu/ws_moveit/install/setup.bash ]]; then + source /home/ssu/ws_moveit/install/setup.bash +fi +if [[ -f /home/ssu/ros2_ws/install/setup.bash ]]; then + source /home/ssu/ros2_ws/install/setup.bash +fi +if [[ -f "${ROOT}/install/setup.bash" ]]; then + source "${ROOT}/install/setup.bash" +else + source "${ROOT}/install/local_setup.bash" +fi +source "${ROOT}/install/dsr_practice/share/dsr_practice/package.bash" +set -u + +export DISPLAY XAUTHORITY ROS_DOMAIN_ID ROS_LOCALHOST_ONLY +export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +export PYTHONPATH="${ROOT}/tools/run/python_compat:${PYTHONPATH:-}" +export PYTHONUNBUFFERED=1 +export RCUTILS_LOGGING_BUFFERED_STREAM=0 +mkdir -p "${ROS_LOG_DIR}" + +echo "[Azas] START Changhyun side-grip direct tmux command" +echo "[Azas] OpenCV window: confirm cup, then press p. Quit with q/Esc." +echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XAUTHORITY}" +echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}" +echo "[Azas] start_joint_state_relay=${START_JOINT_STATE_RELAY:-false}" +echo "[Azas] moving to side-grip camera scan pose before starting YOLO" + +trap 'jobs -pr | xargs -r kill >/dev/null 2>&1 || true' EXIT + +python3 "${ROOT}/tools/run/direct_movej_joints.py" \ + --service-prefix "${SERVICE_PREFIX}" \ + --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 \ + --velocity 20 --acceleration 20 \ + --j5-min-deg -150 --j5-max-deg 150 \ + --timeout-sec 60 --motion-timeout-sec 120 \ + --execute --confirm ENABLE_DIRECT_MOVEJ + +echo "[Azas] side-grip camera scan pose reached; starting YOLO/OpenCV node" + +ros2 run tf2_ros static_transform_publisher \ + --x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 \ + --frame-id world --child-frame-id base_link & + +ros2 run azas_perception hand_eye_static_tf_node \ + --ros-args -p compose_timeout_sec:=30.0 -p allow_direct_fallback:=false & + +# Publish only the attached RG2/link_6 collision object before launching the +# picker. Keep the launch-side include disabled because it also starts an +# auxiliary robot_state_publisher and can stall MoveItPy initialization in the +# field tmux workflow. +ros2 run azas_motion link6_gripper_collision_node & + +if [[ "${START_JOINT_STATE_RELAY:-false}" == "true" ]]; then + ( + sleep 5 + python3 "${ROOT}/src/dsr_practice/dsr_practice/joint_state_relay.py" \ + --ros-args -r __node:=azas_joint_state_relay \ + -p input_topic:=/"${SERVICE_PREFIX}"/joint_states \ + -p output_topic:=/joint_states + ) & +fi + +ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ + model_path:="${ROOT}/local_models/best.pt" \ + conf:=0.35 imgsz:=640 device:=cpu target_class:=cup \ + auto_pick:=false auto_pick_interval:=8.0 exit_after_pick:=false \ + depth_patch_radius:=7 min_depth_valid_ratio:=0.03 min_depth_m:=0.15 max_depth_m:=1.20 \ + redetect_on_approach:=false redetect_settle_sec:=0.5 \ + grasp_mode:=side side_far_stage_enabled:=false side_approach_offset:=0.18 \ + side_short_stage_backoff_m:=0.08 side_grasp_stop_backoff_m:=0.04 side_close_underreach_m:=0.03 \ + side_low_retry_lift_m:=0.0 side_low_retry_attempts:=0 \ + side_linear_approach_enabled:=true side_final_slide_enabled:=false \ + side_fixed_grasp_z_enabled:=true side_fixed_grasp_z:=0.07 side_project_bbox_center_to_fixed_z:=true \ + side_candidate_plan_check_enabled:=true pre_pick_joint1_clearance_deg:=12.0 \ + side_move_to_initial_center_before_close:=false verify_motion:=false \ + skip_initial_home_move:=true move_to_camera_home:=false move_joint_home_before_camera_home:=false camera_home_mode:=joint min_motion_z:=0.07 \ + workspace_xy_clamp_enabled:=false return_home_after_task:=false return_to_camera_home_after_attempt:=true \ + workspace_collision_scene_enabled:=false table_collision_enabled:=true table_surface_z:=0.0 table_thickness:=0.04 \ + table_size_x:=1.10 table_size_y:=0.65 table_center_x:=0.29 table_center_y:=0.0 table_collision_expand_to_workspace_walls:=true \ + workspace_boundary_collision_enabled:=true dispenser_collision_enabled:=true dispenser_collision_publish_objects:=true \ + dispenser_collision_publish_markers:=true link6_gripper_collision_enabled:=false \ + dispenser_collision_config_path:="${ROOT}/src/azas_bringup/config/measured_dispenser_collision.yaml" \ + moveit_controller_name:=/"${SERVICE_PREFIX}"/dsr_moveit_controller start_joint_state_relay:=false diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 62111a0..6e5e419 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -48,17 +48,40 @@ def color_to_dispenser_id(color: str, color_map: dict[str, str]) -> str | None: def parse_colors_arg(raw: str) -> list[tuple[str, int]]: - """'red:2,blue:1' → [('red', 2), ('blue', 1)].""" - result = [] - for part in raw.split(","): - part = part.strip() - if not part: + """Parse color pump input. + + Accepted forms: + red:2,blue:1 + redx2,bluex1 + red2,blue1 + red,blue + """ + result: list[tuple[str, int]] = [] + for part in raw.replace(";", ",").split(","): + item = part.strip().lower() + if not item: continue - if ":" in part: - c, n = part.split(":", 1) - result.append((c.strip().lower(), int(n.strip()))) + if ":" in item: + color, count_raw = item.split(":", 1) + elif "x" in item: + color, count_raw = item.split("x", 1) else: - result.append((part.lower(), 1)) + match = __import__("re").match(r"^([a-zA-Z가-힣_ -]+?)(\d+)?$", item) + if not match: + raise ValueError(f"invalid color token: {part!r}") + color, count_raw = match.group(1), match.group(2) or "1" + color = color.strip().lower() + if not color: + raise ValueError(f"empty color in token: {part!r}") + try: + count = int(str(count_raw).strip()) + except ValueError as exc: + raise ValueError(f"invalid count for color {color}: {count_raw!r}") from exc + if count < 1: + raise ValueError(f"count must be >= 1 for color {color}") + result.append((color, count)) + if not result: + raise ValueError("color input is empty") return result @@ -99,7 +122,7 @@ def parse_direct_dispenser_sequence(raw: str) -> list[str]: def main() -> int: parser = argparse.ArgumentParser() parser.add_argument("--colors", default="", - help="직접 색깔 지정: 'red:2,blue:1' (생략 시 latest_recipe.json 사용)") + help="직접 색깔 지정: 'red:2,blue:1', 'redx2,bluex1', 'red2,blue1' (생략 시 latest_recipe.json 사용)") parser.add_argument("--dispenser-ids", default="", help="직접 물리 디스펜서 지정: '1,2,2,3' 또는 '1x1,2x2,3x1'") parser.add_argument("--confirm", action="store_true", @@ -136,6 +159,8 @@ def main() -> int: "--wait-service-sec", str(args.wait_service_sec), "--pose-read-retries", str(args.pose_read_retries), "--pose-read-retry-sleep-sec", str(args.pose_read_retry_sleep_sec), + "--safe-lift-joint-fallback", + "--no-integrated-regrasp-fallback-subprocess", ] direct_dispenser_ids = args.dispenser_ids.strip() @@ -177,7 +202,11 @@ def main() -> int: # 색깔+펌프 수 결정 if args.colors: - color_pumps = parse_colors_arg(args.colors) + try: + color_pumps = parse_colors_arg(args.colors) + except ValueError as exc: + print(f"[run_color_recipe] 잘못된 색상 입력: {exc}", file=sys.stderr) + return 1 else: if not RECIPE_PATH.exists(): print(f"[run_color_recipe] 레시피 없음: {RECIPE_PATH}", file=sys.stderr) diff --git a/tools/run/run_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh new file mode 100755 index 0000000..8262950 --- /dev/null +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -0,0 +1,91 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT="${ROOT:-/home/ssu/Azas}" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +DISPLAY="${DISPLAY:-:0}" +XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" +MODEL_PATH="${MODEL_PATH:-${ROOT}/local_models/best.pt}" +ARUCO_DICTIONARY="${ARUCO_DICTIONARY:-DICT_6X6_250}" +ARUCO_MARKER_ID="${ARUCO_MARKER_ID:-0}" +ARUCO_FALLBACK_MARKERS="${ARUCO_FALLBACK_MARKERS:-DICT_4X4_50:14}" +ARUCO_MARKER_LENGTH_M="${ARUCO_MARKER_LENGTH_M:-0.03}" +ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" + +cd "${ROOT}" + +set +u +source /opt/ros/humble/setup.bash +if [[ -f /home/ssu/ws_moveit/install/setup.bash ]]; then + source /home/ssu/ws_moveit/install/setup.bash +fi +if [[ -f /home/ssu/ros2_ws/install/setup.bash ]]; then + source /home/ssu/ros2_ws/install/setup.bash +fi +if [[ -f "${ROOT}/install/setup.bash" ]]; then + source "${ROOT}/install/setup.bash" +else + source "${ROOT}/install/local_setup.bash" +fi +set -u + +export DISPLAY XAUTHORITY ROS_DOMAIN_ID ROS_LOCALHOST_ONLY +export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +export PYTHONPATH="${ROOT}/tools/run/python_compat:${PYTHONPATH:-}" +mkdir -p "${ROS_LOG_DIR}" "${ROOT}/log/tmux_logic" + +if [[ "${SERVICE_PREFIX}" != /* ]]; then + SERVICE_PREFIX="/${SERVICE_PREFIX}" +fi + +echo "[Azas] START Kang lid_grip_close direct command" +echo "[Azas] OpenCV window: confirm lid ArUco, then press p. Quit with q/Esc." +echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XAUTHORITY}" +echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}" +echo "[Azas] aruco=${ARUCO_DICTIONARY}:${ARUCO_MARKER_ID} fallback=${ARUCO_FALLBACK_MARKERS} length_m=${ARUCO_MARKER_LENGTH_M}" + +if [[ ! -f "${MODEL_PATH}" ]]; then + echo "[Azas][WARN] model_path not found: ${MODEL_PATH}" +fi + +ros2 pkg executables azas_perception | grep -q '^azas_perception lid_sticker_detector_node$' || { + echo "[Azas][FAIL] missing azas_perception lid_sticker_detector_node" >&2 + exit 2 +} +ros2 pkg executables azas_motion | grep -q '^azas_motion lid_grip_planner_node$' || { + echo "[Azas][FAIL] missing azas_motion lid_grip_planner_node" >&2 + exit 3 +} + +ros2 launch azas_bringup lid_sticker_grip_planning.launch.py \ + model_path:="${MODEL_PATH}" \ + marker_type:=aruco require_lid_detection:=false \ + allow_aruco_only_after_grip_request:=false aruco_only_after_grip_request_sec:=20.0 \ + aruco_dictionary:="${ARUCO_DICTIONARY}" aruco_marker_id:="${ARUCO_MARKER_ID}" \ + aruco_fallback_markers:="${ARUCO_FALLBACK_MARKERS}" aruco_marker_length_m:="${ARUCO_MARKER_LENGTH_M}" \ + use_aruco_axis_for_orientation:=true aruco_finger_axis_quarter_turns:=0 \ + use_lid_pose_yaw_for_pick:=true lid_pose_yaw_axis:=y lid_pose_yaw_offset_deg:=0.0 lid_pose_yaw_equivalence_deg:=180.0 \ + visual_refine_before_grasp:=true visual_refine_sample_count:=5 visual_refine_timeout_sec:=3.0 visual_refine_max_yaw_std_deg:=3.0 \ + visual_refine_max_position_std_m:=0.005 visual_refine_apply_xy:=true visual_refine_apply_yaw:=true visual_refine_fallback_to_initial_plan:=true \ + enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION allow_service_control_without_moveit:=true service_prefix:="${SERVICE_PREFIX}" \ + rx:=108.41 ry:=-176.32 rz:=175.98 offset_axis:=base_z surface_offset_m:=0.0 \ + tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:=-0.040 min_grasp_z_m:=0.025 \ + approach_offset_m:=0.08 lift_offset_m:=0.10 settle_seconds_before_grasp:=0.5 hold_seconds_after_grasp:=3.0 \ + line_velocity:=30.0 line_acceleration:=10.0 move_timeout_sec:=90.0 \ + enable_gripper_service_calls:=true gripper_set_service:=/jarvis/rg2/set_width \ + gripper_preopen_width_m:=0.110 gripper_grasp_width_m:=0.020 gripper_force_n:=12.0 \ + continue_after_gripper_grasp_failure:=true gripper_grasp_failure_wait_sec:=2.0 \ + enable_lid_twist_after_grasp:=true \ + lid_twist_target_x_m:=0.422959106 lid_twist_target_y_m:=0.223224869 lid_twist_target_z_m:=0.166827988 \ + lid_twist_rx:=73.901489 lid_twist_ry:=-178.542740 lid_twist_rz:=117.385612 \ + lid_twist_transfer_clearance_m:=0.12 lid_twist_transfer_max_z_m:=0.60 \ + lid_twist_use_force_control:=false lid_twist_force_rotation_mode:=j6 \ + lid_twist_preseat_periodic_before_turn:=true \ + lid_twist_preseat_periodic_x_amp_mm:=0.0 lid_twist_preseat_periodic_y_amp_mm:=0.0 lid_twist_preseat_periodic_z_amp_mm:=1.0 \ + lid_twist_preseat_periodic_rx_amp_deg:=0.0 lid_twist_preseat_periodic_ry_amp_deg:=0.0 lid_twist_preseat_periodic_rz_amp_deg:=10.0 \ + lid_twist_preseat_periodic_period_sec:=3.6 lid_twist_preseat_periodic_acc_time_sec:=1.0 lid_twist_preseat_periodic_repeat:=2 \ + lid_twist_preseat_periodic_ref:=tool lid_twist_rz_delta_deg:=300.0 lid_twist_turn_step_deg:=50.0 \ + lid_twist_release_lift_m:=0.03 lid_twist_min_z_m:=0.140 lid_twist_max_z_m:=0.220 \ + lid_twist_transfer_velocity:=25.0 lid_twist_press_velocity:=5.0 lid_twist_turn_velocity:=30.0 lid_twist_acceleration:=15.0 \ + lid_twist_hold_seconds_before_turn:=0.0 lid_twist_hold_seconds_after_turn:=0.5 diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index e2ded84..99cdd0a 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -454,6 +454,23 @@ def move_front_hold( ) self.move_front_hold_joint_fallback(pos, label=label) + def move_posx_joint_fallback( + self, + posx_mm_deg: list[float], + *, + label: str, + velocity: float, + acceleration: float, + ) -> None: + joints_deg = self.ikin_posj(posx_mm_deg, label=f"{label} IK joint fallback") + self.movej( + joints_deg, + label=f"{label} IK MoveJoint fallback", + velocity=velocity, + acceleration=acceleration, + ) + self.wait_for_target(posx_mm_deg, label=f"{label} IK MoveJoint fallback posx") + def move_front_hold_joint_fallback(self, posx_mm_deg: list[float], *, label: str) -> None: joints_deg = self.ikin_posj(posx_mm_deg, label=f"{label} IK joint fallback") self.movej( @@ -482,13 +499,27 @@ def safe_lift_current( ) return target = [pose[0], pose[1], target_z_mm, pose[3], pose[4], pose[5]] - self.move_posx( - target, - label=label, - velocity=velocity, - acceleration=acceleration, - timeout_sec=timeout_sec, - ) + try: + self.move_posx( + target, + label=label, + velocity=velocity, + acceleration=acceleration, + timeout_sec=timeout_sec, + ) + except RuntimeError as exc: + if not self.args.safe_lift_joint_fallback: + raise + print( + f"[WARN] MoveLine safe lift failed for {label}: {exc}; " + "retrying the same high-Z target with IK MoveJoint fallback" + ) + self.move_posx_joint_fallback( + target, + label=label, + velocity=self.args.safe_lift_joint_fallback_velocity, + acceleration=self.args.safe_lift_joint_fallback_acceleration, + ) def move_posx( self, @@ -1442,6 +1473,17 @@ def parse_args() -> argparse.Namespace: ) parser.add_argument("--front-hold-joint-fallback-velocity", type=float, default=30.0) parser.add_argument("--front-hold-joint-fallback-acceleration", type=float, default=40.0) + parser.add_argument( + "--safe-lift-joint-fallback", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "When the post-press vertical MoveLine to safe transit Z stalls in a singularity, " + "retry the same live-TCP-derived high-Z target with IK MoveJoint before failing." + ), + ) + parser.add_argument("--safe-lift-joint-fallback-velocity", type=float, default=30.0) + parser.add_argument("--safe-lift-joint-fallback-acceleration", type=float, default=40.0) parser.add_argument("--gripper-service", default="/jarvis/rg2/set_width") parser.add_argument("--gripper-open-width-m", type=float, default=0.110) parser.add_argument("--gripper-open-force-n", type=float, default=12.0) @@ -1472,11 +1514,12 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--integrated-regrasp-fallback-subprocess", action=argparse.BooleanOptionalAction, - default=True, + default=False, help=( "If the persistent integrated re-grasp/lift stalls verification, retry once " - "with the legacy pick_from_measured_dispenser_front_hold helper instead of " - "ending the recipe at the first target timeout." + "with the legacy pick_from_measured_dispenser_front_hold helper. Default false " + "because that helper uses Cartesian front-hold entry and can reproduce the " + "post-press singularity/low direct approach." ), ) parser.add_argument("--execute", action="store_true") diff --git a/tools/run/run_somyeong_cup_uprighting_direct.sh b/tools/run/run_somyeong_cup_uprighting_direct.sh new file mode 100755 index 0000000..ad1bf21 --- /dev/null +++ b/tools/run/run_somyeong_cup_uprighting_direct.sh @@ -0,0 +1,59 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT="${ROOT:-/home/ssu/Azas}" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +DISPLAY="${DISPLAY:-:0}" +XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" +MODEL_PATH="${MODEL_PATH:-${ROOT}/src/azas_perception/config/yolo_cup_uprighting_best.pt}" +AUTO_PICK="${AUTO_PICK:-false}" +SKIP_INITIAL_HOME_MOVE="${SKIP_INITIAL_HOME_MOVE:-true}" +PUBLISH_HAND_EYE_TF="${PUBLISH_HAND_EYE_TF:-true}" +ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" + +cd "${ROOT}" + +set +u +source /opt/ros/humble/setup.bash +if [[ -f /home/ssu/ws_moveit/install/setup.bash ]]; then + source /home/ssu/ws_moveit/install/setup.bash +fi +if [[ -f /home/ssu/ros2_ws/install/setup.bash ]]; then + source /home/ssu/ros2_ws/install/setup.bash +fi +if [[ -f "${ROOT}/install/setup.bash" ]]; then + source "${ROOT}/install/setup.bash" +else + source "${ROOT}/install/local_setup.bash" +fi +set -u + +export DISPLAY XAUTHORITY ROS_DOMAIN_ID ROS_LOCALHOST_ONLY +export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +export PYTHONPATH="${ROOT}/tools/run/python_compat:${PYTHONPATH:-}" +export AZAS_CUP_UPRIGHTING_MODEL_PATH="${MODEL_PATH}" +mkdir -p "${ROS_LOG_DIR}" "${ROOT}/log/tmux_logic" + +echo "[Azas] START Somyeong cup_uprighting direct command" +echo "[Azas] OpenCV window: confirm fallen cup, then press p. Quit with q/Esc." +echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XAUTHORITY}" +echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}" +echo "[Azas] model_path=${MODEL_PATH}" +echo "[Azas] auto_pick=${AUTO_PICK} skip_initial_home_move=${SKIP_INITIAL_HOME_MOVE} publish_hand_eye_tf=${PUBLISH_HAND_EYE_TF}" + +if [[ ! -f "${MODEL_PATH}" ]]; then + echo "[Azas][FAIL] YOLO model missing: ${MODEL_PATH}" >&2 + exit 2 +fi + +if [[ ! -x "${ROOT}/install/azas_cup_uprighting/lib/azas_cup_uprighting/yolo_cup_uprighting" ]]; then + echo "[Azas][FAIL] yolo_cup_uprighting executable missing. Build azas_cup_uprighting first." >&2 + exit 3 +fi + +ros2 launch "${ROOT}/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py" \ + model_path:="${MODEL_PATH}" \ + auto_pick:="${AUTO_PICK}" \ + skip_initial_home_move:="${SKIP_INITIAL_HOME_MOVE}" \ + publish_hand_eye_tf:="${PUBLISH_HAND_EYE_TF}" diff --git a/tools/run/run_tmux_logic_sequence.sh b/tools/run/run_tmux_logic_sequence.sh new file mode 100755 index 0000000..b6dfb34 --- /dev/null +++ b/tools/run/run_tmux_logic_sequence.sh @@ -0,0 +1,248 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT="/home/ssu/Azas" +LOG_DIR="${ROOT}/log/tmux_logic" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +DISPLAY="${DISPLAY:-:0}" +XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" + +mkdir -p "${LOG_DIR}" /tmp/azas_ros_logs +cd "${ROOT}" + +set +u +source /opt/ros/humble/setup.bash +if [[ -f /home/ssu/ws_moveit/install/setup.bash ]]; then source /home/ssu/ws_moveit/install/setup.bash; fi +if [[ -f /home/ssu/ros2_ws/install/setup.bash ]]; then source /home/ssu/ros2_ws/install/setup.bash; fi +if [[ -f "${ROOT}/install/setup.bash" ]]; then source "${ROOT}/install/setup.bash"; else source "${ROOT}/install/local_setup.bash"; fi +set -u + +export ROS_LOG_DIR=/tmp/azas_ros_logs +export PYTHONPATH="${ROOT}/tools/run/python_compat:${PYTHONPATH:-}" +export DISPLAY XAUTHORITY + +support_pids=() + +cleanup_support() { + for pid in "${support_pids[@]:-}"; do + if kill -0 "${pid}" >/dev/null 2>&1; then + kill -TERM "${pid}" >/dev/null 2>&1 || true + fi + done +} +trap cleanup_support EXIT + +log_msg() { + printf '\n[%(%Y-%m-%d %H:%M:%S)T] %s\n' -1 "$*" +} + +wait_service_call() { + local service="$1" + local type="$2" + local label="$3" + local timeout="${4:-8.0}" + local attempt=1 + while true; do + log_msg "waiting: ${label} (${service}) attempt=${attempt}" + if timeout 12s python3 tools/run/ros_call_empty_service.py "${service}" "${type}" --timeout "${timeout}"; then + return 0 + fi + sleep 2 + attempt=$((attempt + 1)) + done +} + +wait_topic_once() { + local topic="$1" + local label="$2" + local attempt=1 + while true; do + log_msg "waiting topic: ${label} (${topic}) attempt=${attempt}" + if timeout 4s ros2 topic echo "${topic}" --once >/tmp/azas_topic_wait.log 2>&1; then + cat /tmp/azas_topic_wait.log | head -n 12 + return 0 + fi + tail -n 8 /tmp/azas_topic_wait.log || true + sleep 2 + attempt=$((attempt + 1)) + done +} + +ensure_robot_ready() { + wait_service_call "/${SERVICE_PREFIX}/system/get_robot_state" "dsr_msgs2/srv/GetRobotState" "Doosan robot_state" + wait_service_call "/${SERVICE_PREFIX}/motion/check_motion" "dsr_msgs2/srv/CheckMotion" "Doosan check_motion" + log_msg "robot ready gate passed" +} + +ensure_gripper() { + if timeout 3s ros2 service type /jarvis/rg2/set_width >/dev/null 2>&1; then + log_msg "RG2 services already visible" + return 0 + fi + log_msg "starting RG2 bridge" + ( + set +u + source /opt/ros/humble/setup.bash + source "${ROOT}/install/setup.bash" + set -u + source "${ROOT}/install/azas_gripper/share/azas_gripper/package.bash" + ros2 launch "${ROOT}/install/azas_gripper/share/azas_gripper/launch/rg2_trigger.launch.py" \ + ip:=192.168.1.1 port:=502 connect:=true open_width:=1100 close_width:=0 force:=300 settle_seconds:=0.6 + ) >"${LOG_DIR}/gripper.log" 2>&1 & + support_pids+=("$!") + until timeout 3s ros2 service type /jarvis/rg2/set_width >/dev/null 2>&1; do + tail -n 12 "${LOG_DIR}/gripper.log" || true + sleep 2 + done + log_msg "RG2 services ready" +} + +ensure_camera() { + if timeout 3s ros2 topic echo /camera/camera/aligned_depth_to_color/image_raw --once >/dev/null 2>&1; then + log_msg "RealSense aligned depth already visible" + return 0 + fi + log_msg "starting RealSense camera with initial_reset" + ros2 launch realsense2_camera rs_launch.py \ + camera_name:=camera \ + initial_reset:=true reconnect_timeout:=5.0 \ + enable_color:=true enable_depth:=true align_depth.enable:=true \ + rgb_camera.color_profile:=640x480x30 \ + depth_module.depth_profile:=640x480x30 \ + >"${LOG_DIR}/camera.log" 2>&1 & + support_pids+=("$!") + wait_topic_once /camera/camera/color/image_raw "RealSense color" + wait_topic_once /camera/camera/aligned_depth_to_color/image_raw "RealSense aligned depth" + wait_topic_once /camera/camera/color/camera_info "RealSense camera info" + log_msg "RealSense camera topics ready" +} + +ensure_collision_scene() { + log_msg "starting collision/TF support stack" + ( + ros2 launch azas_bringup workspace_collision_scene.launch.py \ + publish_collision_objects:=true \ + table_collision_enabled:=true \ + workspace_boundary_collision_enabled:=true \ + table_collision_expand_to_workspace_walls:=true \ + dispenser_collision_enabled:=true \ + dispenser_collision_publish_objects:=true \ + dispenser_collision_publish_markers:=true & + ros2 launch azas_bringup rg2_link6_tcp.launch.py publish_gripper_collision:=true & + ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id base_link & + ros2 run azas_perception hand_eye_static_tf_node --ros-args -p compose_timeout_sec:=30.0 -p allow_direct_fallback:=false & + python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=publish_detected -p object_id:=detected_tumbler -p use_lidded_height:=true + ) >"${LOG_DIR}/collision_scene.log" 2>&1 & + support_pids+=("$!") + sleep 5 + tail -n 40 "${LOG_DIR}/collision_scene.log" || true +} + +run_side_grip() { + log_msg "START 창현 side-grip. OpenCV 창에서 컵 확인 후 p를 누르세요. 종료는 q/Esc." + source "${ROOT}/install/dsr_practice/share/dsr_practice/package.bash" + ( + trap 'jobs -pr | xargs -r kill >/dev/null 2>&1 || true' EXIT + ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id base_link & + ros2 run azas_perception hand_eye_static_tf_node --ros-args -p compose_timeout_sec:=30.0 -p allow_direct_fallback:=false & + (sleep 5; python3 "${ROOT}/src/dsr_practice/dsr_practice/joint_state_relay.py" --ros-args -r __node:=azas_joint_state_relay -p input_topic:=/${SERVICE_PREFIX}/joint_states -p output_topic:=/joint_states) & + ros2 launch "${ROOT}/src/dsr_practice/launch/yolo_cup_pick_node.launch.py" \ + model_path:="${ROOT}/local_models/best.pt" \ + conf:=0.35 imgsz:=640 device:=cpu target_class:=cup \ + auto_pick:=false auto_pick_interval:=8.0 exit_after_pick:=false \ + depth_patch_radius:=7 min_depth_valid_ratio:=0.03 min_depth_m:=0.15 max_depth_m:=1.20 \ + redetect_on_approach:=false redetect_settle_sec:=0.5 \ + grasp_mode:=side side_far_stage_enabled:=false side_approach_offset:=0.18 \ + side_short_stage_backoff_m:=0.08 side_grasp_stop_backoff_m:=0.04 side_close_underreach_m:=0.03 \ + side_low_retry_lift_m:=0.0 side_low_retry_attempts:=0 \ + side_linear_approach_enabled:=true side_final_slide_enabled:=false \ + side_fixed_grasp_z_enabled:=true side_fixed_grasp_z:=0.07 side_project_bbox_center_to_fixed_z:=true \ + side_candidate_plan_check_enabled:=true pre_pick_joint1_clearance_deg:=12.0 \ + side_move_to_initial_center_before_close:=false verify_motion:=false \ + move_to_camera_home:=true move_joint_home_before_camera_home:=false camera_home_mode:=joint min_motion_z:=0.07 \ + workspace_xy_clamp_enabled:=false return_home_after_task:=false return_to_camera_home_after_attempt:=true \ + workspace_collision_scene_enabled:=true table_collision_enabled:=true table_surface_z:=0.0 table_thickness:=0.04 \ + table_size_x:=1.10 table_size_y:=0.65 table_center_x:=0.29 table_center_y:=0.0 table_collision_expand_to_workspace_walls:=true \ + workspace_boundary_collision_enabled:=true dispenser_collision_enabled:=true dispenser_collision_publish_objects:=true \ + dispenser_collision_publish_markers:=true link6_gripper_collision_enabled:=true \ + dispenser_collision_config_path:="${ROOT}/src/azas_bringup/config/measured_dispenser_collision.yaml" \ + moveit_controller_name:=/${SERVICE_PREFIX}/dsr_moveit_controller start_joint_state_relay:=false + ) +} + +run_cup_uprighting() { + log_msg "START 소명 cup_uprighting. OpenCV 창에서 누운 컵 확인 후 p를 누르세요. 종료는 q/Esc." + export AZAS_CUP_UPRIGHTING_MODEL_PATH="${ROOT}/src/azas_perception/config/yolo_cup_uprighting_best.pt" + ros2 launch "${ROOT}/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py" \ + model_path:="${AZAS_CUP_UPRIGHTING_MODEL_PATH}" \ + service_prefix:="${SERVICE_PREFIX}" \ + enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION \ + run_yolo:=true auto_pick:=false publish_hand_eye_tf:=true +} + +run_lid_grip_close() { + log_msg "START 강개발자 lid_grip_close. ArUco는 기본 DICT_6X6_250 id0, fallback DICT_4X4_50 id14." + ros2 launch azas_bringup lid_sticker_grip_planning.launch.py \ + model_path:="${ROOT}/local_models/best.pt" \ + marker_type:=aruco require_lid_detection:=false \ + allow_aruco_only_after_grip_request:=false aruco_only_after_grip_request_sec:=20.0 \ + aruco_dictionary:=DICT_6X6_250 aruco_marker_id:=0 aruco_fallback_markers:=DICT_4X4_50:14 aruco_marker_length_m:=0.03 \ + use_aruco_axis_for_orientation:=true aruco_finger_axis_quarter_turns:=0 \ + use_lid_pose_yaw_for_pick:=true lid_pose_yaw_axis:=y lid_pose_yaw_offset_deg:=0.0 lid_pose_yaw_equivalence_deg:=180.0 \ + visual_refine_before_grasp:=true visual_refine_sample_count:=5 visual_refine_timeout_sec:=3.0 visual_refine_max_yaw_std_deg:=3.0 \ + visual_refine_max_position_std_m:=0.005 visual_refine_apply_xy:=true visual_refine_apply_yaw:=true visual_refine_fallback_to_initial_plan:=true \ + enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION allow_service_control_without_moveit:=true service_prefix:=/${SERVICE_PREFIX} \ + rx:=108.41 ry:=-176.32 rz:=175.98 offset_axis:=base_z surface_offset_m:=0.0 \ + tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:=-0.040 min_grasp_z_m:=0.025 \ + approach_offset_m:=0.08 lift_offset_m:=0.10 settle_seconds_before_grasp:=0.5 hold_seconds_after_grasp:=3.0 \ + line_velocity:=30.0 line_acceleration:=10.0 move_timeout_sec:=90.0 \ + enable_gripper_service_calls:=true gripper_set_service:=/jarvis/rg2/set_width \ + gripper_preopen_width_m:=0.110 gripper_grasp_width_m:=0.020 gripper_force_n:=12.0 \ + continue_after_gripper_grasp_failure:=true gripper_grasp_failure_wait_sec:=2.0 \ + enable_lid_twist_after_grasp:=true \ + lid_twist_target_x_m:=0.422959106 lid_twist_target_y_m:=0.223224869 lid_twist_target_z_m:=0.166827988 \ + lid_twist_rx:=73.901489 lid_twist_ry:=-178.542740 lid_twist_rz:=117.385612 \ + lid_twist_transfer_clearance_m:=0.12 lid_twist_transfer_max_z_m:=0.60 \ + lid_twist_use_force_control:=false lid_twist_force_rotation_mode:=j6 \ + lid_twist_preseat_periodic_before_turn:=true \ + lid_twist_preseat_periodic_x_amp_mm:=0.0 lid_twist_preseat_periodic_y_amp_mm:=0.0 lid_twist_preseat_periodic_z_amp_mm:=1.0 \ + lid_twist_preseat_periodic_rx_amp_deg:=0.0 lid_twist_preseat_periodic_ry_amp_deg:=0.0 lid_twist_preseat_periodic_rz_amp_deg:=10.0 \ + lid_twist_preseat_periodic_period_sec:=3.6 lid_twist_preseat_periodic_acc_time_sec:=1.0 lid_twist_preseat_periodic_repeat:=2 \ + lid_twist_preseat_periodic_ref:=tool lid_twist_rz_delta_deg:=300.0 lid_twist_turn_step_deg:=50.0 \ + lid_twist_release_lift_m:=0.03 lid_twist_min_z_m:=0.140 lid_twist_max_z_m:=0.220 \ + lid_twist_transfer_velocity:=25.0 lid_twist_press_velocity:=5.0 lid_twist_turn_velocity:=30.0 lid_twist_acceleration:=15.0 \ + lid_twist_hold_seconds_before_turn:=0.0 lid_twist_hold_seconds_after_turn:=0.5 +} + +run_with_retry() { + local name="$1" + shift + while true; do + log_msg "running ${name}" + if "$@"; then + log_msg "${name} exited cleanly" + break + fi + log_msg "${name} failed. Press Enter to retry, type s then Enter to skip, or q then Enter to stop." + read -r answer + case "${answer}" in + s|S) break ;; + q|Q) exit 1 ;; + esac + done +} + +log_msg "tmux logic sequence started. Connect the robot in another tmux pane if it is not connected yet." +ensure_robot_ready +ensure_gripper +ensure_camera +ensure_collision_scene +run_with_retry "창현 side-grip" run_side_grip +ensure_robot_ready +ensure_camera +run_with_retry "소명 cup_uprighting" run_cup_uprighting +ensure_robot_ready +ensure_camera +ensure_gripper +run_with_retry "강개발자 lid_grip_close" run_lid_grip_close +log_msg "logic sequence complete" diff --git a/tools/run/start_azas_tmux_stack.sh b/tools/run/start_azas_tmux_stack.sh new file mode 100755 index 0000000..06916fc --- /dev/null +++ b/tools/run/start_azas_tmux_stack.sh @@ -0,0 +1,44 @@ +#!/usr/bin/env bash +set -euo pipefail + +ROOT="${ROOT:-/home/ssu/Azas}" +SESSION="${SESSION:-azas-logic}" +ROBOT_HOST="${ROBOT_HOST:-192.168.1.100}" +ROBOT_NAME="${ROBOT_NAME:-dsr01}" +RT_HOST="${RT_HOST:-0.0.0.0}" +RG2_IP="${RG2_IP:-192.168.1.1}" +RG2_PORT="${RG2_PORT:-502}" +ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" + +cd "${ROOT}" +mkdir -p "${ROOT}/log/tmux_logic" /tmp/azas_ros_logs + +tmux kill-session -t "${SESSION}" >/dev/null 2>&1 || true + +# Stop only the ROS CLI graph daemon. Robot/camera processes are cleaned by the tmux session above. +while read -r pid cmd; do + [[ -z "${pid:-}" ]] && continue + if [[ "${cmd}" == *"ros2cli.daemon.daemonize"* ]]; then + kill "${pid}" >/dev/null 2>&1 || true + fi +done < <(ps -eo pid=,cmd=) + +common_env="export ROS_DOMAIN_ID=${ROS_DOMAIN_ID}; export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}; export ROS_LOG_DIR=/tmp/azas_ros_logs" +robot_cmd="cd ${ROOT}; mkdir -p log/tmux_logic /tmp/azas_ros_logs; ${common_env}; export ROBOT_HOST=${ROBOT_HOST}; export ROBOT_NAME=${ROBOT_NAME}; export RT_HOST=${RT_HOST}; export DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP; bash ${ROOT}/tools/run/run_doosan_real_m0609.sh 2>&1 | tee ${ROOT}/log/tmux_logic/robot-\$(date +%Y%m%d-%H%M%S).log" +gripper_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch ${ROOT}/install/azas_gripper/share/azas_gripper/launch/rg2_trigger.launch.py ip:=${RG2_IP} port:=${RG2_PORT} connect:=true open_width:=1100 close_width:=0 force:=300 settle_seconds:=0.6 2>&1 | tee ${ROOT}/log/tmux_logic/gripper-\$(date +%Y%m%d-%H%M%S).log" +camera_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch realsense2_camera rs_launch.py camera_name:=camera initial_reset:=true reconnect_timeout:=5.0 enable_color:=true enable_depth:=true align_depth.enable:=true rgb_camera.color_profile:=640x480x30 depth_module.depth_profile:=640x480x30 2>&1 | tee ${ROOT}/log/tmux_logic/camera-\$(date +%Y%m%d-%H%M%S).log" +relay_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; python3 ${ROOT}/src/dsr_practice/dsr_practice/joint_state_relay.py --ros-args -r __node:=azas_joint_state_relay -p input_topic:=/${ROBOT_NAME}/joint_states -p output_topic:=/joint_states 2>&1 | tee ${ROOT}/log/tmux_logic/joint_relay-\$(date +%Y%m%d-%H%M%S).log" + +tmux new-session -d -s "${SESSION}" -n robot "${robot_cmd}" +sleep 10 +tmux new-window -t "${SESSION}" -n gripper "${gripper_cmd}" +sleep 3 +tmux new-window -t "${SESSION}" -n camera "${camera_cmd}" +sleep 8 +tmux new-window -t "${SESSION}" -n joint_relay "${relay_cmd}" + +echo "[Azas] tmux stack started: ${SESSION}" +echo "[Azas] attach outside tmux: tmux attach -t ${SESSION}" +echo "[Azas] switch inside tmux: tmux switch-client -t ${SESSION}" +tmux list-windows -t "${SESSION}" From bcad928f8606f318f4390e81234c11e72842a665 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 16:50:55 +0900 Subject: [PATCH 32/88] Make cup uprighting use field-verified execution MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit MoveItPy planning remains in the Somyeong stack, but trajectory execution now uses the namespaced Doosan FollowJointTrajectory action directly. The panel also treats the terminal-verified cup_uprighting path like side_grip, avoiding slow ROS CLI discovery gates for this real-motion GUI workflow. Constraint: Real robot execution succeeded only through the tmux stack with ROS_DOMAIN_ID=9 and /dsr01/dsr_moveit_controller/follow_joint_trajectory. Rejected: Keep using robot.execute() | MoveItPy reported the controller but failed to connect its internal action client during real execution. Confidence: high Scope-risk: moderate Directive: Do not reintroduce blocking ROS CLI preflight for field-verified GUI motion steps; use runtime node logs and controller action results as the evidence path. Tested: python3 -m py_compile for panel server, cup_uprighting node files, launch file; real tmux run reached camera pose, manual pick, lift, upright, place, retract, and logged '== 시퀀스 완료 =='. Not-tested: Panel button click after commit; Kang lid-grip logic remains unverified from terminal/tmux. --- .../azas_cup_uprighting/_base_node.py | 81 ++++++++++++----- .../azas_cup_uprighting/_motion.py | 75 +++++++++++++++- .../yolo_cup_uprighting_node.py | 2 + .../launch/yolo_cup_uprighting.launch.py | 88 ++++++++++++++----- tools/run/robot_pipeline_control_server.py | 12 ++- .../run/run_somyeong_cup_uprighting_direct.sh | 10 ++- 6 files changed, 215 insertions(+), 53 deletions(-) diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py index fe4b0f2..556eb04 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -75,6 +75,8 @@ def __init__(self): # ── 픽 상태 ── self.declare_parameter("auto_pick", False) self.declare_parameter("skip_initial_home_move", False) + self.declare_parameter("controller_action_name", "/dsr01/dsr_moveit_controller/follow_joint_trajectory") + self.declare_parameter("controller_action_wait_sec", 60.0) self.picking = False self.home_xyz = None # (x, y, z) [m] — initialize_home 에서 설정 self.home_ori = None # quat dict {x, y, z, w} @@ -94,16 +96,14 @@ def __init__(self): self.gripper = RG(cfg.GRIPPER_NAME, cfg.TOOLCHARGER_IP, cfg.TOOLCHARGER_PORT) # ── MoveIt ── - log.info("MoveItPy 초기화 중...") - self.robot = MoveItPy(node_name=self.MOVEIT_NODE_NAME) - self.arm = self.robot.get_planning_component(cfg.GROUP_NAME) - self.robot_model = self.robot.get_robot_model() - log.info("MoveItPy 초기화 완료") - - self.ompl_params = self._make_plan_params( - "ompl", "RRTConnect", vel=0.2, acc=0.1, time=2.0) - self.pilz_params = self._make_plan_params( - "pilz_industrial_motion_planner", "PTP", vel=0.15, acc=0.1, time=2.0) + # 현장 실행은 카메라 확인이 먼저다. MoveItPy가 joint state/PlanningScene + # 대기에서 막혀도 OpenCV 화면은 떠야 하므로 모션이 필요해지는 순간까지 + # 초기화를 미룬다. + self.robot = None + self.arm = None + self.robot_model = None + self.ompl_params = None + self.pilz_params = None # ── YOLO ── self.declare_parameter("model_path", cfg.YOLO_MODEL_PATH) @@ -139,6 +139,23 @@ def _make_plan_params(self, pipeline, planner_id, *, p.planning_time = time return p + def _ensure_moveit(self) -> bool: + if self.robot is not None: + return True + + log = self.get_logger() + log.info("MoveItPy 지연 초기화 중...") + self.robot = MoveItPy(node_name=self.MOVEIT_NODE_NAME) + self.arm = self.robot.get_planning_component(cfg.GROUP_NAME) + self.robot_model = self.robot.get_robot_model() + self.ompl_params = self._make_plan_params( + "ompl", "RRTConnect", vel=0.2, acc=0.1, time=2.0) + self.pilz_params = self._make_plan_params( + "pilz_industrial_motion_planner", "PTP", vel=0.15, acc=0.1, time=2.0) + self.on_moveit_ready() + log.info("MoveItPy 지연 초기화 완료") + return True + # ── 콜백 ── def _cam_info_cb(self, msg): self.intrinsics = { @@ -156,9 +173,13 @@ def _depth_cb(self, msg): # Perception 래퍼 # ════════════════════════════════════════════ def transform_to_base(self, cam_xyz_m): + if not self._ensure_moveit(): + return None return perc.transform_to_base(self.robot, self.gripper2cam, cam_xyz_m) def pixel_to_base(self, px, py): + if not self._ensure_moveit(): + return None return perc.pixel_to_base( self.robot, self.gripper2cam, self.depth_image, self.intrinsics, @@ -171,19 +192,35 @@ def run_yolo(self, frame): # Motion 래퍼 # ════════════════════════════════════════════ def plan_pose(self, x, y, z, ori, params=None) -> bool: + if not self._ensure_moveit(): + return False return plan_and_execute( self.robot, self.arm, self.get_logger(), pose_goal=make_pose(x, y, z, ori), - params=params or self.pilz_params) + params=params or self.pilz_params, + node=self, + controller_action_name=self.get_parameter("controller_action_name").value, + controller_action_wait_sec=float( + self.get_parameter("controller_action_wait_sec").value + )) def plan_state(self, state, params=None) -> bool: + if not self._ensure_moveit(): + return False return plan_and_execute( self.robot, self.arm, self.get_logger(), state_goal=state, - params=params or self.ompl_params) + params=params or self.ompl_params, + node=self, + controller_action_name=self.get_parameter("controller_action_name").value, + controller_action_wait_sec=float( + self.get_parameter("controller_action_wait_sec").value + )) def go_home_pose(self) -> bool: """관절 home 자세로 이동.""" + if not self._ensure_moveit(): + return False home_state = RobotState(self.robot_model) home_state.joint_positions = cfg.HOME_JOINTS home_state.update() @@ -200,6 +237,8 @@ def approach_and_redetect(self, target_cls_id: int, target_xy): log = self.get_logger() ori = self.home_ori ox, oy = cfg.APPROACH_OFFSET + if not self._ensure_moveit(): + return None cur_ee = get_ee_matrix(self.robot) ax = target_xy[0] + ox ay = target_xy[1] + oy @@ -296,6 +335,10 @@ def on_ready(self): """Home 이동 완료 후 호출. 자식이 추가 init 가능 (e.g. scan_box).""" pass + def on_moveit_ready(self): + """MoveIt 지연 초기화 직후 호출. 자식이 planning scene 설정 가능.""" + pass + def is_auto_ready(self) -> bool: """auto 모드 트리거 전제 조건.""" return True @@ -310,22 +353,14 @@ def _handle_key_extra(self, key: int): def initialize_home(self) -> bool: log = self.get_logger() if self._skip_initial_home_move: - log.info("[Init] Home 이동 생략: 현재 로봇 자세를 관찰 시작 자세로 사용") - T = get_ee_matrix(self.robot) - self.home_xyz = (T[0, 3], T[1, 3], T[2, 3]) - qx, qy, qz, qw = Rotation.from_matrix(T[:3, :3]).as_quat() - self.home_ori = { - "x": float(qx), - "y": float(qy), - "z": float(qz), - "w": float(qw), - } - log.info(f"[Init] Current = ({T[0,3]:.3f}, {T[1,3]:.3f}, {T[2,3]:.3f}) m") + log.info("[Init] Home 이동/MoveIt 초기화 생략: 카메라 화면을 먼저 시작") self.gripper.open_gripper() time.sleep(1.0) return True log.info("[Init] Home 이동") + if not self._ensure_moveit(): + return False if not self.go_home_pose(): log.error("Home 실패") return False diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_motion.py b/src/azas_cup_uprighting/azas_cup_uprighting/_motion.py index 89dc46c..ec27aa0 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_motion.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_motion.py @@ -1,7 +1,11 @@ """MoveIt 모션 유틸 (순수 함수).""" import numpy as np +import rclpy +import time +from control_msgs.action import FollowJointTrajectory from geometry_msgs.msg import PoseStamped +from rclpy.action import ActionClient from scipy.spatial.transform import Rotation as R from . import _config as cfg @@ -70,8 +74,64 @@ def get_ee_matrix(moveit_robot) -> np.ndarray: return np.asarray(T, dtype=float) +def _wait_future(future, label: str, timeout_sec: float, logger) -> bool: + deadline = time.monotonic() + timeout_sec + while rclpy.ok() and not future.done(): + if time.monotonic() >= deadline: + logger.error(f"{label} timeout after {timeout_sec:.1f}s") + return False + time.sleep(min(0.05, max(0.0, deadline - time.monotonic()))) + return future.done() + + +def _execute_with_controller_action(node, trajectory, logger, *, + action_name: str, + wait_sec: float) -> bool: + trajectory_msg = trajectory + if hasattr(trajectory_msg, "get_robot_trajectory_msg"): + trajectory_msg = trajectory_msg.get_robot_trajectory_msg() + joint_trajectory = trajectory_msg.joint_trajectory + if not joint_trajectory.points: + logger.error("Controller action execution rejected empty trajectory") + return False + + client = ActionClient(node, FollowJointTrajectory, action_name) + if not client.wait_for_server(timeout_sec=wait_sec): + logger.error(f"Controller action server not ready after {wait_sec:.1f}s: {action_name}") + client.destroy() + return False + + goal = FollowJointTrajectory.Goal() + goal.trajectory = joint_trajectory + goal_future = client.send_goal_async(goal) + if not _wait_future(goal_future, "send controller goal", wait_sec, logger): + client.destroy() + return False + goal_handle = goal_future.result() + if not goal_handle.accepted: + logger.error("Controller rejected trajectory") + client.destroy() + return False + + result_future = goal_handle.get_result_async() + if not _wait_future(result_future, "execute controller trajectory", wait_sec, logger): + client.destroy() + return False + result = result_future.result().result + client.destroy() + if result.error_code != FollowJointTrajectory.Result.SUCCESSFUL: + logger.error( + f"Controller execution failed: code={result.error_code} {result.error_string}" + ) + return False + logger.info(f"Controller trajectory reached via {action_name}") + return True + + def plan_and_execute(robot, arm, logger, - pose_goal=None, state_goal=None, params=None) -> bool: + pose_goal=None, state_goal=None, params=None, + node=None, controller_action_name=None, + controller_action_wait_sec: float = 30.0) -> bool: """Pose 또는 RobotState 목표로 plan + execute. 실패 시 False.""" arm.set_start_state_to_current_state() @@ -95,9 +155,18 @@ def plan_and_execute(robot, arm, logger, logger.error("Planning 실패") return False + if node is not None and controller_action_name: + return _execute_with_controller_action( + node, + plan_result.trajectory, + logger, + action_name=str(controller_action_name), + wait_sec=float(controller_action_wait_sec), + ) + result = robot.execute(group_name=cfg.GROUP_NAME, - robot_trajectory=plan_result.trajectory, - blocking=True) + robot_trajectory=plan_result.trajectory, + blocking=True) return bool(result) diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py index 134072d..bf2e3c2 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py @@ -31,6 +31,8 @@ class YoloCupUprightingNode(BaseMoveItPickNode): def __init__(self): super().__init__() + + def on_moveit_ready(self): self.setup_safety_environment() def setup_safety_environment(self): diff --git a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py index 6648f31..e3e52cd 100644 --- a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py +++ b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py @@ -1,5 +1,7 @@ +from copy import deepcopy + from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -8,6 +10,53 @@ from launch_ros.substitutions import FindPackageShare from moveit_configs_utils import MoveItConfigsBuilder + +def _runtime_nodes(context, moveit_params, moveit_py_params): + controller_name = LaunchConfiguration("moveit_controller_name").perform(context) + runtime_moveit_params = deepcopy(moveit_params) + runtime_moveit_params["moveit_simple_controller_manager"] = { + "controller_names": [controller_name], + controller_name: { + "type": "FollowJointTrajectory", + "action_ns": "follow_joint_trajectory", + "default": True, + "joints": [ + "joint_1", + "joint_2", + "joint_3", + "joint_4", + "joint_5", + "joint_6", + ], + }, + } + + yolo_cup_uprighting_node = Node( + package="azas_cup_uprighting", + executable="yolo_cup_uprighting", + name="yolo_cup_uprighting_py", + output="screen", + parameters=[ + runtime_moveit_params, + moveit_py_params, + { + "model_path": ParameterValue( + LaunchConfiguration("model_path"), + value_type=str, + ), + "auto_pick": LaunchConfiguration("auto_pick"), + "skip_initial_home_move": LaunchConfiguration("skip_initial_home_move"), + "controller_action_name": ParameterValue( + LaunchConfiguration("controller_action_name"), + value_type=str, + ), + "controller_action_wait_sec": 60.0, + }, + ], + ) + return [yolo_cup_uprighting_node] + + def generate_launch_description(): # 1. 두산 M0609 로봇의 MoveIt 파라미터 빌드 (URDF, SRDF, Kinematics 등) moveit_config = ( @@ -53,6 +102,16 @@ def generate_launch_description(): default_value="false", description="Use the current robot pose as the camera observation pose without commanding Home first.", ) + moveit_controller_name_arg = DeclareLaunchArgument( + "moveit_controller_name", + default_value="/dsr01/dsr_moveit_controller", + description="MoveIt FollowJointTrajectory controller name for namespaced Doosan bringup.", + ) + controller_action_name_arg = DeclareLaunchArgument( + "controller_action_name", + default_value="/dsr01/dsr_moveit_controller/follow_joint_trajectory", + description="Full FollowJointTrajectory action name used for direct controller execution.", + ) # 3. 공통 안전/충돌 장면: side-grip, dispenser, cup-uprighting이 같은 바닥/벽/디스펜서 기준을 보도록 통일 workspace_collision_scene = IncludeLaunchDescription( @@ -103,33 +162,18 @@ def generate_launch_description(): }], ) - # 4. 컵 직립화(Uprighting) 노드 실행 및 파라미터 주입 - yolo_cup_uprighting_node = Node( - package="azas_cup_uprighting", - executable="yolo_cup_uprighting", - name="yolo_cup_uprighting_py", - output="screen", - parameters=[ - moveit_config.to_dict(), - moveit_py_params, - { - "model_path": ParameterValue( - LaunchConfiguration("model_path"), - value_type=str, - ), - "auto_pick": LaunchConfiguration("auto_pick"), - "skip_initial_home_move": LaunchConfiguration("skip_initial_home_move"), - }, - ], - ) - return LaunchDescription([ model_path_arg, publish_hand_eye_tf_arg, auto_pick_arg, skip_initial_home_move_arg, + moveit_controller_name_arg, + controller_action_name_arg, workspace_collision_scene, world_base_tf, hand_eye_tf, - yolo_cup_uprighting_node, + OpaqueFunction( + function=_runtime_nodes, + args=[moveit_config.to_dict(), moveit_py_params], + ), ]) diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 60a55e2..8329cce 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -771,6 +771,7 @@ class Step: # mechanism are allowed to start from the panel. Static package/launch checks # are not enough for real-motion GUI workflows. "side_grip", + "cup_uprighting", } DOOSAN_STACK_PATTERNS = ( @@ -3506,14 +3507,19 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: ), } cleanup_output = "" - if step.key == "side_grip": - cleanup_output = "\n".join(cleanup_side_grip_stack(grace_sec=3.0)) + if step.key in {"side_grip", "cup_uprighting"}: + if step.key == "side_grip": + cleanup_output = "\n".join(cleanup_side_grip_stack(grace_sec=3.0)) + label = "창현 side_grip" + else: + cleanup_output = "\n".join(cleanup_cup_uprighting_stack(grace_sec=3.0)) + label = "소명 cup_uprighting" time.sleep(1.0) cmd = command_for(step, payload) restart_output = "\n".join( part for part in ( - "[Azas] field-verified tmux mode: 창현 side_grip은 ROS CLI discovery preflight로 막지 않고 검증된 tmux 명령을 직접 실행합니다.", + f"[Azas] field-verified tmux mode: {label}은 ROS CLI discovery preflight로 막지 않고 검증된 tmux 명령을 직접 실행합니다.", "[Azas] 전제: 먼저 'tmux 연결 스택 시작'으로 robot/gripper/camera/joint_relay 창이 떠 있어야 합니다.", cleanup_output, ) diff --git a/tools/run/run_somyeong_cup_uprighting_direct.sh b/tools/run/run_somyeong_cup_uprighting_direct.sh index ad1bf21..4021195 100755 --- a/tools/run/run_somyeong_cup_uprighting_direct.sh +++ b/tools/run/run_somyeong_cup_uprighting_direct.sh @@ -7,8 +7,10 @@ DISPLAY="${DISPLAY:-:0}" XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" MODEL_PATH="${MODEL_PATH:-${ROOT}/src/azas_perception/config/yolo_cup_uprighting_best.pt}" AUTO_PICK="${AUTO_PICK:-false}" -SKIP_INITIAL_HOME_MOVE="${SKIP_INITIAL_HOME_MOVE:-true}" +SKIP_INITIAL_HOME_MOVE="${SKIP_INITIAL_HOME_MOVE:-false}" PUBLISH_HAND_EYE_TF="${PUBLISH_HAND_EYE_TF:-true}" +MOVEIT_CONTROLLER_NAME="${MOVEIT_CONTROLLER_NAME:-/dsr01/dsr_moveit_controller}" +CONTROLLER_ACTION_NAME="${CONTROLLER_ACTION_NAME:-${MOVEIT_CONTROLLER_NAME}/follow_joint_trajectory}" ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" @@ -41,6 +43,8 @@ echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XA echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}" echo "[Azas] model_path=${MODEL_PATH}" echo "[Azas] auto_pick=${AUTO_PICK} skip_initial_home_move=${SKIP_INITIAL_HOME_MOVE} publish_hand_eye_tf=${PUBLISH_HAND_EYE_TF}" +echo "[Azas] moveit_controller_name=${MOVEIT_CONTROLLER_NAME}" +echo "[Azas] controller_action_name=${CONTROLLER_ACTION_NAME}" if [[ ! -f "${MODEL_PATH}" ]]; then echo "[Azas][FAIL] YOLO model missing: ${MODEL_PATH}" >&2 @@ -56,4 +60,6 @@ ros2 launch "${ROOT}/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.p model_path:="${MODEL_PATH}" \ auto_pick:="${AUTO_PICK}" \ skip_initial_home_move:="${SKIP_INITIAL_HOME_MOVE}" \ - publish_hand_eye_tf:="${PUBLISH_HAND_EYE_TF}" + publish_hand_eye_tf:="${PUBLISH_HAND_EYE_TF}" \ + moveit_controller_name:="${MOVEIT_CONTROLLER_NAME}" \ + controller_action_name:="${CONTROLLER_ACTION_NAME}" From 8b216d459a4f7c7e608c8b40210d795633c16836 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 17:01:29 +0900 Subject: [PATCH 33/88] Prevent blind Somyeong gripper closure Stop cup uprighting from issuing gripper close/open commands after a required motion plan fails, and make depth lookup tolerate a zero center pixel by using nearby valid depth. Constraint: real robot testing showed the sequence can detect a cup but fail IK/collision planning before physical grasp. Rejected: closing the gripper after a failed plan | this can close on empty air or near glass without a confirmed reach. Confidence: high Scope-risk: narrow Directive: do not add fallback grasp offsets without field-observed directional error or measured calibration evidence. Tested: python3 -m py_compile src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py src/azas_cup_uprighting/azas_cup_uprighting/_perception.py; git diff --check Not-tested: live Somyeong p-key run after patch because a side_grip motion node was active in tmux and concurrent robot motion would be unsafe. --- .../azas_cup_uprighting/_perception.py | 37 +++++++++++++++---- .../yolo_cup_uprighting_node.py | 35 ++++++++++++++---- 2 files changed, 57 insertions(+), 15 deletions(-) diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_perception.py b/src/azas_cup_uprighting/azas_cup_uprighting/_perception.py index 152f5e3..4850a31 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_perception.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_perception.py @@ -51,13 +51,14 @@ def pixel_to_base(robot, gripper2cam, depth_image, intrinsics, logger.warn("pixel 범위 초과") return None - z_raw = depth_image[py, px] - if z_raw == 0: - logger.warn(f"depth=0 at ({px}, {py})") - return None - - z_m = (float(z_raw) / 1000.0 - if depth_image.dtype == np.uint16 else float(z_raw)) + z_m = _depth_at(depth_image, px, py) + if not np.isfinite(z_m): + z_m = _median_depth_near(depth_image, px, py) + if np.isfinite(z_m): + logger.info(f"depth=0 at ({px}, {py}); using nearby median depth={z_m:.3f}m") + else: + logger.warn(f"depth=0 at ({px}, {py}) and no nearby valid depth") + return None fx, fy = intrinsics["fx"], intrinsics["fy"] ppx, ppy = intrinsics["ppx"], intrinsics["ppy"] @@ -74,6 +75,28 @@ def pixel_to_base(robot, gripper2cam, depth_image, intrinsics, return tuple(float(v) for v in base) +def _median_depth_near(depth_image, cx: int, cy: int, radius: int = 4) -> float: + """주변 patch의 유효 depth median (m). 없으면 inf.""" + if depth_image is None: + return float("inf") + h, w = depth_image.shape[:2] + x1 = max(0, cx - radius) + x2 = min(w, cx + radius + 1) + y1 = max(0, cy - radius) + y2 = min(h, cy + radius + 1) + patch = depth_image[y1:y2, x1:x2] + if patch.size == 0: + return float("inf") + + if depth_image.dtype == np.uint16: + valid = patch[patch > 0].astype(float) / 1000.0 + else: + valid = patch[np.isfinite(patch) & (patch > 0)].astype(float) + if valid.size == 0: + return float("inf") + return float(np.median(valid)) + + def _depth_at(depth_image, cx: int, cy: int) -> float: """픽셀의 depth (m). 없으면 inf.""" if depth_image is None: diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py index bf2e3c2..6f9ef92 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py @@ -169,6 +169,10 @@ def _pick_and_straighten(self, bx, by, bz, cup_theta): safe_z = floor_z + 0.25 + Z_OFFSET log.info(f"== 컵 구출 시퀀스 준비 (각도: {np.degrees(cup_theta):.1f}도) ==") + log.info( + "Target base=(%.3f, %.3f, %.3f), safe_z=%.3f, pick_z=%.3f, place_z=%.3f" + % (bx, by, bz, safe_z, pick_z, place_z) + ) @@ -189,22 +193,30 @@ def _pick_and_straighten(self, bx, by, bz, cup_theta): } # 추출한 현재 방향(current_ori)을 유지하면서 Z축만 상공으로 이동 - self.plan_pose(bx, by, safe_z, current_ori) + if not self.plan_pose(bx, by, safe_z, current_ori): + log.error("[ABORT] [1-1] 상공 진입 실패: 그리퍼를 닫지 않고 중단합니다.") + return False time.sleep(1.0) log.info("[1-2] 상공에서 컵 방향으로 정렬") - self.plan_pose(bx, by, safe_z, target_ori) + if not self.plan_pose(bx, by, safe_z, target_ori): + log.error("[ABORT] [1-2] 컵 방향 정렬 실패: 그리퍼를 닫지 않고 중단합니다.") + return False time.sleep(1.0) log.info("[2] 컵 집기 시작") - self.plan_pose(bx, by, pick_z, target_ori) + if not self.plan_pose(bx, by, pick_z, target_ori): + log.error("[ABORT] [2] 집기 위치 진입 실패: 그리퍼를 닫지 않고 중단합니다.") + return False self.gripper.close_gripper() log.info("[2] 컵 집기 완료") time.sleep(1.0) log.info("[3] Lift Up (다시 바닥 기준 25cm 상공으로 리프트업)") - self.plan_pose(bx, by, safe_z, target_ori) + if not self.plan_pose(bx, by, safe_z, target_ori): + log.error("[ABORT] [3] 리프트업 실패: 컵을 잡은 상태로 후속 직립/릴리즈를 중단합니다.") + return False time.sleep(1.0) # 직립화 실행 (항상 카메라가 위를 향하는 Roll=90 고정) @@ -230,22 +242,29 @@ def _pick_and_straighten(self, bx, by, bz, cup_theta): best_ori = ori_target else: log.error("=> 치명적 오류: 관절 한계로 인해 직립화 궤적 생성에 실패했습니다.") - return + return False log.info("[4-1] 공중에서 컵 수직 정렬 완료") log.info(f"[4-2] Z-Height Adjustment (Z: {place_z:.3f})") - self.plan_pose(place_x, place_y, place_z + 0.02, best_ori) + if not self.plan_pose(place_x, place_y, place_z + 0.02, best_ori): + log.error("[ABORT] [4-2] 놓기 높이 접근 실패: 그리퍼를 열지 않고 중단합니다.") + return False log.info("[5] Place & Release") - self.plan_pose(place_x, place_y, place_z, best_ori) + if not self.plan_pose(place_x, place_y, place_z, best_ori): + log.error("[ABORT] [5] 놓기 위치 진입 실패: 그리퍼를 열지 않고 중단합니다.") + return False self.gripper.open_gripper() time.sleep(1.0) log.info("[6] Retract") - self.plan_pose(place_x, place_y, place_z + 0.15, best_ori) + if not self.plan_pose(place_x, place_y, place_z + 0.15, best_ori): + log.error("[WARN] [6] 후퇴 실패: 릴리즈는 완료됐지만 수동 확인이 필요합니다.") + return False log.info("== 시퀀스 완료 ==") + return True def main(args=None): run_node(YoloCupUprightingNode) From d50208b3c2b09d5f1f4b38fc2404a37e1aa7b672 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 17:06:52 +0900 Subject: [PATCH 34/88] Keep side grip from driving link6 into the floor Make the field side-grip command use perception-derived cup depth plus the existing side Z offset instead of forcing link_6 to a 7 cm base height. Also block the sequence before motion when the robot is not STANDBY and auto-start a local joint-state relay only when /joint_states is missing. Constraint: RViz and field logs showed the previous fixed 0.07 m link_6 target could put the mounted RG2 model through the floor while MoveIt still accepted parts of the motion. Rejected: keeping side_fixed_grasp_z=0.07 | it treats link_6 as the grasp TCP and bypasses the mounted gripper clearance problem. Confidence: high Scope-risk: narrow Directive: do not reintroduce fixed link_6 table-height targets for side grip unless a measured TCP transform and collision clearance check are added. Tested: bash -n tools/run/run_changhyun_side_grip_direct.sh tools/run/run_tmux_logic_sequence.sh; git diff --check; current disconnected robot state blocks before movej with no YOLO launch. Not-tested: live side-grip p-key grasp after patch because the robot reported non-STANDBY/disconnected state. --- tools/run/run_changhyun_side_grip_direct.sh | 29 ++++++++++++++++++--- tools/run/run_tmux_logic_sequence.sh | 4 +-- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/tools/run/run_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh index 27cd06d..3afccfe 100755 --- a/tools/run/run_changhyun_side_grip_direct.sh +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -37,11 +37,22 @@ echo "[Azas] START Changhyun side-grip direct tmux command" echo "[Azas] OpenCV window: confirm cup, then press p. Quit with q/Esc." echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XAUTHORITY}" echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}" -echo "[Azas] start_joint_state_relay=${START_JOINT_STATE_RELAY:-false}" +echo "[Azas] start_joint_state_relay=${START_JOINT_STATE_RELAY:-auto}" echo "[Azas] moving to side-grip camera scan pose before starting YOLO" trap 'jobs -pr | xargs -r kill >/dev/null 2>&1 || true' EXIT +robot_state_output="$( + python3 "${ROOT}/tools/run/ros_call_empty_service.py" \ + /"${SERVICE_PREFIX}"/system/get_robot_state dsr_msgs2/srv/GetRobotState \ + --timeout 5.0 2>&1 || true +)" +echo "${robot_state_output}" +if ! grep -q "robot_state=1" <<<"${robot_state_output}"; then + echo "[Azas] BLOCKED: robot_state is not STANDBY(1); side-grip motion not started." + exit 2 +fi + python3 "${ROOT}/tools/run/direct_movej_joints.py" \ --service-prefix "${SERVICE_PREFIX}" \ --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 \ @@ -65,7 +76,17 @@ ros2 run azas_perception hand_eye_static_tf_node \ # field tmux workflow. ros2 run azas_motion link6_gripper_collision_node & -if [[ "${START_JOINT_STATE_RELAY:-false}" == "true" ]]; then +should_start_relay=false +if [[ "${START_JOINT_STATE_RELAY:-auto}" == "true" ]]; then + should_start_relay=true +elif [[ "${START_JOINT_STATE_RELAY:-auto}" == "auto" ]]; then + if ! timeout 2s ros2 topic echo /joint_states --once >/dev/null 2>&1; then + echo "[Azas] /joint_states sample missing; starting side-grip local relay" + should_start_relay=true + fi +fi + +if [[ "${should_start_relay}" == "true" ]]; then ( sleep 5 python3 "${ROOT}/src/dsr_practice/dsr_practice/joint_state_relay.py" \ @@ -85,10 +106,10 @@ ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ side_short_stage_backoff_m:=0.08 side_grasp_stop_backoff_m:=0.04 side_close_underreach_m:=0.03 \ side_low_retry_lift_m:=0.0 side_low_retry_attempts:=0 \ side_linear_approach_enabled:=true side_final_slide_enabled:=false \ - side_fixed_grasp_z_enabled:=true side_fixed_grasp_z:=0.07 side_project_bbox_center_to_fixed_z:=true \ + side_fixed_grasp_z_enabled:=false side_grasp_z_offset:=0.05 side_project_bbox_center_to_fixed_z:=false \ side_candidate_plan_check_enabled:=true pre_pick_joint1_clearance_deg:=12.0 \ side_move_to_initial_center_before_close:=false verify_motion:=false \ - skip_initial_home_move:=true move_to_camera_home:=false move_joint_home_before_camera_home:=false camera_home_mode:=joint min_motion_z:=0.07 \ + skip_initial_home_move:=true move_to_camera_home:=false move_joint_home_before_camera_home:=false camera_home_mode:=joint min_motion_z:=0.10 \ workspace_xy_clamp_enabled:=false return_home_after_task:=false return_to_camera_home_after_attempt:=true \ workspace_collision_scene_enabled:=false table_collision_enabled:=true table_surface_z:=0.0 table_thickness:=0.04 \ table_size_x:=1.10 table_size_y:=0.65 table_center_x:=0.29 table_center_y:=0.0 table_collision_expand_to_workspace_walls:=true \ diff --git a/tools/run/run_tmux_logic_sequence.sh b/tools/run/run_tmux_logic_sequence.sh index b6dfb34..33d652d 100755 --- a/tools/run/run_tmux_logic_sequence.sh +++ b/tools/run/run_tmux_logic_sequence.sh @@ -156,10 +156,10 @@ run_side_grip() { side_short_stage_backoff_m:=0.08 side_grasp_stop_backoff_m:=0.04 side_close_underreach_m:=0.03 \ side_low_retry_lift_m:=0.0 side_low_retry_attempts:=0 \ side_linear_approach_enabled:=true side_final_slide_enabled:=false \ - side_fixed_grasp_z_enabled:=true side_fixed_grasp_z:=0.07 side_project_bbox_center_to_fixed_z:=true \ + side_fixed_grasp_z_enabled:=false side_grasp_z_offset:=0.05 side_project_bbox_center_to_fixed_z:=false \ side_candidate_plan_check_enabled:=true pre_pick_joint1_clearance_deg:=12.0 \ side_move_to_initial_center_before_close:=false verify_motion:=false \ - move_to_camera_home:=true move_joint_home_before_camera_home:=false camera_home_mode:=joint min_motion_z:=0.07 \ + move_to_camera_home:=true move_joint_home_before_camera_home:=false camera_home_mode:=joint min_motion_z:=0.10 \ workspace_xy_clamp_enabled:=false return_home_after_task:=false return_to_camera_home_after_attempt:=true \ workspace_collision_scene_enabled:=true table_collision_enabled:=true table_surface_z:=0.0 table_thickness:=0.04 \ table_size_x:=1.10 table_size_y:=0.65 table_center_x:=0.29 table_center_y:=0.0 table_collision_expand_to_workspace_walls:=true \ From 3ac3561206354aff7e4540fd130a1d19a0ca0986 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 17:27:13 +0900 Subject: [PATCH 35/88] Avoid false side-grip gripper scene contacts Use a compact link_6 RG2 attached collision envelope for side-grip launch paths and clear stale attached objects before republishing. This keeps gripper collision represented without letting the long default preview body dominate low side-grip plans. Constraint: side-grip must keep gripper collision visible while avoiding false contact against table/dispenser scene objects. Rejected: disabling gripper collision entirely | loses the safety representation the operator requested. Confidence: medium Scope-risk: moderate Directive: If real RG2 geometry is recalibrated, update the compact side-grip parameters instead of reverting to the full default envelope blindly. Tested: python3 -m py_compile src/azas_motion/azas_motion/link6_gripper_collision_node.py tools/run/robot_pipeline_control_server.py; bash -n tools/run/run_changhyun_side_grip_direct.sh tools/run/run_tmux_logic_sequence.sh; git diff --check; ROS one-shot remove/add of link6_gripper_collision_node with compact params. Not-tested: live robot side-grip motion while MoveIt reports the exact colliding scene object. --- .../link6_gripper_collision_node.py | 172 +++++++++++++++--- tools/run/robot_pipeline_control_server.py | 11 +- tools/run/run_changhyun_side_grip_direct.sh | 10 +- tools/run/run_tmux_logic_sequence.sh | 13 +- 4 files changed, 178 insertions(+), 28 deletions(-) diff --git a/src/azas_motion/azas_motion/link6_gripper_collision_node.py b/src/azas_motion/azas_motion/link6_gripper_collision_node.py index a67c430..e89ae17 100644 --- a/src/azas_motion/azas_motion/link6_gripper_collision_node.py +++ b/src/azas_motion/azas_motion/link6_gripper_collision_node.py @@ -15,6 +15,7 @@ from moveit_msgs.msg import AttachedCollisionObject, CollisionObject from rclpy.executors import ExternalShutdownException from rclpy.node import Node +from rclpy.parameter import Parameter from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy from shape_msgs.msg import SolidPrimitive from std_msgs.msg import Header @@ -74,6 +75,27 @@ def __init__(self) -> None: self.declare_parameter("publish_once", False) self.declare_parameter("publish_markers", True) self.declare_parameter("marker_topic", "/azas/link6_gripper/markers") + self.declare_parameter("operation", "add") + self.declare_parameter("remove_on_shutdown", True) + self.declare_parameter("mount_height_m", 0.050) + self.declare_parameter("mount_radius_m", 0.040) + self.declare_parameter("mount_z_m", 0.025) + self.declare_parameter("palm_size_x_m", 0.090) + self.declare_parameter("palm_size_y_m", 0.140) + self.declare_parameter("palm_size_z_m", 0.050) + self.declare_parameter("palm_z_m", 0.075) + self.declare_parameter("include_fingers", True) + self.declare_parameter("finger_size_x_m", 0.035) + self.declare_parameter("finger_size_y_m", 0.018) + self.declare_parameter("finger_size_z_m", 0.160) + self.declare_parameter("finger_y_m", 0.055) + self.declare_parameter("finger_z_m", 0.155) + self.declare_parameter("include_pads", True) + self.declare_parameter("pad_size_x_m", 0.025) + self.declare_parameter("pad_size_y_m", 0.012) + self.declare_parameter("pad_size_z_m", 0.035) + self.declare_parameter("pad_y_m", 0.040) + self.declare_parameter("pad_z_m", 0.245) self.publisher = self.create_publisher( AttachedCollisionObject, @@ -89,9 +111,18 @@ def __init__(self) -> None: self._publish() period = float(self.get_parameter("publish_period_sec").value) - if not bool(self.get_parameter("publish_once").value): + if not bool(self.get_parameter("publish_once").value) and self._operation() == CollisionObject.ADD: self.timer = self.create_timer(max(period, 0.2), self._publish) + def _operation(self) -> int: + operation = str(self.get_parameter("operation").value).lower() + if operation == "remove": + return CollisionObject.REMOVE + return CollisionObject.ADD + + def _float_param(self, name: str) -> float: + return float(self.get_parameter(name).value) + def _attached_object(self) -> AttachedCollisionObject: link_name = str(self.get_parameter("attached_link_name").value) attached = AttachedCollisionObject() @@ -103,30 +134,64 @@ def _attached_object(self) -> AttachedCollisionObject: obj.header = Header() obj.header.frame_id = link_name obj.header.stamp = self.get_clock().now().to_msg() - obj.operation = CollisionObject.ADD + obj.operation = self._operation() + + if obj.operation == CollisionObject.REMOVE: + attached.object = obj + return attached - # Same envelope as rg2_link6_tcp.urdf.xacro: - # flange/mount cylinder, palm, two long fingers, and inward blue pads. + # Default geometry matches rg2_link6_tcp.urdf.xacro. Side-grip runners + # can pass a shorter envelope to avoid false table/dispenser contacts. obj.primitives.extend( [ - cylinder_z(0.050, 0.040), - box((0.090, 0.140, 0.050)), - box((0.035, 0.018, 0.160)), - box((0.035, 0.018, 0.160)), - box((0.025, 0.012, 0.035)), - box((0.025, 0.012, 0.035)), + cylinder_z(self._float_param("mount_height_m"), self._float_param("mount_radius_m")), + box( + ( + self._float_param("palm_size_x_m"), + self._float_param("palm_size_y_m"), + self._float_param("palm_size_z_m"), + ) + ), ] ) obj.primitive_poses.extend( [ - make_pose((0.0, 0.0, 0.025)), - make_pose((0.0, 0.0, 0.075)), - make_pose((0.0, 0.055, 0.155)), - make_pose((0.0, -0.055, 0.155)), - make_pose((0.0, 0.040, 0.245)), - make_pose((0.0, -0.040, 0.245)), + make_pose((0.0, 0.0, self._float_param("mount_z_m"))), + make_pose((0.0, 0.0, self._float_param("palm_z_m"))), ] ) + + if bool(self.get_parameter("include_fingers").value): + finger_size = ( + self._float_param("finger_size_x_m"), + self._float_param("finger_size_y_m"), + self._float_param("finger_size_z_m"), + ) + finger_y = self._float_param("finger_y_m") + finger_z = self._float_param("finger_z_m") + obj.primitives.extend([box(finger_size), box(finger_size)]) + obj.primitive_poses.extend( + [ + make_pose((0.0, finger_y, finger_z)), + make_pose((0.0, -finger_y, finger_z)), + ] + ) + + if bool(self.get_parameter("include_pads").value): + pad_size = ( + self._float_param("pad_size_x_m"), + self._float_param("pad_size_y_m"), + self._float_param("pad_size_z_m"), + ) + pad_y = self._float_param("pad_y_m") + pad_z = self._float_param("pad_z_m") + obj.primitives.extend([box(pad_size), box(pad_size)]) + obj.primitive_poses.extend( + [ + make_pose((0.0, pad_y, pad_z)), + make_pose((0.0, -pad_y, pad_z)), + ] + ) attached.object = obj return attached @@ -134,13 +199,57 @@ def _marker_array(self) -> MarkerArray: link_name = str(self.get_parameter("attached_link_name").value) stamp = self.get_clock().now().to_msg() specs = [ - ("mount", Marker.CYLINDER, (0.0, 0.0, 0.025), (0.080, 0.080, 0.050), (0.42, 0.43, 0.45, 0.95)), - ("palm", Marker.CUBE, (0.0, 0.0, 0.075), (0.090, 0.140, 0.050), (0.08, 0.08, 0.09, 0.95)), - ("left_finger", Marker.CUBE, (0.0, 0.055, 0.155), (0.035, 0.018, 0.160), (0.08, 0.08, 0.09, 0.95)), - ("right_finger", Marker.CUBE, (0.0, -0.055, 0.155), (0.035, 0.018, 0.160), (0.08, 0.08, 0.09, 0.95)), - ("left_pad", Marker.CUBE, (0.0, 0.040, 0.245), (0.025, 0.012, 0.035), (0.05, 0.35, 0.95, 0.95)), - ("right_pad", Marker.CUBE, (0.0, -0.040, 0.245), (0.025, 0.012, 0.035), (0.05, 0.35, 0.95, 0.95)), + ( + "mount", + Marker.CYLINDER, + (0.0, 0.0, self._float_param("mount_z_m")), + ( + self._float_param("mount_radius_m") * 2.0, + self._float_param("mount_radius_m") * 2.0, + self._float_param("mount_height_m"), + ), + (0.42, 0.43, 0.45, 0.95), + ), + ( + "palm", + Marker.CUBE, + (0.0, 0.0, self._float_param("palm_z_m")), + ( + self._float_param("palm_size_x_m"), + self._float_param("palm_size_y_m"), + self._float_param("palm_size_z_m"), + ), + (0.08, 0.08, 0.09, 0.95), + ), ] + if bool(self.get_parameter("include_fingers").value): + finger_y = self._float_param("finger_y_m") + finger_z = self._float_param("finger_z_m") + finger_scale = ( + self._float_param("finger_size_x_m"), + self._float_param("finger_size_y_m"), + self._float_param("finger_size_z_m"), + ) + specs.extend( + [ + ("left_finger", Marker.CUBE, (0.0, finger_y, finger_z), finger_scale, (0.08, 0.08, 0.09, 0.95)), + ("right_finger", Marker.CUBE, (0.0, -finger_y, finger_z), finger_scale, (0.08, 0.08, 0.09, 0.95)), + ] + ) + if bool(self.get_parameter("include_pads").value): + pad_y = self._float_param("pad_y_m") + pad_z = self._float_param("pad_z_m") + pad_scale = ( + self._float_param("pad_size_x_m"), + self._float_param("pad_size_y_m"), + self._float_param("pad_size_z_m"), + ) + specs.extend( + [ + ("left_pad", Marker.CUBE, (0.0, pad_y, pad_z), pad_scale, (0.05, 0.35, 0.95, 0.95)), + ("right_pad", Marker.CUBE, (0.0, -pad_y, pad_z), pad_scale, (0.05, 0.35, 0.95, 0.95)), + ] + ) markers: list[Marker] = [] for index, (name, marker_type, xyz, scale, rgba) in enumerate(specs): marker = Marker() @@ -164,14 +273,20 @@ def _marker_array(self) -> MarkerArray: def _publish(self) -> None: self.publisher.publish(self._attached_object()) - if bool(self.get_parameter("publish_markers").value): + if bool(self.get_parameter("publish_markers").value) and self._operation() == CollisionObject.ADD: self.marker_publisher.publish(self._marker_array()) if not self._logged: + action = "Removing" if self._operation() == CollisionObject.REMOVE else "Publishing" + object_id = str(self.get_parameter("object_id").value) self.get_logger().info( - "Publishing RG2-style attached collision envelope and markers on link_6" + f"{action} RG2-style attached collision object {object_id} on link_6" ) self._logged = True + def publish_remove(self) -> None: + self.set_parameters([Parameter("operation", Parameter.Type.STRING, "remove")]) + self.publisher.publish(self._attached_object()) + def main(args: list[str] | None = None) -> None: rclpy.init(args=args) @@ -186,6 +301,15 @@ def main(args: list[str] | None = None) -> None: except (ExternalShutdownException, KeyboardInterrupt): pass finally: + if ( + rclpy.ok() + and str(node.get_parameter("operation").value).lower() == "add" + and bool(node.get_parameter("remove_on_shutdown").value) + ): + node.publish_remove() + import time + + time.sleep(0.2) node.destroy_node() if rclpy.ok(): rclpy.shutdown() diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 8329cce..e2e46df 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -3201,7 +3201,16 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "dispenser_collision_publish_objects:=true " "dispenser_collision_publish_markers:=true & " "ros2 launch azas_bringup rg2_link6_tcp.launch.py " - "publish_gripper_collision:=true & " + "publish_gripper_collision:=false & " + "(timeout 12s ros2 run azas_motion link6_gripper_collision_node " + "--ros-args -p operation:=remove -p publish_once:=true -p publish_markers:=false || true; " + "ros2 run azas_motion link6_gripper_collision_node " + "--ros-args " + "-p palm_size_x_m:=0.075 -p palm_size_y_m:=0.115 -p palm_size_z_m:=0.040 -p palm_z_m:=0.070 " + "-p finger_size_x_m:=0.030 -p finger_size_y_m:=0.014 -p finger_size_z_m:=0.120 " + "-p finger_y_m:=0.050 -p finger_z_m:=0.125 " + "-p pad_size_x_m:=0.022 -p pad_size_y_m:=0.010 -p pad_size_z_m:=0.025 " + "-p pad_y_m:=0.037 -p pad_z_m:=0.180) & " "ros2 run tf2_ros static_transform_publisher " "--x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 " "--frame-id world --child-frame-id base_link & " diff --git a/tools/run/run_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh index 3afccfe..b6167e5 100755 --- a/tools/run/run_changhyun_side_grip_direct.sh +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -74,7 +74,15 @@ ros2 run azas_perception hand_eye_static_tf_node \ # picker. Keep the launch-side include disabled because it also starts an # auxiliary robot_state_publisher and can stall MoveItPy initialization in the # field tmux workflow. -ros2 run azas_motion link6_gripper_collision_node & +timeout 12s ros2 run azas_motion link6_gripper_collision_node \ + --ros-args -p operation:=remove -p publish_once:=true -p publish_markers:=false || true +ros2 run azas_motion link6_gripper_collision_node \ + --ros-args \ + -p palm_size_x_m:=0.075 -p palm_size_y_m:=0.115 -p palm_size_z_m:=0.040 -p palm_z_m:=0.070 \ + -p finger_size_x_m:=0.030 -p finger_size_y_m:=0.014 -p finger_size_z_m:=0.120 \ + -p finger_y_m:=0.050 -p finger_z_m:=0.125 \ + -p pad_size_x_m:=0.022 -p pad_size_y_m:=0.010 -p pad_size_z_m:=0.025 \ + -p pad_y_m:=0.037 -p pad_z_m:=0.180 & should_start_relay=false if [[ "${START_JOINT_STATE_RELAY:-auto}" == "true" ]]; then diff --git a/tools/run/run_tmux_logic_sequence.sh b/tools/run/run_tmux_logic_sequence.sh index 33d652d..11fd41e 100755 --- a/tools/run/run_tmux_logic_sequence.sh +++ b/tools/run/run_tmux_logic_sequence.sh @@ -128,7 +128,16 @@ ensure_collision_scene() { dispenser_collision_enabled:=true \ dispenser_collision_publish_objects:=true \ dispenser_collision_publish_markers:=true & - ros2 launch azas_bringup rg2_link6_tcp.launch.py publish_gripper_collision:=true & + ros2 launch azas_bringup rg2_link6_tcp.launch.py publish_gripper_collision:=false & + timeout 12s ros2 run azas_motion link6_gripper_collision_node \ + --ros-args -p operation:=remove -p publish_once:=true -p publish_markers:=false || true + ros2 run azas_motion link6_gripper_collision_node \ + --ros-args \ + -p palm_size_x_m:=0.075 -p palm_size_y_m:=0.115 -p palm_size_z_m:=0.040 -p palm_z_m:=0.070 \ + -p finger_size_x_m:=0.030 -p finger_size_y_m:=0.014 -p finger_size_z_m:=0.120 \ + -p finger_y_m:=0.050 -p finger_z_m:=0.125 \ + -p pad_size_x_m:=0.022 -p pad_size_y_m:=0.010 -p pad_size_z_m:=0.025 \ + -p pad_y_m:=0.037 -p pad_z_m:=0.180 & ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id base_link & ros2 run azas_perception hand_eye_static_tf_node --ros-args -p compose_timeout_sec:=30.0 -p allow_direct_fallback:=false & python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=publish_detected -p object_id:=detected_tumbler -p use_lidded_height:=true @@ -164,7 +173,7 @@ run_side_grip() { workspace_collision_scene_enabled:=true table_collision_enabled:=true table_surface_z:=0.0 table_thickness:=0.04 \ table_size_x:=1.10 table_size_y:=0.65 table_center_x:=0.29 table_center_y:=0.0 table_collision_expand_to_workspace_walls:=true \ workspace_boundary_collision_enabled:=true dispenser_collision_enabled:=true dispenser_collision_publish_objects:=true \ - dispenser_collision_publish_markers:=true link6_gripper_collision_enabled:=true \ + dispenser_collision_publish_markers:=true link6_gripper_collision_enabled:=false \ dispenser_collision_config_path:="${ROOT}/src/azas_bringup/config/measured_dispenser_collision.yaml" \ moveit_controller_name:=/${SERVICE_PREFIX}/dsr_moveit_controller start_joint_state_relay:=false ) From dca340796920a24c9c21ec215715a5707a052146 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 17:31:48 +0900 Subject: [PATCH 36/88] Honor direct color recipes in the panel Keep scan result files synchronized for the panel, prevent implicit missing-map dispenser fallback, and pass the panel's current color map when operators enter direct color pump counts such as red1,blue3,green3. Constraint: Direct physical dispenser IDs must still work for diagnostics, while color names require an explicit color map source. Rejected: silently falling back to 1,2,3,4 for color recipes | it can press the wrong dispensers when scan JSON is missing. Confidence: high Scope-risk: moderate Directive: Do not reintroduce automatic physical dispenser fallback for color-name recipes; use --dispenser-ids for diagnostic physical order. Tested: python3 -m py_compile tools/perception/dispenser_color_scan.py tools/run/robot_pipeline_control_server.py tools/run/run_color_recipe_sequence.py; dry-run red1,blue3,green3 with panel color map produced 1,4,4,4,2,2,2; command_for emits --colors plus --color-map-json; git diff --check. Not-tested: live dispenser motion from the browser after restarting the panel server. --- docs/robot_pipeline_control.html | 9 ++-- tools/perception/dispenser_color_scan.py | 40 +++++++++++++++--- tools/run/robot_pipeline_control_server.py | 45 ++++++++++++++++---- tools/run/run_color_recipe_sequence.py | 49 +++++++++++++++++----- 4 files changed, 116 insertions(+), 27 deletions(-) diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index 6e39576..51c980d 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -1841,12 +1841,15 @@

RealSense 카메라 화면

}); const mainDirectInput = document.getElementById("recipeDispenserIds"); - async function resolveRecipeDispenserIdsFromJsonOrFallback() { + async function requireColorMapForRecipeCycle() { const input = document.getElementById("recipeDispenserIds"); const directOrder = input?.value.trim() || ""; if (directOrder) return {order: directOrder, source: /[A-Za-z가-힣]/.test(directOrder) ? "direct colors" : "direct dispenser ids", fallback: false, fallbackReason: ""}; const res = await fetch("/api/dispenser_color_map"); const data = await res.json(); + if (data.fallback || !data.map) { + throw new Error(`색상맵이 없습니다. 먼저 색상 스캔을 성공시키거나, 진단용 물리 디스펜서 번호(예: 1x1,2x1)를 직접 입력하세요.\n${data.fallback_reason || ""}`); + } const order = String(data.sequence_compact || data.sequence_csv || "1x1,2x1,3x1,4x1").trim(); input.value = order; return { @@ -1920,7 +1923,7 @@

RealSense 카메라 화면

document.getElementById("colorJsonCheckBtn")?.addEventListener("click", refreshColorScanResult); document.getElementById("manualColorJsonBtn")?.addEventListener("click", saveManualColorMap); document.getElementById("recipeCycleBtn")?.addEventListener("click", async () => { - try { await resolveRecipeDispenserIdsFromJsonOrFallback(); } + try { await requireColorMapForRecipeCycle(); } catch (err) { log.textContent = String(err); focusLog(); return; } queueOnly(["run_color_recipe_sequence"], "레시피 기반 디스펜서 사이클을 큐에 추가했습니다."); }); @@ -1928,8 +1931,6 @@

RealSense 카메라 화면

queueOnly(["place_cup_holder"], "컵홀더 배치를 큐에 추가했습니다. 이 단계는 MoveItPy 경로계획으로 pre_place→place_final→RG2 open→retreat를 실행합니다."); }); document.getElementById("fullCocktailRealBtn")?.addEventListener("click", async () => { - try { await resolveRecipeDispenserIdsFromJsonOrFallback(); } - catch (err) { log.textContent = String(err); focusLog(); return; } queueOnly([...PREP_STEPS, "side_grip_camera_home", "side_grip", "move_to_color_scan_pose", "color_scan", "run_color_recipe_sequence", "place_cup_holder"], "전체 플로우를 큐에 추가했습니다. (tmux 연결 준비 → 카메라 홈→컵 side-grip → 검증자세 색상 핸들 JSON → 레시피 사이클 → MoveIt 컵홀더 배치)", {clear: true}); }); document.getElementById("oneClickResultBtn")?.addEventListener("click", () => { diff --git a/tools/perception/dispenser_color_scan.py b/tools/perception/dispenser_color_scan.py index 9800300..c5fc921 100644 --- a/tools/perception/dispenser_color_scan.py +++ b/tools/perception/dispenser_color_scan.py @@ -15,6 +15,7 @@ import itertools import json import math +import os import sys from pathlib import Path @@ -55,6 +56,34 @@ } +def write_json_immediately(path: Path, payload: dict[str, str]) -> None: + """Atomically write JSON and fsync it so the panel can read it immediately.""" + path.parent.mkdir(parents=True, exist_ok=True) + tmp = path.with_name(f".{path.name}.{os.getpid()}.tmp") + with tmp.open("w", encoding="utf-8") as handle: + json.dump(payload, handle, ensure_ascii=False, indent=2) + handle.write("\n") + handle.flush() + os.fsync(handle.fileno()) + os.replace(tmp, path) + dir_fd = os.open(str(path.parent), os.O_RDONLY) + try: + os.fsync(dir_fd) + finally: + os.close(dir_fd) + + +def unlink_immediately(path: Path) -> None: + if not path.exists(): + return + path.unlink() + dir_fd = os.open(str(path.parent), os.O_RDONLY) + try: + os.fsync(dir_fd) + finally: + os.close(dir_fd) + + def load_dispenser_ids() -> list[str]: """Return dispenser IDs from calibration.yaml, falling back to 1-4.""" try: @@ -483,10 +512,8 @@ def main() -> int: if unknown_ids: out = Path(args.output) failed_out = out.with_suffix(out.suffix + ".failed") - failed_out.parent.mkdir(parents=True, exist_ok=True) - failed_out.write_text(json.dumps(color_map, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") - if out.exists(): - out.unlink() + write_json_immediately(failed_out, color_map) + unlink_immediately(out) print( "[dispenser_color_scan] ERROR: unknown color result for dispenser(s): " + ", ".join(sorted(unknown_ids, key=lambda x: int(x) if str(x).isdigit() else str(x))), @@ -496,8 +523,9 @@ def main() -> int: print(json.dumps(color_map, ensure_ascii=False)) return 1 out = Path(args.output) - out.parent.mkdir(parents=True, exist_ok=True) - out.write_text(json.dumps(color_map, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") + write_json_immediately(out, color_map) + failed_out = out.with_suffix(out.suffix + ".failed") + unlink_immediately(failed_out) print(f"[dispenser_color_scan] saved: {out}") print(json.dumps(color_map, ensure_ascii=False)) return 0 diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index e2e46df..dd41f8b 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -176,6 +176,34 @@ def _read_json_file(path: Path) -> Any: return json.loads(path.read_text(encoding="utf-8")) +def _write_json_file_immediately(path: Path, data: Any) -> None: + """Atomically write JSON and flush it to disk before returning to the UI.""" + path.parent.mkdir(parents=True, exist_ok=True) + tmp = path.with_name(f".{path.name}.{os.getpid()}.{threading.get_ident()}.tmp") + with tmp.open("w", encoding="utf-8") as handle: + json.dump(data, handle, ensure_ascii=False, indent=2) + handle.write("\n") + handle.flush() + os.fsync(handle.fileno()) + os.replace(tmp, path) + dir_fd = os.open(str(path.parent), os.O_RDONLY) + try: + os.fsync(dir_fd) + finally: + os.close(dir_fd) + + +def _unlink_file_immediately(path: Path) -> None: + if not path.exists(): + return + path.unlink() + dir_fd = os.open(str(path.parent), os.O_RDONLY) + try: + os.fsync(dir_fd) + finally: + os.close(dir_fd) + + def _normalize_color_map(raw: Any) -> dict[str, str]: if not isinstance(raw, dict): raise ValueError("color map must be a JSON object") @@ -3063,7 +3091,11 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe if numeric_dispenser_override: direct_arg = f" --dispenser-ids {shlex.quote(recipe_override)}" else: - direct_arg = f" --colors {shlex.quote(recipe_override)}" + direct_color_map = json.dumps(DISPENSER_PRESS_TARGETS, ensure_ascii=False) + direct_arg = ( + f" --colors {shlex.quote(recipe_override)}" + f" --color-map-json {shlex.quote(direct_color_map)}" + ) return ( f"cd {ROOT} && {ROS_SETUP} && " "python3 tools/run/run_color_recipe_sequence.py --execute --confirm" @@ -4050,6 +4082,8 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: if step.key == "color_scan": try: color_map = json.loads(DISPENSER_COLOR_MAP_PATH.read_text(encoding="utf-8")) + DISPENSER_PRESS_TARGETS.clear() + DISPENSER_PRESS_TARGETS.update({str(k): str(v) for k, v in color_map.items()}) lines = ["--- 색상 스캔 결과 ---"] for did in sorted(color_map.keys(), key=lambda x: int(x) if x.isdigit() else x): lines.append(f" 디스펜서 {did}: {color_map[did]}") @@ -4277,12 +4311,9 @@ def do_POST(self) -> None: validated = {str(k): str(v) for k, v in new_map.items()} DISPENSER_PRESS_TARGETS.clear() DISPENSER_PRESS_TARGETS.update(validated) - DISPENSER_COLOR_MAP_PATH.parent.mkdir(parents=True, exist_ok=True) - DISPENSER_COLOR_MAP_PATH.write_text( - json.dumps(validated, ensure_ascii=False, indent=2) + "\n", - encoding="utf-8", - ) - self.send_json({"map": DISPENSER_PRESS_TARGETS}) + _write_json_file_immediately(DISPENSER_COLOR_MAP_PATH, validated) + _unlink_file_immediately(DISPENSER_COLOR_MAP_FAILED_PATH) + self.send_json(dispenser_color_map_status()) return if path == "/api/command_override": step_key = str(payload.get("key") or "") diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 6e5e419..86e536f 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -22,18 +22,33 @@ RECIPE_PATH = ROOT / "outputs" / "latest_recipe.json" SEQUENCE_SCRIPT = ROOT / "tools" / "run" / "run_measured_dispenser_recipe_sequence.py" CONFIRM_PHRASE = "ENABLE_MEASURED_DISPENSER_RECIPE_SEQUENCE" -FALLBACK_DISPENSER_SEQUENCE = ["1", "2", "3", "4"] + +def normalize_color_map(data: object) -> dict[str, str]: + if not isinstance(data, dict): + return {} + return {str(k): str(v).lower().strip() for k, v in data.items()} -def load_color_map() -> dict[str, str]: - """dispenser_id → color_name 매핑 로드. 정상 맵이 없으면 빈 dict로 fallback.""" +def load_color_map(*, override_json: str = "") -> dict[str, str]: + """Load dispenser_id → color_name mapping from the latest scan JSON.""" + if override_json.strip(): + try: + mapped = normalize_color_map(json.loads(override_json)) + except json.JSONDecodeError as exc: + print(f"[run_color_recipe] 직접 색상맵 JSON 파싱 실패: {exc}", file=sys.stderr) + return {} + if mapped and not all(v == "unknown" for v in mapped.values()): + print("[run_color_recipe] 패널 직접 색상맵 사용") + return mapped + print("[run_color_recipe] 패널 직접 색상맵이 비어 있거나 전부 unknown", file=sys.stderr) + return {} if not COLOR_MAP_PATH.exists(): - print(f"[run_color_recipe] 색상 맵 없음: {COLOR_MAP_PATH}; fallback 1,2,3,4 사용", file=sys.stderr) + print(f"[run_color_recipe] 색상 맵 없음: {COLOR_MAP_PATH}", file=sys.stderr) return {} data = json.loads(COLOR_MAP_PATH.read_text(encoding="utf-8")) - mapped = {str(k): str(v).lower().strip() for k, v in data.items()} + mapped = normalize_color_map(data) if not mapped or all(v == "unknown" for v in mapped.values()): - print("[run_color_recipe] 색상 맵이 비어 있거나 전부 unknown; fallback 1,2,3,4 사용", file=sys.stderr) + print("[run_color_recipe] 색상 맵이 비어 있거나 전부 unknown", file=sys.stderr) return {} return mapped @@ -125,6 +140,8 @@ def main() -> int: help="직접 색깔 지정: 'red:2,blue:1', 'redx2,bluex1', 'red2,blue1' (생략 시 latest_recipe.json 사용)") parser.add_argument("--dispenser-ids", default="", help="직접 물리 디스펜서 지정: '1,2,2,3' 또는 '1x1,2x2,3x1'") + parser.add_argument("--color-map-json", default="", + help="패널이 현재 알고 있는 dispenser_id→color JSON. --colors 직접 입력 시 우선 사용") parser.add_argument("--confirm", action="store_true", help=f"확인 구문({CONFIRM_PHRASE}) 자동 전달") parser.add_argument("--execute", action="store_true", @@ -141,6 +158,11 @@ def main() -> int: parser.add_argument("--wait-service-sec", default="15.0") parser.add_argument("--pose-read-retries", default="3") parser.add_argument("--pose-read-retry-sleep-sec", default="0.5") + parser.add_argument( + "--allow-missing-color-map-fallback", + action="store_true", + help="debug only: if no color map exists, run physical dispensers 1,2,3,4", + ) args = parser.parse_args() if args.execute and not args.confirm: print(f"[BLOCKED] --execute requires --confirm ({CONFIRM_PHRASE})", file=sys.stderr) @@ -185,12 +207,19 @@ def main() -> int: result = subprocess.run(cmd, check=False) return result.returncode - color_map = load_color_map() - print(f"[run_color_recipe] 색상 맵: {color_map if color_map else 'fallback 1,2,3,4'}") + color_map = load_color_map(override_json=args.color_map_json) + print(f"[run_color_recipe] 색상 맵: {color_map if color_map else 'missing/invalid'}") if not color_map: - dispenser_ids_str = ",".join(FALLBACK_DISPENSER_SEQUENCE) - print(f"[run_color_recipe] fallback 직접 디스펜서 실행 순서: {dispenser_ids_str}") + if not args.allow_missing_color_map_fallback: + print( + "[BLOCKED] 색상 기반 레시피는 outputs/dispenser_color_map.json이 필요합니다. " + "색상 스캔을 성공시키거나, 진단용으로 물리 디스펜서 번호를 직접 입력하세요.", + file=sys.stderr, + ) + return 2 + dispenser_ids_str = "1,2,3,4" + print(f"[run_color_recipe] debug fallback 직접 디스펜서 실행 순서: {dispenser_ids_str}") cmd = [sys.executable, str(SEQUENCE_SCRIPT), "--dispenser-ids", dispenser_ids_str, *sequence_extra_args] if args.execute: cmd += ["--execute"] From cb58ac17136dce6f7d15f0e1490349d29adfb6e2 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 20:01:26 +0900 Subject: [PATCH 37/88] Enhance lid grip planning and execution with new parameters and validation - Added parameters for joint-space approach and overhead approach in lid grip planning. - Implemented validation for IK fallback joints to ensure safety during execution. - Updated scripts for improved handling of color recipe sequences and dispenser interactions. - Adjusted default values for various motion parameters to optimize performance. - Enhanced logging and error handling for better debugging and user feedback. --- docs/robot_pipeline_control.html | 62 +++++- .../lid_sticker_grip_planning.launch.py | 25 +++ .../azas_motion/lid_grip_planner_node.py | 194 +++++++++++++++++- tools/run/run_changhyun_side_grip_direct.sh | 4 +- tools/run/run_color_recipe_sequence.py | 16 +- tools/run/run_kang_lid_grip_close_direct.sh | 41 +++- .../run_measured_dispenser_recipe_sequence.py | 173 ++++++++++++++-- 7 files changed, 476 insertions(+), 39 deletions(-) diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index 51c980d..cb3b5e4 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -856,7 +856,7 @@

Azas Robot Pipeline Control

- + @@ -1879,6 +1879,61 @@

RealSense 카메라 화면

focusLog(); } + async function runExpandedKeysNow(keys, message) { + if (isRunning) { + log.textContent = "이미 실행 중입니다. 현재 실행이 끝나거나 정리/중지 후 다시 누르세요."; + focusLog(); + return; + } + selectedQueue = []; + const missing = keys.filter((key) => !stepByKey(key)); + const added = addQueueItems(keys); + itemStatuses = new Map(); + resetResultBadges(); + updateSelectedCount(); + renderSteps(); + isRunning = Boolean(added); + activeLogKeys = new Set(keys); + if (isRunning) startRunningLogPolling(keys); + renderFlow(selectedQueue[0]?.id || ""); + log.textContent = `${message}\n실행 시작: ${added}/${keys.length}${missing.length ? `\n누락된 key: ${missing.join(", ")}` : ""}`; + focusLog(); + if (!isRunning) return; + try { + const body = payload(); + body.selected = keys; + body.selected_already_expanded = true; + const res = await fetch("/api/run", { + method: "POST", + headers: {"Content-Type": "application/json"}, + body: JSON.stringify(body) + }); + const data = await res.json(); + const results = Array.isArray(data.results) && data.results.length + ? data.results + : [{ + key: "pipeline", + status: "failed", + output: data.error ? `서버 오류: ${data.error}` : `결과 없음: ${JSON.stringify(data)}` + }]; + for (const returned of results) { + const queuedItem = selectedQueue.find((item) => item.key === returned.key); + setStepStatus(returned.key || "pipeline", returned.status, queuedItem?.id || ""); + } + log.textContent = JSON.stringify({execution_order: data.execution_order || keys, results}, null, 2); + if (results.some((item) => item.key === "color_scan" && item.status === "passed")) { + await refreshColorScanResult(); + } + } catch (err) { + log.textContent = String(err); + } finally { + isRunning = false; + stopRunningLogPolling(); + refreshRunningLogs(true); + updateSelectedCount(); + } + } + document.getElementById("oneClickReadyBtn")?.addEventListener("click", () => { queueOnly(["check_one_click_cocktail_ready"], "준비확인을 큐에 추가했습니다."); }); @@ -1918,7 +1973,10 @@

RealSense 카메라 화면

}); document.getElementById("colorScanJsonBtn")?.addEventListener("click", () => { - queueOnly(["connect_robot", "status_check", "move_to_color_scan_pose", "start_camera", "color_scan"], "로봇 연결 확인 → 검증된 색상 스캔 자세 이동 → 저부하 RealSense → 1.5초/5프레임 안정화 → 화면의 색상 핸들 직접 검출 → JSON/debug 이미지 저장을 큐에 추가했습니다."); + runExpandedKeysNow( + ["move_to_color_scan_pose", "color_scan"], + "빠른 색상구분: 검증된 색상 스캔 자세 이동 → 색상 핸들 검출 → JSON/debug 이미지 저장을 바로 실행합니다. 연결/카메라는 상단 준비 버튼으로 이미 켜져 있어야 합니다." + ); }); document.getElementById("colorJsonCheckBtn")?.addEventListener("click", refreshColorScanResult); document.getElementById("manualColorJsonBtn")?.addEventListener("click", saveManualColorMap); diff --git a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py index ce5f856..292e14e 100644 --- a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py +++ b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py @@ -117,6 +117,11 @@ def generate_launch_description(): DeclareLaunchArgument("line_velocity", default_value="15.0"), DeclareLaunchArgument("line_acceleration", default_value="30.0"), DeclareLaunchArgument("move_timeout_sec", default_value="10.0"), + DeclareLaunchArgument("approach_lid_with_movej", default_value="false"), + DeclareLaunchArgument("approach_movej_velocity", default_value="20.0"), + DeclareLaunchArgument("approach_movej_acceleration", default_value="20.0"), + DeclareLaunchArgument("lid_overhead_approach_enabled", default_value="false"), + DeclareLaunchArgument("lid_overhead_min_z_m", default_value="0.22"), DeclareLaunchArgument("precheck_ikin", default_value="true"), DeclareLaunchArgument("ikin_sol_space", default_value="2"), DeclareLaunchArgument("ikin_timeout_sec", default_value="5.0"), @@ -445,6 +450,26 @@ def generate_launch_description(): LaunchConfiguration("move_timeout_sec"), value_type=float, ), + "approach_lid_with_movej": ParameterValue( + LaunchConfiguration("approach_lid_with_movej"), + value_type=bool, + ), + "approach_movej_velocity": ParameterValue( + LaunchConfiguration("approach_movej_velocity"), + value_type=float, + ), + "approach_movej_acceleration": ParameterValue( + LaunchConfiguration("approach_movej_acceleration"), + value_type=float, + ), + "lid_overhead_approach_enabled": ParameterValue( + LaunchConfiguration("lid_overhead_approach_enabled"), + value_type=bool, + ), + "lid_overhead_min_z_m": ParameterValue( + LaunchConfiguration("lid_overhead_min_z_m"), + value_type=float, + ), "precheck_ikin": ParameterValue( LaunchConfiguration("precheck_ikin"), value_type=bool, diff --git a/src/azas_motion/azas_motion/lid_grip_planner_node.py b/src/azas_motion/azas_motion/lid_grip_planner_node.py index 5536344..da5964a 100644 --- a/src/azas_motion/azas_motion/lid_grip_planner_node.py +++ b/src/azas_motion/azas_motion/lid_grip_planner_node.py @@ -109,6 +109,11 @@ def __init__(self): self.declare_parameter("line_velocity", 15.0) self.declare_parameter("line_acceleration", 30.0) self.declare_parameter("move_timeout_sec", 10.0) + self.declare_parameter("approach_lid_with_movej", False) + self.declare_parameter("approach_movej_velocity", 20.0) + self.declare_parameter("approach_movej_acceleration", 20.0) + self.declare_parameter("lid_overhead_approach_enabled", False) + self.declare_parameter("lid_overhead_min_z_m", 0.22) self.declare_parameter("precheck_ikin", True) self.declare_parameter("ikin_sol_space", 2) self.declare_parameter("ikin_timeout_sec", 5.0) @@ -503,6 +508,22 @@ def _try_motion_sequence(self, source_msg: PoseStamped, plan, allow_gripper: boo real_motion=False, ) return + approach_with_movej = bool(self.get_parameter("approach_lid_with_movej").value) + if approach_with_movej: + if self._move_joint_client is None: + self._publish_status( + "failed", + error="MoveJoint client unavailable; cannot use joint-space lid approach", + real_motion=False, + ) + return + if self._ikin_client is None: + self._publish_status( + "failed", + error="Ikin client unavailable; cannot compute joint-space lid approach", + real_motion=False, + ) + return timeout_sec = float(self.get_parameter("move_timeout_sec").value) if not self._move_line_client.wait_for_service(timeout_sec=max(timeout_sec, 0.0)): self._publish_status( @@ -511,6 +532,14 @@ def _try_motion_sequence(self, source_msg: PoseStamped, plan, allow_gripper: boo real_motion=False, ) return + if approach_with_movej: + if not self._move_joint_client.wait_for_service(timeout_sec=max(timeout_sec, 0.0)): + self._publish_status( + "failed", + error=f"MoveJoint service unavailable: {self._move_joint_service_name}", + real_motion=False, + ) + return if bool(self.get_parameter("verify_motion_reached").value): if self._current_posx_client is None: self._publish_status( @@ -542,8 +571,12 @@ def _try_motion_sequence(self, source_msg: PoseStamped, plan, allow_gripper: boo return use_visual_refine = bool(self.get_parameter("visual_refine_before_grasp").value) - initial_steps = (("approach_lid", plan.approach_pose),) - full_steps = ( + initial_steps = self._initial_approach_precheck_steps(plan.approach_pose) + full_steps = initial_steps + ( + ("grasp_lid", plan.grasp_pose), + ("lift_lid", plan.lift_pose), + ) + motion_steps_without_refine = ( ("approach_lid", plan.approach_pose), ("grasp_lid", plan.grasp_pose), ("lift_lid", plan.lift_pose), @@ -556,7 +589,7 @@ def _try_motion_sequence(self, source_msg: PoseStamped, plan, allow_gripper: boo return if use_visual_refine: - if not self._call_movel("approach_lid", plan.approach_pose): + if not self._call_initial_approach(plan.approach_pose): return refined_steps = self._try_visual_refine_steps(source_msg, plan) if refined_steps is None: @@ -565,10 +598,14 @@ def _try_motion_sequence(self, source_msg: PoseStamped, plan, allow_gripper: boo return motion_steps = refined_steps else: - motion_steps = full_steps + motion_steps = motion_steps_without_refine for label, target in motion_steps: - if not self._call_movel(label, target): + if label in {"approach_lid", "visual_refine_align_lid"}: + motion_ok = self._call_initial_approach(target) + else: + motion_ok = self._call_movel(label, target) + if not motion_ok: return if label == "grasp_lid": if not self._finish_grasp_at_current_pose(gripper_targets): @@ -922,6 +959,153 @@ def _call_movel(self, label: str, pose) -> bool: verify_orientation=False, ) + def _initial_approach_precheck_steps(self, approach_pose) -> tuple: + if not bool(self.get_parameter("lid_overhead_approach_enabled").value): + return (("approach_lid", approach_pose),) + return ( + ("lid_overhead_approach", self._overhead_pose_for_approach(approach_pose)), + ("approach_lid", approach_pose), + ) + + def _overhead_pose_for_approach(self, approach_pose): + overhead_pose = copy.deepcopy(approach_pose) + min_z_m = float(self.get_parameter("lid_overhead_min_z_m").value) + if not math.isfinite(min_z_m): + min_z_m = 0.22 + overhead_pose.position.z = max(float(overhead_pose.position.z), min_z_m) + return overhead_pose + + def _call_initial_approach(self, pose) -> bool: + if bool(self.get_parameter("lid_overhead_approach_enabled").value): + overhead_pose = self._overhead_pose_for_approach(pose) + overhead_pos = self._pose_to_dsr_pos(overhead_pose) + joints_deg = self._ikin_joints_for_pos("lid_overhead_approach", overhead_pos) + if joints_deg is None: + return False + if not self._call_movej_abs( + "lid_overhead_approach", + joints_deg, + velocity=float(self.get_parameter("approach_movej_velocity").value), + acceleration=float(self.get_parameter("approach_movej_acceleration").value), + ): + return False + return self._call_movel("approach_lid", pose) + + if not bool(self.get_parameter("approach_lid_with_movej").value): + return self._call_movel("approach_lid", pose) + if isinstance(pose, (list, tuple)): + pos = [float(value) for value in pose] + else: + pos = self._pose_to_dsr_pos(pose) + joints_deg = self._ikin_joints_for_pos("approach_lid", pos) + if joints_deg is None: + return False + return self._call_movej_abs( + "approach_lid", + joints_deg, + velocity=float(self.get_parameter("approach_movej_velocity").value), + acceleration=float(self.get_parameter("approach_movej_acceleration").value), + ) + + def _ikin_joints_for_pos(self, label: str, pos_mm_deg: list[float]) -> list[float] | None: + if self._ikin_client is None: + self._publish_status( + "failed", + error=f"Ikin client unavailable for {label}", + real_motion=False, + ) + return None + timeout_sec = max(float(self.get_parameter("ikin_timeout_sec").value), 0.1) + if not self._ikin_client.wait_for_service(timeout_sec=timeout_sec): + self._publish_status( + "failed", + error=f"Ikin service unavailable: {self._ikin_service_name}", + real_motion=False, + ) + return None + req = Ikin.Request() + req.pos = [float(value) for value in pos_mm_deg] + req.sol_space = int(self.get_parameter("ikin_sol_space").value) + req.ref = DR_BASE + future = self._ikin_client.call_async(req) + result = self._wait_for_future(future, f"{label}_ikin", timeout_sec) + if result is None or not bool(result.success): + self._publish_status( + "failed", + error=f"{label} Ikin failed before joint-space approach", + target_mm_deg=[round(value, 3) for value in req.pos], + real_motion=False, + ) + return None + joints_deg = [float(value) for value in result.conv_posj] + if len(joints_deg) < 6: + self._publish_status( + "failed", + error=f"{label} Ikin returned fewer than 6 joints", + joints_deg=[round(value, 3) for value in joints_deg], + real_motion=False, + ) + return None + self._publish_status( + "ikin_result", + step=f"{label}_movej", + sol_space=req.sol_space, + joints_deg=[round(value, 3) for value in joints_deg[:6]], + target_mm_deg=[round(value, 3) for value in req.pos], + real_motion=True, + ) + return joints_deg[:6] + + def _call_movej_abs( + self, + label: str, + joints_deg: list[float], + *, + velocity: float, + acceleration: float, + ) -> bool: + if self._move_joint_client is None: + self._publish_status( + "failed", + error=f"MoveJoint client unavailable: {self._move_joint_service_name}", + real_motion=False, + ) + return False + req = MoveJoint.Request() + req.pos = [float(value) for value in joints_deg] + req.vel = float(velocity) + req.acc = float(acceleration) + req.time = 0.0 + req.radius = 0.0 + req.mode = MOVE_MODE_ABSOLUTE + req.blend_type = BLENDING_SPEED_TYPE_DUPLICATE + req.sync_type = SYNC + self.get_logger().warn( + f"[뚜껑픽] 관절 전이 시작: {self._step_ko(label)} " + f"movej_deg=[{', '.join(f'{value:.1f}' for value in req.pos)}]" + ) + future = self._move_joint_client.call_async(req) + result = self._wait_for_future( + future, + label, + float(self.get_parameter("move_timeout_sec").value), + ) + if result is None: + self._publish_status( + "failed", + error=f"{label} MoveJoint timed out or failed", + real_motion=True, + ) + return False + if not bool(result.success): + self._publish_status( + "failed", + error=f"{label} MoveJoint returned success=false", + real_motion=True, + ) + return False + return True + def _call_movel_pos( self, label: str, diff --git a/tools/run/run_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh index b6167e5..d9e293b 100755 --- a/tools/run/run_changhyun_side_grip_direct.sh +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -34,7 +34,7 @@ export RCUTILS_LOGGING_BUFFERED_STREAM=0 mkdir -p "${ROS_LOG_DIR}" echo "[Azas] START Changhyun side-grip direct tmux command" -echo "[Azas] OpenCV window: confirm cup, then press p. Quit with q/Esc." +echo "[Azas] OpenCV window: confirm cup, then press p. On successful side-grip this command exits for the next pipeline step." echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XAUTHORITY}" echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}" echo "[Azas] start_joint_state_relay=${START_JOINT_STATE_RELAY:-auto}" @@ -107,7 +107,7 @@ fi ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ model_path:="${ROOT}/local_models/best.pt" \ conf:=0.35 imgsz:=640 device:=cpu target_class:=cup \ - auto_pick:=false auto_pick_interval:=8.0 exit_after_pick:=false \ + auto_pick:=false auto_pick_interval:=8.0 exit_after_pick:="${EXIT_AFTER_PICK:-true}" \ depth_patch_radius:=7 min_depth_valid_ratio:=0.03 min_depth_m:=0.15 max_depth_m:=1.20 \ redetect_on_approach:=false redetect_settle_sec:=0.5 \ grasp_mode:=side side_far_stage_enabled:=false side_approach_offset:=0.18 \ diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 86e536f..e1d8326 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -146,13 +146,20 @@ def main() -> int: help=f"확인 구문({CONFIRM_PHRASE}) 자동 전달") parser.add_argument("--execute", action="store_true", help="실제 measured dispenser sequence를 실행") - parser.add_argument("--press-min-transit-z-m", default="0.720") + parser.add_argument("--press-min-transit-z-m", default="0.500") parser.add_argument("--press-line-velocity", default="18.0") parser.add_argument("--press-line-acceleration", default="25.0") parser.add_argument("--press-travel-velocity", default="45.0") parser.add_argument("--press-travel-acceleration", default="60.0") parser.add_argument("--press-contact-joint-velocity", default="22.0") parser.add_argument("--press-contact-joint-acceleration", default="30.0") + parser.add_argument("--press-pre-lift-m", default="0.080") + parser.add_argument("--press-transit-height-m", default="0.080") + parser.add_argument("--press-pre-lift-retreat-x-m", default="0.0") + parser.add_argument("--press-pre-lift-retreat-y-m", default="-0.050") + parser.add_argument("--move-release-offset-x-m", default="0.0") + parser.add_argument("--move-release-offset-y-m", default="-0.060") + parser.add_argument("--move-release-offset-z-m", default="-0.010") parser.add_argument("--gripper-open-settle-seconds", default="1.5") parser.add_argument("--gripper-settle-seconds", default="0.8") parser.add_argument("--wait-service-sec", default="15.0") @@ -170,6 +177,13 @@ def main() -> int: sequence_extra_args = [ "--press-min-transit-z-m", str(args.press_min_transit_z_m), + "--press-pre-lift-m", str(args.press_pre_lift_m), + "--press-transit-height-m", str(args.press_transit_height_m), + "--press-pre-lift-retreat-x-m", str(args.press_pre_lift_retreat_x_m), + "--press-pre-lift-retreat-y-m", str(args.press_pre_lift_retreat_y_m), + "--move-release-offset-x-m", str(args.move_release_offset_x_m), + "--move-release-offset-y-m", str(args.move_release_offset_y_m), + "--move-release-offset-z-m", str(args.move_release_offset_z_m), "--press-line-velocity", str(args.press_line_velocity), "--press-line-acceleration", str(args.press_line_acceleration), "--press-travel-velocity", str(args.press_travel_velocity), diff --git a/tools/run/run_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh index 8262950..90d272b 100755 --- a/tools/run/run_kang_lid_grip_close_direct.sh +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -6,12 +6,13 @@ SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" DISPLAY="${DISPLAY:-:0}" XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" MODEL_PATH="${MODEL_PATH:-${ROOT}/local_models/best.pt}" -ARUCO_DICTIONARY="${ARUCO_DICTIONARY:-DICT_6X6_250}" -ARUCO_MARKER_ID="${ARUCO_MARKER_ID:-0}" -ARUCO_FALLBACK_MARKERS="${ARUCO_FALLBACK_MARKERS:-DICT_4X4_50:14}" +ARUCO_DICTIONARY="${ARUCO_DICTIONARY:-DICT_4X4_50}" +ARUCO_MARKER_ID="${ARUCO_MARKER_ID:-14}" +ARUCO_FALLBACK_MARKERS="${ARUCO_FALLBACK_MARKERS:-}" ARUCO_MARKER_LENGTH_M="${ARUCO_MARKER_LENGTH_M:-0.03}" ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +MOVE_TO_LID_VIEW_POSE="${MOVE_TO_LID_VIEW_POSE:-true}" cd "${ROOT}" @@ -32,7 +33,7 @@ set -u export DISPLAY XAUTHORITY ROS_DOMAIN_ID ROS_LOCALHOST_ONLY export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" -export PYTHONPATH="${ROOT}/tools/run/python_compat:${PYTHONPATH:-}" +export PYTHONPATH="${ROOT}/src/azas_motion:${ROOT}/tools/run/python_compat:${PYTHONPATH:-}" mkdir -p "${ROS_LOG_DIR}" "${ROOT}/log/tmux_logic" if [[ "${SERVICE_PREFIX}" != /* ]]; then @@ -49,6 +50,16 @@ if [[ ! -f "${MODEL_PATH}" ]]; then echo "[Azas][WARN] model_path not found: ${MODEL_PATH}" fi +if [[ "${MOVE_TO_LID_VIEW_POSE}" == "true" ]]; then + echo "[Azas] moving to lid camera view pose before ArUco detection" + python3 "${ROOT}/tools/run/direct_movej_joints.py" \ + --service-prefix "${SERVICE_PREFIX}" \ + --j1 3.0 --j2 -20.0 --j3 52.0 --j4 -9.0 --j5 125.0 --j6 90.0 \ + --velocity 10 --acceleration 10 \ + --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 \ + --execute --confirm ENABLE_DIRECT_MOVEJ +fi + ros2 pkg executables azas_perception | grep -q '^azas_perception lid_sticker_detector_node$' || { echo "[Azas][FAIL] missing azas_perception lid_sticker_detector_node" >&2 exit 2 @@ -58,21 +69,24 @@ ros2 pkg executables azas_motion | grep -q '^azas_motion lid_grip_planner_node$' exit 3 } -ros2 launch azas_bringup lid_sticker_grip_planning.launch.py \ +launch_args=( + azas_bringup lid_sticker_grip_planning.launch.py model_path:="${MODEL_PATH}" \ - marker_type:=aruco require_lid_detection:=false \ - allow_aruco_only_after_grip_request:=false aruco_only_after_grip_request_sec:=20.0 \ + marker_type:=aruco require_lid_detection:=true \ + allow_aruco_only_after_grip_request:=true aruco_only_after_grip_request_sec:=20.0 \ aruco_dictionary:="${ARUCO_DICTIONARY}" aruco_marker_id:="${ARUCO_MARKER_ID}" \ - aruco_fallback_markers:="${ARUCO_FALLBACK_MARKERS}" aruco_marker_length_m:="${ARUCO_MARKER_LENGTH_M}" \ + aruco_marker_length_m:="${ARUCO_MARKER_LENGTH_M}" \ use_aruco_axis_for_orientation:=true aruco_finger_axis_quarter_turns:=0 \ use_lid_pose_yaw_for_pick:=true lid_pose_yaw_axis:=y lid_pose_yaw_offset_deg:=0.0 lid_pose_yaw_equivalence_deg:=180.0 \ visual_refine_before_grasp:=true visual_refine_sample_count:=5 visual_refine_timeout_sec:=3.0 visual_refine_max_yaw_std_deg:=3.0 \ visual_refine_max_position_std_m:=0.005 visual_refine_apply_xy:=true visual_refine_apply_yaw:=true visual_refine_fallback_to_initial_plan:=true \ enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION allow_service_control_without_moveit:=true service_prefix:="${SERVICE_PREFIX}" \ + approach_lid_with_movej:=true approach_movej_velocity:=20.0 approach_movej_acceleration:=20.0 \ + lid_overhead_approach_enabled:=true lid_overhead_min_z_m:=0.260 \ rx:=108.41 ry:=-176.32 rz:=175.98 offset_axis:=base_z surface_offset_m:=0.0 \ - tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:=-0.040 min_grasp_z_m:=0.025 \ + tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:=0.0 min_grasp_z_m:=0.065 \ approach_offset_m:=0.08 lift_offset_m:=0.10 settle_seconds_before_grasp:=0.5 hold_seconds_after_grasp:=3.0 \ - line_velocity:=30.0 line_acceleration:=10.0 move_timeout_sec:=90.0 \ + line_velocity:=15.0 line_acceleration:=8.0 move_timeout_sec:=90.0 \ enable_gripper_service_calls:=true gripper_set_service:=/jarvis/rg2/set_width \ gripper_preopen_width_m:=0.110 gripper_grasp_width_m:=0.020 gripper_force_n:=12.0 \ continue_after_gripper_grasp_failure:=true gripper_grasp_failure_wait_sec:=2.0 \ @@ -89,3 +103,10 @@ ros2 launch azas_bringup lid_sticker_grip_planning.launch.py \ lid_twist_release_lift_m:=0.03 lid_twist_min_z_m:=0.140 lid_twist_max_z_m:=0.220 \ lid_twist_transfer_velocity:=25.0 lid_twist_press_velocity:=5.0 lid_twist_turn_velocity:=30.0 lid_twist_acceleration:=15.0 \ lid_twist_hold_seconds_before_turn:=0.0 lid_twist_hold_seconds_after_turn:=0.5 +) + +if [[ -n "${ARUCO_FALLBACK_MARKERS}" ]]; then + launch_args+=(aruco_fallback_markers:="${ARUCO_FALLBACK_MARKERS}") +fi + +ros2 launch "${launch_args[@]}" diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index 99cdd0a..3c0b826 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -463,6 +463,7 @@ def move_posx_joint_fallback( acceleration: float, ) -> None: joints_deg = self.ikin_posj(posx_mm_deg, label=f"{label} IK joint fallback") + self.validate_ik_fallback_joints(joints_deg, label=label) self.movej( joints_deg, label=f"{label} IK MoveJoint fallback", @@ -473,6 +474,7 @@ def move_posx_joint_fallback( def move_front_hold_joint_fallback(self, posx_mm_deg: list[float], *, label: str) -> None: joints_deg = self.ikin_posj(posx_mm_deg, label=f"{label} IK joint fallback") + self.validate_ik_fallback_joints(joints_deg, label=label) self.movej( joints_deg, label=f"{label} IK MoveJoint fallback", @@ -657,9 +659,33 @@ def ikin_posj(self, posx_mm_deg: list[float], *, label: str) -> list[float]: ) return values + def validate_ik_fallback_joints(self, joints_deg: list[float], *, label: str) -> None: + max_abs = max(float(self.args.ik_fallback_max_abs_joint_deg), 0.0) + if max_abs > 0.0: + for index, value in enumerate(joints_deg, start=1): + if abs(value) > max_abs: + raise RuntimeError( + f"IK fallback rejected for {label}: joint_{index}={value:.1f}deg " + f"exceeds limit {max_abs:.1f}deg" + ) + max_delta = max(float(self.args.ik_fallback_max_joint_delta_deg), 0.0) + if max_delta <= 0.0: + return + current = self.current_posj(timeout_sec=5.0) + deltas = [abs(joints_deg[index] - current[index]) for index in range(6)] + worst_delta = max(deltas) + if worst_delta > max_delta: + joint_index = deltas.index(worst_delta) + 1 + raise RuntimeError( + f"IK fallback rejected for {label}: joint_{joint_index} delta " + f"{worst_delta:.1f}deg exceeds limit {max_delta:.1f}deg" + ) + def wait_for_joint_target(self, target_joints_deg: list[float], *, label: str) -> None: deadline = time.monotonic() + max(self.args.verify_timeout_sec, 0.1) last_error = 999999.0 + best_error = last_error + last_progress_time = time.monotonic() while time.monotonic() < deadline: actual = self.current_posj(timeout_sec=5.0) errors = [abs(actual[index] - target_joints_deg[index]) for index in range(6)] @@ -670,6 +696,19 @@ def wait_for_joint_target(self, target_joints_deg: list[float], *, label: str) - ) if last_error <= max(self.args.joint_target_tolerance_deg, 0.1): return + if best_error - last_error >= max(self.args.target_stall_delta_mm, 0.1): + best_error = last_error + last_progress_time = time.monotonic() + elif ( + self.args.target_stall_timeout_sec > 0.0 + and last_error >= max(self.args.joint_target_tolerance_deg, 0.1) + and time.monotonic() - last_progress_time >= max(self.args.target_stall_timeout_sec, 0.0) + ): + raise RuntimeError( + f"joint target verification stalled for {label}; " + f"max_error={last_error:.2f}deg best={best_error:.2f}deg " + f"no_progress_for={time.monotonic() - last_progress_time:.1f}s" + ) time.sleep(max(self.args.verify_poll_seconds, 0.05)) raise RuntimeError(f"joint target verification timeout for {label}; max_error={last_error:.2f}deg") @@ -727,8 +766,22 @@ def move_and_release(self, dispenser_id: str) -> None: self.args.move_prehold_velocity, self.args.move_prehold_acceleration, ), - ("above-hold", 0.0, 0.0, self.args.move_prehold_offset_z_m, self.args.move_prehold_velocity, self.args.move_prehold_acceleration), - ("front-hold", 0.0, 0.0, 0.0, self.args.move_velocity, self.args.move_acceleration), + ( + "above-hold", + self.args.move_prehold_offset_x_m, + self.args.move_prehold_offset_y_m, + self.args.move_prehold_offset_z_m, + self.args.move_prehold_velocity, + self.args.move_prehold_acceleration, + ), + ( + "front-hold", + self.args.move_release_offset_x_m, + self.args.move_release_offset_y_m, + self.args.move_release_offset_z_m, + self.args.move_velocity, + self.args.move_acceleration, + ), ] seen: set[tuple[float, float, float, float, float]] = set() for stage_label, offset_x, offset_y, offset_z, velocity, acceleration in stages: @@ -779,8 +832,9 @@ def regrasp_and_lift(self, dispenser_id: str) -> None: timeout_sec=self.args.move_timeout_sec, ) front_hold_position, _, _ = load_front_hold_pose(self.args.config, dispenser_id) + released_hold_z_m = front_hold_position[2] + self.args.move_release_offset_z_m desired_approach_z_m = max( - front_hold_position[2] + max(self.args.regrasp_approach_offset_z_m, 0.0), + released_hold_z_m + max(self.args.regrasp_approach_offset_z_m, 0.0), max(self.args.regrasp_min_transit_z_m, 0.0), ) capped_approach_z_m = min(desired_approach_z_m, max(self.args.regrasp_max_transit_z_m, 0.0)) @@ -793,8 +847,8 @@ def regrasp_and_lift(self, dispenser_id: str) -> None: self.move_front_hold( dispenser_id, label="final re-grasp high transit above front-hold", - offset_x_m=0.0, - offset_y_m=0.0, + offset_x_m=self.args.move_release_offset_x_m, + offset_y_m=self.args.move_release_offset_y_m, offset_z_m=approach_offset_z_m, velocity=self.args.regrasp_approach_velocity, acceleration=self.args.regrasp_approach_acceleration, @@ -809,9 +863,9 @@ def regrasp_and_lift(self, dispenser_id: str) -> None: self.move_front_hold( dispenser_id, label="final re-grasp front-hold", - offset_x_m=0.0, - offset_y_m=0.0, - offset_z_m=0.0, + offset_x_m=self.args.move_release_offset_x_m, + offset_y_m=self.args.move_release_offset_y_m, + offset_z_m=self.args.move_release_offset_z_m, velocity=self.args.pick_approach_velocity, acceleration=self.args.pick_approach_acceleration, ) @@ -900,6 +954,23 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: f"contact=({x_mm:.1f}, {y_mm:.1f}, {contact_z:.1f}) " f"pre_z={pre_z:.1f} pressed_z={pressed_z:.1f} transit_z={transit_z:.1f}" ) + if abs(self.args.press_pre_lift_retreat_x_m) > 1e-6 or abs(self.args.press_pre_lift_retreat_y_m) > 1e-6: + retreat = [ + current_pose[0] + self.args.press_pre_lift_retreat_x_m * 1000.0, + current_pose[1] + self.args.press_pre_lift_retreat_y_m * 1000.0, + current_pose[2], + current_pose[3], + current_pose[4], + current_pose[5], + ] + self.move_posx( + retreat, + label="safe lateral retreat away from dispenser before press lift", + velocity=self.args.press_travel_velocity, + acceleration=self.args.press_travel_acceleration, + timeout_sec=self.args.press_timeout_sec, + ) + current_pose = self.current_posx(timeout_sec=self.args.wait_service_sec) safe_lift = [ current_pose[0], current_pose[1], @@ -1302,16 +1373,42 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--move-velocity", type=float, default=70.0) parser.add_argument("--move-acceleration", type=float, default=90.0) parser.add_argument("--move-prehold-offset-x-m", type=float, default=0.0) - parser.add_argument("--move-prehold-offset-y-m", type=float, default=0.0) + parser.add_argument( + "--move-prehold-offset-y-m", + type=float, + default=-0.080, + help=( + "Y retreat from measured front_hold for the pre-hold/above-hold approach. " + "Default -0.080 m keeps the cup farther from dispenser bottles before final placement." + ), + ) parser.add_argument( "--move-prehold-offset-z-m", type=float, - default=0.300, + default=0.180, help=( "Vertical approach offset for initial cup placement at dispenser front-hold. " - "Default 0.300 m keeps lateral travel above bottles/dispensers before descending." + "Default 0.180 m avoids the previously too-high approach near the glass bottles." ), ) + parser.add_argument( + "--move-release-offset-x-m", + type=float, + default=0.0, + help="Final cup release X offset from measured front_hold.", + ) + parser.add_argument( + "--move-release-offset-y-m", + type=float, + default=-0.060, + help="Final cup release Y retreat from measured front_hold to avoid placing too close to the dispenser bottle.", + ) + parser.add_argument( + "--move-release-offset-z-m", + type=float, + default=-0.010, + help="Final cup release Z offset from measured front_hold; negative lowers the cup slightly.", + ) parser.add_argument("--move-prehold-velocity", type=float, default=50.0) parser.add_argument("--move-prehold-acceleration", type=float, default=70.0) parser.add_argument("--move-timeout-sec", type=float, default=180.0) @@ -1329,13 +1426,13 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--regrasp-min-transit-z-m", type=float, - default=0.720, + default=0.500, help="Minimum absolute TCP Z for the vertical lift immediately after pressing, before returning to the cup.", ) parser.add_argument( "--regrasp-approach-offset-z-m", type=float, - default=0.650, + default=0.250, help=( "High front-hold Z offset used before opening the gripper for the post-press re-grasp. " "The gripper opens only after this high approach is reached." @@ -1344,7 +1441,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--regrasp-max-transit-z-m", type=float, - default=0.780, + default=0.560, help="Maximum absolute TCP/front-hold high approach Z used for post-press re-grasp transit.", ) parser.add_argument("--regrasp-approach-velocity", type=float, default=45.0) @@ -1379,15 +1476,27 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--press-pre-lift-m", type=float, - default=0.300, + default=0.080, help="Z lift above the measured dispenser-head contact pose before descending to press.", ) parser.add_argument("--press-approach-height-m", type=float, default=0.100) - parser.add_argument("--press-transit-height-m", type=float, default=0.300) + parser.add_argument("--press-transit-height-m", type=float, default=0.080) + parser.add_argument( + "--press-pre-lift-retreat-x-m", + type=float, + default=0.0, + help="X retreat after cup release and before the vertical press lift.", + ) + parser.add_argument( + "--press-pre-lift-retreat-y-m", + type=float, + default=-0.050, + help="Y retreat after cup release and before the vertical press lift; negative backs away from the dispenser.", + ) parser.add_argument( "--press-min-transit-z-m", type=float, - default=0.720, + default=0.500, help="Minimum absolute TCP Z before moving from cup release toward dispenser press joints.", ) parser.add_argument("--press-line-velocity", type=float, default=18.0) @@ -1462,6 +1571,18 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--target-stall-min-distance-mm", type=float, default=80.0) parser.add_argument("--target-stall-delta-mm", type=float, default=2.0) parser.add_argument("--joint-target-tolerance-deg", type=float, default=2.0) + parser.add_argument( + "--ik-fallback-max-abs-joint-deg", + type=float, + default=360.0, + help="Reject IK fallback joint solutions with absolute joint values beyond this limit before commanding MoveJoint.", + ) + parser.add_argument( + "--ik-fallback-max-joint-delta-deg", + type=float, + default=170.0, + help="Reject IK fallback joint solutions that jump too far from the current joint state before commanding MoveJoint.", + ) parser.add_argument( "--front-hold-joint-fallback", action=argparse.BooleanOptionalAction, @@ -1522,6 +1643,14 @@ def parse_args() -> argparse.Namespace: "post-press singularity/low direct approach." ), ) + parser.add_argument( + "--skip-initial-move-release", + action="store_true", + help=( + "Recovery mode: assume the cup is already resting at the current dispenser front-hold " + "and start from press -> re-grasp/lift without repeating the move/release placement." + ), + ) parser.add_argument("--execute", action="store_true") parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} when --execute is used") args = parser.parse_args() @@ -1580,13 +1709,19 @@ def main() -> int: for index, (dispenser_id, press_count) in enumerate(grouped_dispenser_ids, start=1): label_prefix = f"recipe group {index}/{total_groups} dispenser {dispenser_id} x{press_count}" if not args.execute: + move_release_step = "skip initial move/release" if args.skip_initial_move_release else "integrated move/release" print( - f"[PLAN] {label_prefix}: integrated move/release -> " + f"[PLAN] {label_prefix}: {move_release_step} -> " f"integrated press {press_count} time(s) -> integrated re-grasp/lift" ) continue - if motion is None: + if args.skip_initial_move_release: + print( + f"[Azas] {label_prefix}: skipping initial move/release; " + "cup is assumed already released at dispenser front-hold" + ) + elif motion is None: rc = run_command(f"{label_prefix}: move cup to front-hold and release", move_and_release_cmd(args, dispenser_id)) if rc != 0: return rc From b272706fda6428b9504468395c1fe170cc3cc27e Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Tue, 9 Jun 2026 20:01:30 +0900 Subject: [PATCH 38/88] Add --force-cartesian-press option to override contact joints during dispenser press --- tools/run/run_color_recipe_sequence.py | 3 +++ tools/run/run_measured_dispenser_recipe_sequence.py | 10 +++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index e1d8326..d6d0784 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -160,6 +160,7 @@ def main() -> int: parser.add_argument("--move-release-offset-x-m", default="0.0") parser.add_argument("--move-release-offset-y-m", default="-0.060") parser.add_argument("--move-release-offset-z-m", default="-0.010") + parser.add_argument("--force-cartesian-press", action="store_true") parser.add_argument("--gripper-open-settle-seconds", default="1.5") parser.add_argument("--gripper-settle-seconds", default="0.8") parser.add_argument("--wait-service-sec", default="15.0") @@ -198,6 +199,8 @@ def main() -> int: "--safe-lift-joint-fallback", "--no-integrated-regrasp-fallback-subprocess", ] + if args.force_cartesian_press: + sequence_extra_args.append("--force-cartesian-press") direct_dispenser_ids = args.dispenser_ids.strip() if direct_dispenser_ids: diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index 3c0b826..4c6f8bc 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -896,7 +896,7 @@ def regrasp_and_lift(self, dispenser_id: str) -> None: def press_dispenser(self, dispenser_id: str, press_count: int) -> None: press_xyz_m, press_rpy_deg = load_press_pose(dispenser_id) current_pose = self.current_posx() - contact_joints = load_press_ready_joints_deg(dispenser_id) + contact_joints = None if self.args.force_cartesian_press else load_press_ready_joints_deg(dispenser_id) joint_space_press = contact_joints is not None if contact_joints is None: x_mm = press_xyz_m[0] * 1000.0 @@ -1533,6 +1533,14 @@ def parse_args() -> argparse.Namespace: "use the measured joints as the authoritative press target." ), ) + parser.add_argument( + "--force-cartesian-press", + action="store_true", + help=( + "Ignore dispenser press_contact_joints_deg and press using measured " + "press_pose_xyz_m/press_pose_rpy_deg Cartesian poses." + ), + ) parser.add_argument( "--press-post-retreat-after-sequence", action=argparse.BooleanOptionalAction, From 9285161d1d9becabef0e15de14ed8b90744b093c Mon Sep 17 00:00:00 2001 From: ummobro Date: Wed, 10 Jun 2026 13:37:21 +0900 Subject: [PATCH 39/88] feat: add J6 lid grip and step preseat workflow --- .../lid_sticker_grip_planning.launch.py | 70 ++ .../azas_motion/lid_grip_planner_node.py | 597 +++++++++++++++++- 2 files changed, 640 insertions(+), 27 deletions(-) diff --git a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py index f62218b..8bc2cf6 100644 --- a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py +++ b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py @@ -113,6 +113,14 @@ def generate_launch_description(): DeclareLaunchArgument("lid_pose_yaw_axis", default_value="x"), DeclareLaunchArgument("lid_pose_yaw_offset_deg", default_value="0.0"), DeclareLaunchArgument("lid_pose_yaw_equivalence_deg", default_value="180.0"), + DeclareLaunchArgument("use_j6_yaw_for_pick", default_value="false"), + DeclareLaunchArgument("pick_j6_yaw_axis", default_value="y"), + DeclareLaunchArgument("pick_j6_yaw_sign", default_value="-1.0"), + DeclareLaunchArgument("pick_j6_yaw_offset_deg", default_value="1.2"), + DeclareLaunchArgument("pick_j6_yaw_equivalence_deg", default_value="360.0"), + DeclareLaunchArgument("pick_j6_yaw_tolerance_deg", default_value="1.0"), + DeclareLaunchArgument("pick_j6_velocity", default_value="30.0"), + DeclareLaunchArgument("pick_j6_acceleration", default_value="15.0"), DeclareLaunchArgument("line_velocity", default_value="15.0"), DeclareLaunchArgument("line_acceleration", default_value="30.0"), DeclareLaunchArgument("move_timeout_sec", default_value="10.0"), @@ -166,6 +174,7 @@ def generate_launch_description(): DeclareLaunchArgument("lid_twist_regrip_gripper_wait_sec", default_value="0.5"), DeclareLaunchArgument("lid_twist_force_settle_seconds", default_value="0.4"), DeclareLaunchArgument("lid_twist_force_release_time", default_value="0.2"), + DeclareLaunchArgument("lid_twist_preseat_mode", default_value="periodic"), DeclareLaunchArgument("lid_twist_preseat_periodic_before_turn", default_value="false"), DeclareLaunchArgument("lid_twist_preseat_periodic_x_amp_mm", default_value="0.0"), DeclareLaunchArgument("lid_twist_preseat_periodic_y_amp_mm", default_value="0.0"), @@ -177,6 +186,11 @@ def generate_launch_description(): DeclareLaunchArgument("lid_twist_preseat_periodic_acc_time_sec", default_value="0.2"), DeclareLaunchArgument("lid_twist_preseat_periodic_repeat", default_value="2"), DeclareLaunchArgument("lid_twist_preseat_periodic_ref", default_value="tool"), + DeclareLaunchArgument("lid_twist_preseat_periodic_descend_m", default_value="0.0"), + DeclareLaunchArgument("lid_twist_preseat_step_m", default_value="0.005"), + DeclareLaunchArgument("lid_twist_preseat_wiggle_deg", default_value="10.0"), + DeclareLaunchArgument("lid_twist_preseat_wiggle_velocity", default_value="50.0"), + DeclareLaunchArgument("lid_twist_preseat_down_velocity", default_value="8.0"), DeclareLaunchArgument("lid_twist_compliance_x_stiffness", default_value="3000.0"), DeclareLaunchArgument("lid_twist_compliance_y_stiffness", default_value="3000.0"), DeclareLaunchArgument("lid_twist_compliance_z_stiffness", default_value="300.0"), @@ -431,6 +445,38 @@ def generate_launch_description(): LaunchConfiguration("lid_pose_yaw_equivalence_deg"), value_type=float, ), + "use_j6_yaw_for_pick": ParameterValue( + LaunchConfiguration("use_j6_yaw_for_pick"), + value_type=bool, + ), + "pick_j6_yaw_axis": ParameterValue( + LaunchConfiguration("pick_j6_yaw_axis"), + value_type=str, + ), + "pick_j6_yaw_sign": ParameterValue( + LaunchConfiguration("pick_j6_yaw_sign"), + value_type=float, + ), + "pick_j6_yaw_offset_deg": ParameterValue( + LaunchConfiguration("pick_j6_yaw_offset_deg"), + value_type=float, + ), + "pick_j6_yaw_equivalence_deg": ParameterValue( + LaunchConfiguration("pick_j6_yaw_equivalence_deg"), + value_type=float, + ), + "pick_j6_yaw_tolerance_deg": ParameterValue( + LaunchConfiguration("pick_j6_yaw_tolerance_deg"), + value_type=float, + ), + "pick_j6_velocity": ParameterValue( + LaunchConfiguration("pick_j6_velocity"), + value_type=float, + ), + "pick_j6_acceleration": ParameterValue( + LaunchConfiguration("pick_j6_acceleration"), + value_type=float, + ), "line_velocity": ParameterValue( LaunchConfiguration("line_velocity"), value_type=float, @@ -643,6 +689,10 @@ def generate_launch_description(): LaunchConfiguration("lid_twist_force_release_time"), value_type=float, ), + "lid_twist_preseat_mode": ParameterValue( + LaunchConfiguration("lid_twist_preseat_mode"), + value_type=str, + ), "lid_twist_preseat_periodic_before_turn": ParameterValue( LaunchConfiguration("lid_twist_preseat_periodic_before_turn"), value_type=bool, @@ -687,6 +737,26 @@ def generate_launch_description(): LaunchConfiguration("lid_twist_preseat_periodic_ref"), value_type=str, ), + "lid_twist_preseat_periodic_descend_m": ParameterValue( + LaunchConfiguration("lid_twist_preseat_periodic_descend_m"), + value_type=float, + ), + "lid_twist_preseat_step_m": ParameterValue( + LaunchConfiguration("lid_twist_preseat_step_m"), + value_type=float, + ), + "lid_twist_preseat_wiggle_deg": ParameterValue( + LaunchConfiguration("lid_twist_preseat_wiggle_deg"), + value_type=float, + ), + "lid_twist_preseat_wiggle_velocity": ParameterValue( + LaunchConfiguration("lid_twist_preseat_wiggle_velocity"), + value_type=float, + ), + "lid_twist_preseat_down_velocity": ParameterValue( + LaunchConfiguration("lid_twist_preseat_down_velocity"), + value_type=float, + ), "lid_twist_compliance_x_stiffness": ParameterValue( LaunchConfiguration("lid_twist_compliance_x_stiffness"), value_type=float, diff --git a/src/azas_motion/azas_motion/lid_grip_planner_node.py b/src/azas_motion/azas_motion/lid_grip_planner_node.py index 5536344..56f115a 100644 --- a/src/azas_motion/azas_motion/lid_grip_planner_node.py +++ b/src/azas_motion/azas_motion/lid_grip_planner_node.py @@ -47,6 +47,7 @@ MOVE_MODE_ABSOLUTE = 0 MOVE_MODE_RELATIVE = 1 SYNC = 0 +ASYNC = 1 BLENDING_SPEED_TYPE_DUPLICATE = 0 DR_FC_MOD_REL = 1 HARDWARE_CONFIRM_PHRASE = "ENABLE_REAL_ROBOT_MOTION" @@ -106,6 +107,18 @@ def __init__(self): ) self.declare_parameter("lid_pose_yaw_offset_deg", 0.0) self.declare_parameter("lid_pose_yaw_equivalence_deg", 180.0) + self.declare_parameter("use_j6_yaw_for_pick", False) + self.declare_parameter( + "pick_j6_yaw_axis", + "y", + ParameterDescriptor(dynamic_typing=True), + ) + self.declare_parameter("pick_j6_yaw_sign", -1.0) + self.declare_parameter("pick_j6_yaw_offset_deg", 1.2) + self.declare_parameter("pick_j6_yaw_equivalence_deg", 360.0) + self.declare_parameter("pick_j6_yaw_tolerance_deg", 1.0) + self.declare_parameter("pick_j6_velocity", 30.0) + self.declare_parameter("pick_j6_acceleration", 15.0) self.declare_parameter("line_velocity", 15.0) self.declare_parameter("line_acceleration", 30.0) self.declare_parameter("move_timeout_sec", 10.0) @@ -159,6 +172,7 @@ def __init__(self): self.declare_parameter("lid_twist_regrip_gripper_wait_sec", 0.5) self.declare_parameter("lid_twist_force_settle_seconds", 0.4) self.declare_parameter("lid_twist_force_release_time", 0.2) + self.declare_parameter("lid_twist_preseat_mode", "periodic") self.declare_parameter("lid_twist_preseat_periodic_before_turn", False) self.declare_parameter("lid_twist_preseat_periodic_x_amp_mm", 0.0) self.declare_parameter("lid_twist_preseat_periodic_y_amp_mm", 0.0) @@ -170,6 +184,11 @@ def __init__(self): self.declare_parameter("lid_twist_preseat_periodic_acc_time_sec", 0.2) self.declare_parameter("lid_twist_preseat_periodic_repeat", 2) self.declare_parameter("lid_twist_preseat_periodic_ref", "tool") + self.declare_parameter("lid_twist_preseat_periodic_descend_m", 0.0) + self.declare_parameter("lid_twist_preseat_step_m", 0.005) + self.declare_parameter("lid_twist_preseat_wiggle_deg", 10.0) + self.declare_parameter("lid_twist_preseat_wiggle_velocity", 50.0) + self.declare_parameter("lid_twist_preseat_down_velocity", 8.0) self.declare_parameter("lid_twist_compliance_x_stiffness", 3000.0) self.declare_parameter("lid_twist_compliance_y_stiffness", 3000.0) self.declare_parameter("lid_twist_compliance_z_stiffness", 300.0) @@ -542,6 +561,41 @@ def _try_motion_sequence(self, source_msg: PoseStamped, plan, allow_gripper: boo return use_visual_refine = bool(self.get_parameter("visual_refine_before_grasp").value) + use_j6_pick = self._j6_yaw_pick_enabled() + if use_j6_pick: + if gripper_targets is not None: + preopen_width, _grasp_width, force_n = gripper_targets + if not self._call_gripper_sync("preopen", preopen_width, force_n): + return + sequence_steps = self._try_j6_yaw_motion_sequence( + source_msg, + plan, + gripper_targets, + use_visual_refine=use_visual_refine, + ) + if sequence_steps is None: + return + if bool(self.get_parameter("enable_lid_twist_after_grasp").value): + if not self._try_lid_twist_sequence(): + return + sequence_steps.extend( + [ + "lid_twist_transfer_high", + "lid_twist_transfer", + "lid_twist_press", + "lid_twist_turn_clockwise_steps", + "lid_twist_final_preopen", + "lid_twist_home", + ] + ) + self._publish_status( + "motion_sequence_requested", + steps=sequence_steps, + real_motion=True, + note="Doosan MoveLine/MoveJoint requests were sent through the configured services", + ) + return + initial_steps = (("approach_lid", plan.approach_pose),) full_steps = ( ("approach_lid", plan.approach_pose), @@ -628,6 +682,96 @@ def _finish_grasp_at_current_pose(self, gripper_targets) -> bool: time.sleep(float(self.get_parameter("hold_seconds_after_grasp").value)) return True + def _try_j6_yaw_motion_sequence( + self, + source_msg: PoseStamped, + plan, + gripper_targets, + *, + use_visual_refine: bool, + ) -> list[str] | None: + fixed_rz = float(self.get_parameter("rz").value) + approach_pos = self._pose_to_dsr_pos_with_rz(plan.approach_pose, fixed_rz) + grasp_pos = self._pose_to_dsr_pos_with_rz(plan.grasp_pose, fixed_rz) + lift_pos = self._pose_to_dsr_pos_with_rz(plan.lift_pose, fixed_rz) + + if not self._precheck_ikin_steps((("approach_lid", approach_pos),)): + return None + if not self._call_movel_pos( + "approach_lid", + approach_pos, + velocity=float(self.get_parameter("line_velocity").value), + acceleration=float(self.get_parameter("line_acceleration").value), + verify_orientation=False, + ): + return None + + sequence_steps = ["approach_lid"] + yaw_source_pose = source_msg.pose + current_reference_pos = approach_pos + self._last_visual_refine_stats = None + self._last_visual_refine_pose = None + + if use_visual_refine: + refined_steps = self._try_visual_refine_steps(source_msg, plan) + if refined_steps is None: + return None + refined_steps = tuple( + (label, self._target_to_fixed_rz_pos(target, fixed_rz)) + for label, target in refined_steps + ) + if refined_steps and refined_steps[0][0] == "visual_refine_align_lid": + align_label, align_pos = refined_steps[0] + if not self._precheck_ikin_steps(((align_label, align_pos),)): + return None + if not self._call_movel_pos( + align_label, + align_pos, + velocity=float(self.get_parameter("line_velocity").value), + acceleration=float(self.get_parameter("line_acceleration").value), + verify_orientation=False, + ): + return None + sequence_steps.append(align_label) + current_reference_pos = align_pos + remaining_steps = refined_steps[1:] + else: + remaining_steps = refined_steps + for label, pos in remaining_steps: + if label == "grasp_lid": + grasp_pos = pos + elif label == "lift_lid": + lift_pos = pos + if self._last_visual_refine_pose is not None: + yaw_source_pose = self._last_visual_refine_pose + + if not self._call_pick_j6_yaw_alignment(yaw_source_pose): + return None + sequence_steps.append("align_pick_j6_yaw") + + if not self._call_movel_relative_pos( + "grasp_lid", + self._relative_xyz_delta(current_reference_pos, grasp_pos), + velocity=float(self.get_parameter("line_velocity").value), + acceleration=float(self.get_parameter("line_acceleration").value), + ref=DR_BASE, + ): + return None + if not self._finish_grasp_at_current_pose(gripper_targets): + return None + sequence_steps.append("grasp_lid") + + if not self._call_movel_relative_pos( + "lift_lid", + self._relative_xyz_delta(grasp_pos, lift_pos), + velocity=float(self.get_parameter("line_velocity").value), + acceleration=float(self.get_parameter("line_acceleration").value), + ref=DR_BASE, + ): + return None + sequence_steps.append("lift_lid") + return sequence_steps + def _try_visual_refine_steps(self, source_msg: PoseStamped, fallback_plan): sample_count = max(int(self.get_parameter("visual_refine_sample_count").value), 1) timeout_sec = max(float(self.get_parameter("visual_refine_timeout_sec").value), 0.1) @@ -635,7 +779,7 @@ def _try_visual_refine_steps(self, source_msg: PoseStamped, fallback_plan): float(self.get_parameter("visual_refine_min_sample_interval_sec").value), 0.0, ) - axis = self._lid_pose_yaw_axis() + axis = self._pick_j6_yaw_axis() if self._j6_yaw_pick_enabled() else self._lid_pose_yaw_axis() self._publish_status( "visual_refine_collecting", sample_count=sample_count, @@ -706,7 +850,12 @@ def _try_visual_refine_steps(self, source_msg: PoseStamped, fallback_plan): details={"error": str(exc)}, ) - refined_rz = stats["pick_rz_deg"] if apply_yaw else self._pick_rz_deg(refined_msg.pose) + self._last_visual_refine_stats = stats + self._last_visual_refine_pose = copy.deepcopy(refined_msg.pose) + if self._j6_yaw_pick_enabled(): + refined_rz = float(self.get_parameter("rz").value) + else: + refined_rz = stats["pick_rz_deg"] if apply_yaw else self._pick_rz_deg(refined_msg.pose) align_pos = self._pose_to_dsr_pos_with_rz(refined_plan.approach_pose, refined_rz) grasp_pos = self._pose_to_dsr_pos_with_rz(refined_plan.grasp_pose, refined_rz) lift_pos = self._pose_to_dsr_pos_with_rz(refined_plan.lift_pose, refined_rz) @@ -812,7 +961,12 @@ def _visual_refine_stats(self, samples: list[tuple[PoseStamped, float]], *, axis position_std_m = math.sqrt( sum((x - mean_x) ** 2 + (y - mean_y) ** 2 for x, y in zip(xs, ys)) / len(xs) ) - period_deg = float(self.get_parameter("lid_pose_yaw_equivalence_deg").value) + if self._j6_yaw_pick_enabled(): + period_deg = float(self.get_parameter("pick_j6_yaw_equivalence_deg").value) + offset_deg = 0.0 + else: + period_deg = float(self.get_parameter("lid_pose_yaw_equivalence_deg").value) + offset_deg = float(self.get_parameter("lid_pose_yaw_offset_deg").value) mean_yaw = self._mean_equivalent_angle_deg(yaws, period_deg=period_deg) yaw_std = math.sqrt( sum( @@ -821,7 +975,6 @@ def _visual_refine_stats(self, samples: list[tuple[PoseStamped, float]], *, axis ) / len(yaws) ) - offset_deg = float(self.get_parameter("lid_pose_yaw_offset_deg").value) fixed_rz = float(self.get_parameter("rz").value) requested_rz = mean_yaw + offset_deg pick_rz = self._nearest_equivalent_angle_deg( @@ -1104,6 +1257,113 @@ def _pose_to_dsr_pos_with_rz(self, pose, rz_deg: float) -> list[float]: float(rz_deg), ] + def _target_to_fixed_rz_pos(self, target, fixed_rz: float) -> list[float]: + if isinstance(target, (list, tuple)): + pos = [float(value) for value in target] + if len(pos) >= 6: + pos[3] = float(self.get_parameter("rx").value) + pos[4] = float(self.get_parameter("ry").value) + pos[5] = float(fixed_rz) + return pos + return self._pose_to_dsr_pos_with_rz(target, fixed_rz) + + @staticmethod + def _relative_xyz_delta(from_pos_mm_deg: list[float], to_pos_mm_deg: list[float]) -> list[float]: + return [ + float(to_pos_mm_deg[0]) - float(from_pos_mm_deg[0]), + float(to_pos_mm_deg[1]) - float(from_pos_mm_deg[1]), + float(to_pos_mm_deg[2]) - float(from_pos_mm_deg[2]), + 0.0, + 0.0, + 0.0, + ] + + def _j6_yaw_pick_enabled(self) -> bool: + return bool(self.get_parameter("use_j6_yaw_for_pick").value) + + def _call_pick_j6_yaw_alignment(self, pose) -> bool: + if self._move_joint_client is None: + self._publish_status( + "failed", + error=f"MoveJoint client unavailable: {self._move_joint_service_name}", + real_motion=False, + ) + return False + if self._current_posj_client is None: + self._publish_status( + "failed", + error="GetCurrentPosj client unavailable; cannot align J6 pick yaw", + real_motion=False, + ) + return False + timeout_sec = max(float(self.get_parameter("move_timeout_sec").value), 0.0) + if not self._current_posj_client.wait_for_service(timeout_sec=timeout_sec): + self._publish_status( + "failed", + error=f"GetCurrentPosj service unavailable: {self._current_posj_service_name}", + real_motion=False, + ) + return False + current_joints = self._current_posj(timeout_sec=2.0) + if current_joints is None: + self._publish_status( + "failed", + error="current J6 read failed before pick yaw alignment", + real_motion=True, + ) + return False + + axis = self._pick_j6_yaw_axis() + yaw = None + stats = getattr(self, "_last_visual_refine_stats", None) + if isinstance(stats, dict): + yaw = stats.get("mean_yaw_y_deg" if axis == "y" else "mean_yaw_x_deg") + if yaw is None: + yaw = self._pose_axis_yaw_deg(pose, axis) + if yaw is None: + self._publish_status( + "failed", + error=f"ArUco {axis}-axis yaw is unavailable; cannot align J6 pick yaw", + real_motion=True, + ) + return False + + sign = float(self.get_parameter("pick_j6_yaw_sign").value) + offset_deg = float(self.get_parameter("pick_j6_yaw_offset_deg").value) + period_deg = float(self.get_parameter("pick_j6_yaw_equivalence_deg").value) + requested_j6 = sign * float(yaw) + offset_deg + current_j6 = float(current_joints[5]) + target_j6 = self._nearest_equivalent_angle_deg( + requested_j6, + reference_deg=current_j6, + period_deg=period_deg, + ) + delta_j6 = target_j6 - current_j6 + tolerance_deg = max(float(self.get_parameter("pick_j6_yaw_tolerance_deg").value), 0.0) + fields = { + "axis": axis, + "aruco_yaw_deg": round(float(yaw), 3), + "sign": round(sign, 3), + "offset_deg": round(offset_deg, 3), + "period_deg": round(period_deg, 3), + "current_j6_deg": round(current_j6, 3), + "requested_j6_deg": round(requested_j6, 3), + "target_j6_deg": round(target_j6, 3), + "delta_j6_deg": round(delta_j6, 3), + "tolerance_deg": round(tolerance_deg, 3), + "real_motion": True, + } + if abs(delta_j6) <= tolerance_deg: + self._publish_status("pick_j6_yaw_already_aligned", **fields) + return True + self._publish_status("pick_j6_yaw_alignment", **fields) + return self._call_movej_relative_pos( + "align_pick_j6_yaw", + [0.0, 0.0, 0.0, 0.0, 0.0, delta_j6], + velocity=float(self.get_parameter("pick_j6_velocity").value), + acceleration=float(self.get_parameter("pick_j6_acceleration").value), + ) + def _pick_rz_deg(self, pose) -> float: return self._pick_rz_selection(pose)[0] @@ -1208,6 +1468,19 @@ def _round_or_none(value, digits: int): def _lid_pose_yaw_axis(self) -> str: value = self.get_parameter("lid_pose_yaw_axis").value + return self._parse_marker_yaw_axis(value, parameter_name="lid_pose_yaw_axis", default_axis="x") + + def _pick_j6_yaw_axis(self) -> str: + value = self.get_parameter("pick_j6_yaw_axis").value + return self._parse_marker_yaw_axis(value, parameter_name="pick_j6_yaw_axis", default_axis="y") + + def _parse_marker_yaw_axis( + self, + value, + *, + parameter_name: str, + default_axis: str, + ) -> str: if isinstance(value, bool): return "y" if value else "x" axis = str(value).strip().lower() @@ -1216,9 +1489,9 @@ def _lid_pose_yaw_axis(self) -> str: if axis in {"x", "axis_x", "marker_x", "aruco_x"}: return "x" self.get_logger().warn( - f"[뚜껑픽] lid_pose_yaw_axis='{value}' 값이 유효하지 않아 x축을 사용합니다" + f"[뚜껑픽] {parameter_name}='{value}' 값이 유효하지 않아 {default_axis}축을 사용합니다" ) - return "x" + return default_axis def _precheck_ikin_steps(self, steps) -> bool: if not bool(self.get_parameter("precheck_ikin").value): @@ -1429,10 +1702,11 @@ def _try_lid_twist_periodic_j6_sequence(self) -> bool: if not self._precheck_ikin_steps(ikin_steps): return False required_clients = [ - (self._move_periodic_client, self._move_periodic_service_name), (self._move_joint_client, self._move_joint_service_name), (self._current_posj_client, self._current_posj_service_name), ] + if not self._preseat_step_wiggle_enabled(): + required_clients.append((self._move_periodic_client, self._move_periodic_service_name)) for client, name in required_clients: if client is None: self._publish_status( @@ -1441,15 +1715,16 @@ def _try_lid_twist_periodic_j6_sequence(self) -> bool: real_motion=False, ) return False - if not self._move_periodic_client.wait_for_service( - timeout_sec=max(float(self.get_parameter("move_timeout_sec").value), 0.0) - ): - self._publish_status( - "failed", - error=f"MovePeriodic service unavailable: {self._move_periodic_service_name}", - real_motion=False, - ) - return False + if not self._preseat_step_wiggle_enabled(): + if not self._move_periodic_client.wait_for_service( + timeout_sec=max(float(self.get_parameter("move_timeout_sec").value), 0.0) + ): + self._publish_status( + "failed", + error=f"MovePeriodic service unavailable: {self._move_periodic_service_name}", + real_motion=False, + ) + return False if not self._move_joint_client.wait_for_service( timeout_sec=max(float(self.get_parameter("move_timeout_sec").value), 0.0) ): @@ -1491,7 +1766,23 @@ def _try_lid_twist_periodic_j6_sequence(self) -> bool: real_motion=True, ) time.sleep(hold_before_turn) - if not self._call_lid_twist_preseat_periodic(): + if self._preseat_step_wiggle_enabled(): + target = self._lid_twist_target_pos() + if target is None: + self._recover_lid_twist_abort(release_step, acceleration) + return False + if not self._call_lid_twist_preseat_j6_step_wiggle(target, acceleration): + self._recover_lid_twist_abort(release_step, acceleration) + return False + elif self._preseat_periodic_descend_m() > 0.0: + target = self._lid_twist_target_pos() + if target is None: + self._recover_lid_twist_abort(release_step, acceleration) + return False + if not self._call_lid_twist_preseat_periodic_descend(target, acceleration): + self._recover_lid_twist_abort(release_step, acceleration) + return False + elif not self._call_lid_twist_preseat_periodic(): self._recover_lid_twist_abort(release_step, acceleration) return False for action in turn_steps: @@ -1571,7 +1862,20 @@ def _try_lid_twist_force_sequence(self) -> bool: ) time.sleep(hold_before_turn) if self._preseat_periodic_enabled(): - if not self._call_lid_twist_preseat_periodic(): + target = self._lid_twist_target_pos() + if self._preseat_step_wiggle_enabled(): + ok = target is not None and self._call_lid_twist_preseat_j6_step_wiggle( + target, + acceleration, + ) + elif self._preseat_periodic_descend_m() > 0.0: + ok = target is not None and self._call_lid_twist_preseat_periodic_descend( + target, + acceleration, + ) + else: + ok = self._call_lid_twist_preseat_periodic() + if not ok: self._recover_lid_twist_abort(release_step, acceleration) return False for action in turn_steps: @@ -1688,6 +1992,11 @@ def _lid_twist_force_plan(self, *, force_control: bool = True): float(self.get_parameter("lid_twist_transfer_clearance_m").value), 0.0, ) + preseat_descend_m = ( + self._preseat_periodic_descend_m() + if self._preseat_periodic_enabled() + else 0.0 + ) release_lift_m = max(float(self.get_parameter("lid_twist_release_lift_m").value), 0.0) min_z_m = float(self.get_parameter("lid_twist_min_z_m").value) max_z_m = float(self.get_parameter("lid_twist_max_z_m").value) @@ -1709,6 +2018,8 @@ def _lid_twist_force_plan(self, *, force_control: bool = True): target_high = target.copy() target_high[2] += transfer_clearance_m * 1000.0 + preseat_start = target.copy() + preseat_start[2] += preseat_descend_m * 1000.0 turn_velocity = float(self.get_parameter("lid_twist_turn_velocity").value) hold_after_turn = max( float(self.get_parameter("lid_twist_hold_seconds_after_turn").value), @@ -1740,6 +2051,8 @@ def _lid_twist_force_plan(self, *, force_control: bool = True): validation_steps = [] if transfer_clearance_m > 0.0: validation_steps.append(("lid_twist_transfer_high", target_high, transfer_max_z_m)) + if preseat_descend_m > 0.0: + validation_steps.append(("lid_twist_preseat_start", preseat_start, transfer_max_z_m)) validation_steps.extend([ ("lid_twist_transfer", target, max_z_m), ("lid_twist_home", release, transfer_max_z_m if transfer_clearance_m > 0.0 else max_z_m), @@ -1784,6 +2097,7 @@ def _lid_twist_force_plan(self, *, force_control: bool = True): down_force_n=round(float(self.get_parameter("lid_twist_down_force_n").value), 3), preseat_periodic=bool(self._preseat_periodic_enabled()), preseat_periodic_amp=self._preseat_periodic_amp(), + preseat_periodic_descend_m=round(preseat_descend_m, 4), preseat_periodic_period_sec=round( float(self.get_parameter("lid_twist_preseat_periodic_period_sec").value), 3, @@ -1795,7 +2109,10 @@ def _lid_twist_force_plan(self, *, force_control: bool = True): transfer_steps = [] if transfer_clearance_m > 0.0: transfer_steps.append(("lid_twist_transfer_high", target_high, transfer_velocity, True, 0.0)) - transfer_steps.append(("lid_twist_transfer", target, press_velocity, True, 0.0)) + if preseat_descend_m > 0.0: + transfer_steps.append(("lid_twist_preseat_start", preseat_start, press_velocity, True, 0.0)) + else: + transfer_steps.append(("lid_twist_transfer", target, press_velocity, True, 0.0)) release_step = ( "relative_motion", "lid_twist_home", @@ -1902,7 +2219,145 @@ def _enable_lid_twist_force(self) -> bool: ) return True - def _call_lid_twist_preseat_periodic(self) -> bool: + def _call_lid_twist_preseat_periodic_descend( + self, + target_mm_deg: list[float], + acceleration: float, + ) -> bool: + descend_m = self._preseat_periodic_descend_m() + if descend_m <= 0.0: + return self._call_lid_twist_preseat_periodic() + if not self._call_lid_twist_preseat_periodic( + label="lid_twist_preseat_periodic_async", + sync_type=ASYNC, + ): + return False + + period_sec = max( + float(self.get_parameter("lid_twist_preseat_periodic_period_sec").value), + 0.01, + ) + repeat = max(int(self.get_parameter("lid_twist_preseat_periodic_repeat").value), 1) + expected_periodic_sec = period_sec * repeat + press_velocity = float(self.get_parameter("lid_twist_press_velocity").value) + target = [float(value) for value in target_mm_deg] + self._publish_status( + "lid_twist_preseat_periodic_descend_start", + descend_m=round(descend_m, 4), + target_mm_deg=[round(value, 3) for value in target], + velocity=round(press_velocity, 3), + expected_periodic_sec=round(expected_periodic_sec, 3), + real_motion=True, + ) + started_at = time.monotonic() + if not self._call_movel_pos( + "lid_twist_preseat_descend", + target, + velocity=press_velocity, + acceleration=acceleration, + verify_orientation=True, + ): + return False + + remaining_sec = expected_periodic_sec - (time.monotonic() - started_at) + if remaining_sec > 0.0: + self._publish_status( + "lid_twist_holding", + step="lid_twist_preseat_periodic_finish", + seconds=round(remaining_sec, 3), + real_motion=True, + ) + time.sleep(remaining_sec) + return True + + def _call_lid_twist_preseat_j6_step_wiggle( + self, + target_mm_deg: list[float], + acceleration: float, + ) -> bool: + descend_m = self._preseat_periodic_descend_m() + if descend_m <= 0.0: + return True + + step_m = self._preseat_step_m() + wiggle_deg = self._preseat_wiggle_deg() + down_velocity = self._preseat_down_velocity() + wiggle_velocity = self._preseat_wiggle_velocity() + total_mm = descend_m * 1000.0 + step_mm = max(step_m * 1000.0, 0.1) + step_count = max(int(math.ceil(total_mm / step_mm)), 1) + target = [float(value) for value in target_mm_deg] + self._publish_status( + "lid_twist_preseat_step_wiggle_start", + descend_m=round(descend_m, 4), + step_m=round(step_m, 4), + step_count=step_count, + wiggle_deg=round(wiggle_deg, 3), + down_velocity=round(down_velocity, 3), + wiggle_velocity=round(wiggle_velocity, 3), + target_mm_deg=[round(value, 3) for value in target], + real_motion=True, + ) + + remaining_mm = total_mm + for index in range(1, step_count + 1): + down_mm = min(step_mm, remaining_mm) + remaining_mm -= down_mm + self._publish_status( + "lid_twist_preseat_step_wiggle_progress", + index=index, + total=step_count, + down_mm=round(down_mm, 3), + remaining_mm=round(max(remaining_mm, 0.0), 3), + wiggle_deg=round(wiggle_deg, 3), + real_motion=True, + ) + if down_mm > 1e-6 and not self._call_movel_relative_pos( + f"lid_twist_preseat_step_down_{index:02d}", + [0.0, 0.0, -down_mm, 0.0, 0.0, 0.0], + velocity=down_velocity, + acceleration=acceleration, + ref=DR_BASE, + ): + return False + if wiggle_deg <= 1e-9: + continue + if not self._call_movej_relative_pos( + f"lid_twist_preseat_wiggle_plus_{index:02d}", + [0.0, 0.0, 0.0, 0.0, 0.0, wiggle_deg], + velocity=wiggle_velocity, + acceleration=acceleration, + ): + return False + if not self._call_movej_relative_pos( + f"lid_twist_preseat_wiggle_minus_{index:02d}", + [0.0, 0.0, 0.0, 0.0, 0.0, -2.0 * wiggle_deg], + velocity=wiggle_velocity, + acceleration=acceleration, + ): + return False + if not self._call_movej_relative_pos( + f"lid_twist_preseat_wiggle_return_{index:02d}", + [0.0, 0.0, 0.0, 0.0, 0.0, wiggle_deg], + velocity=wiggle_velocity, + acceleration=acceleration, + ): + return False + + self._publish_status( + "lid_twist_preseat_step_wiggle_done", + step_count=step_count, + descend_m=round(descend_m, 4), + real_motion=True, + ) + return True + + def _call_lid_twist_preseat_periodic( + self, + *, + label: str = "lid_twist_preseat_periodic", + sync_type: int = SYNC, + ) -> bool: if self._move_periodic_client is None: self._publish_status( "failed", @@ -1938,7 +2393,7 @@ def _call_lid_twist_preseat_periodic(self) -> bool: req.acc = acc_time_sec req.repeat = repeat req.ref = self._preseat_periodic_ref() - req.sync_type = SYNC + req.sync_type = int(sync_type) self._publish_status( "lid_twist_preseat_periodic_start", amp=[round(value, 3) for value in amp], @@ -1946,12 +2401,13 @@ def _call_lid_twist_preseat_periodic(self) -> bool: acc_time_sec=round(acc_time_sec, 3), repeat=repeat, ref="tool" if req.ref == DR_TOOL else "base", + sync="async" if req.sync_type == ASYNC else "sync", real_motion=True, ) return self._call_bool_service( self._move_periodic_client, req, - "lid_twist_preseat_periodic", + label, self._move_periodic_service_name, float(self.get_parameter("move_timeout_sec").value), ) @@ -1969,6 +2425,30 @@ def _preseat_periodic_amp(self) -> list[float]: float(self.get_parameter("lid_twist_preseat_periodic_rz_amp_deg").value), ] + def _preseat_periodic_descend_m(self) -> float: + return max(float(self.get_parameter("lid_twist_preseat_periodic_descend_m").value), 0.0) + + def _preseat_mode(self) -> str: + value = str(self.get_parameter("lid_twist_preseat_mode").value).strip().lower() + if value in {"j6_step_wiggle", "step_wiggle", "j6_wiggle", "step"}: + return "j6_step_wiggle" + return "periodic" + + def _preseat_step_wiggle_enabled(self) -> bool: + return self._preseat_mode() == "j6_step_wiggle" + + def _preseat_step_m(self) -> float: + return max(float(self.get_parameter("lid_twist_preseat_step_m").value), 0.0005) + + def _preseat_wiggle_deg(self) -> float: + return abs(float(self.get_parameter("lid_twist_preseat_wiggle_deg").value)) + + def _preseat_wiggle_velocity(self) -> float: + return max(float(self.get_parameter("lid_twist_preseat_wiggle_velocity").value), 0.1) + + def _preseat_down_velocity(self) -> float: + return max(float(self.get_parameter("lid_twist_preseat_down_velocity").value), 0.1) + def _preseat_periodic_ref(self) -> int: value = str(self.get_parameter("lid_twist_preseat_periodic_ref").value).strip().lower() if value == "base": @@ -2065,9 +2545,9 @@ def _force_twist_clients_available(self) -> bool: (self._release_compliance_client, self._release_compliance_service_name), ] rotation_mode = self._lid_twist_force_rotation_mode() - if self._preseat_periodic_enabled(): + if self._preseat_periodic_enabled() and not self._preseat_step_wiggle_enabled(): required.append((self._move_periodic_client, self._move_periodic_service_name)) - if rotation_mode == "j6": + if rotation_mode == "j6" or self._preseat_step_wiggle_enabled(): required.extend([ (self._move_joint_client, self._move_joint_service_name), (self._current_posj_client, self._current_posj_service_name), @@ -2640,6 +3120,23 @@ def _human_status_text(self, status: str, fields: dict) -> str: f"xy_std={fields.get('position_std_m')}m " f"yaw_std={fields.get('yaw_std_deg')}deg" ) + if status == "pick_j6_yaw_alignment": + return ( + "[뚜껑픽] J6 파지 방향 정렬 시작: " + f"axis={fields.get('axis')} yaw={fields.get('aruco_yaw_deg')}deg " + f"current_j6={fields.get('current_j6_deg')}deg " + f"target_j6={fields.get('target_j6_deg')}deg " + f"delta={fields.get('delta_j6_deg')}deg " + f"formula=({fields.get('sign')}*yaw + {fields.get('offset_deg')})" + ) + if status == "pick_j6_yaw_already_aligned": + return ( + "[뚜껑픽] J6 파지 방향 정렬 생략: " + f"axis={fields.get('axis')} yaw={fields.get('aruco_yaw_deg')}deg " + f"current_j6={fields.get('current_j6_deg')}deg " + f"target_j6={fields.get('target_j6_deg')}deg " + f"delta={fields.get('delta_j6_deg')}deg" + ) if status == "gripper_grasp_continue_after_failure": return ( "[뚜껑픽] 그리퍼 닫기 확인 실패, 대기 후 lift 계속 진행: " @@ -2683,15 +3180,46 @@ def _human_status_text(self, status: str, fields: dict) -> str: return ( "[뚜껑회전] pre-seat periodic 비틀림 안착 시작: " f"amp={fields.get('amp')} period={fields.get('period_sec')}초 " - f"repeat={fields.get('repeat')} ref={fields.get('ref')}" + f"repeat={fields.get('repeat')} ref={fields.get('ref')} sync={fields.get('sync')}" + ) + if status == "lid_twist_preseat_periodic_descend_start": + return ( + "[뚜껑회전] periodic 비틀림 하강 시작: " + f"descend={fields.get('descend_m')} m " + f"target_mm={self._fmt_target_mm(fields.get('target_mm_deg', []))} " + f"vel={fields.get('velocity')} mm/s " + f"periodic_duration={fields.get('expected_periodic_sec')}초" + ) + if status == "lid_twist_preseat_step_wiggle_start": + return ( + "[뚜껑회전] J6 단계 흔들기 하강 시작: " + f"descend={fields.get('descend_m')} m " + f"step={fields.get('step_m')} m x {fields.get('step_count')}회 " + f"wiggle=±{fields.get('wiggle_deg')}deg " + f"down_vel={fields.get('down_velocity')} mm/s " + f"j6_vel={fields.get('wiggle_velocity')} deg/s" + ) + if status == "lid_twist_preseat_step_wiggle_progress": + return ( + "[뚜껑회전] J6 단계 흔들기 진행: " + f"{fields.get('index')}/{fields.get('total')} " + f"하강={fields.get('down_mm')} mm " + f"남은거리={fields.get('remaining_mm')} mm " + f"wiggle=±{fields.get('wiggle_deg')}deg" + ) + if status == "lid_twist_preseat_step_wiggle_done": + return ( + "[뚜껑회전] J6 단계 흔들기 하강 완료: " + f"총 {fields.get('step_count')}단계 descend={fields.get('descend_m')} m" ) if status == "lid_twist_joint_relative_start": before = fields.get("before_joints_deg") before_j6 = before[5] if isinstance(before, list) and len(before) >= 6 else None delta = fields.get("delta_joints_deg") delta_j6 = delta[5] if isinstance(delta, list) and len(delta) >= 6 else None + prefix = "[뚜껑픽]" if str(fields.get("step", "")) == "align_pick_j6_yaw" else "[뚜껑회전]" return ( - f"[뚜껑회전] J6 상대 회전 시작: {self._step_ko(str(fields.get('step', '')))} " + f"{prefix} J6 상대 회전 시작: {self._step_ko(str(fields.get('step', '')))} " f"before_j6={before_j6}deg delta_j6={delta_j6}deg " f"vel={fields.get('velocity')} acc={fields.get('acceleration')}" ) @@ -2700,8 +3228,9 @@ def _human_status_text(self, status: str, fields: dict) -> str: after_j6 = after[5] if isinstance(after, list) and len(after) >= 6 else None delta = fields.get("delta_joints_deg") delta_j6 = delta[5] if isinstance(delta, list) and len(delta) >= 6 else None + prefix = "[뚜껑픽]" if str(fields.get("step", "")) == "align_pick_j6_yaw" else "[뚜껑회전]" return ( - f"[뚜껑회전] J6 상대 회전 완료: {self._step_ko(str(fields.get('step', '')))} " + f"{prefix} J6 상대 회전 완료: {self._step_ko(str(fields.get('step', '')))} " f"after_j6={after_j6}deg delta_j6={delta_j6}deg" ) if status == "lid_twist_force_released": @@ -2751,6 +3280,14 @@ def _human_status_text(self, status: str, fields: dict) -> str: @staticmethod def _step_ko(label: str) -> str: + if label.startswith("lid_twist_preseat_step_down_"): + return label.replace("lid_twist_preseat_step_down_", "단계 하강 ") + if label.startswith("lid_twist_preseat_wiggle_plus_"): + return label.replace("lid_twist_preseat_wiggle_plus_", "J6 오른쪽 흔들기 ") + if label.startswith("lid_twist_preseat_wiggle_minus_"): + return label.replace("lid_twist_preseat_wiggle_minus_", "J6 왼쪽 흔들기 ") + if label.startswith("lid_twist_preseat_wiggle_return_"): + return label.replace("lid_twist_preseat_wiggle_return_", "J6 중앙 복귀 ") if label.startswith("lid_twist_turn_clockwise_") and "_ikin" not in label: return label.replace("lid_twist_turn_clockwise_", "시계방향 회전 ") if label.startswith("lid_twist_turn_counterclockwise_") and "_ikin" not in label: @@ -2782,12 +3319,16 @@ def _step_ko(label: str) -> str: "lid_twist_release": "8단계 압력 해제 상승", "lid_twist_home": "8단계 그리퍼 open 후 안전 위치 이동", "visual_refine_align_lid": "1.5단계 ArUco 보정 위치 정렬", + "align_pick_j6_yaw": "1.6단계 J6 파지 방향 정렬", "approach_lid_ikin": "접근 위치 IK", "visual_refine_align_lid_ikin": "ArUco 보정 위치 IK", "grasp_lid_ikin": "파지 위치 IK", "lift_lid_ikin": "상승 위치 IK", "lid_twist_transfer_high_ikin": "고정 위치 위 안전 이동 IK", "lid_twist_transfer_ikin": "회전 위치 IK", + "lid_twist_preseat_start": "periodic 하강 시작 위치", + "lid_twist_preseat_start_ikin": "periodic 하강 시작 위치 IK", + "lid_twist_preseat_descend": "periodic 비틀림 하강", "lid_twist_press_ikin": "누르기 위치 IK", "lid_twist_turn_clockwise_ikin": "시계방향 회전 IK", "lid_twist_release_ikin": "압력 해제 위치 IK", @@ -2797,6 +3338,8 @@ def _step_ko(label: str) -> str: "lid_twist_force_settle": "힘 안정화", "lid_twist_periodic_settle": "periodic 비틀림 전 안정화", "lid_twist_preseat_periodic": "pre-seat periodic 비틀림 안착", + "lid_twist_preseat_periodic_async": "pre-seat periodic 비동기 시작", + "lid_twist_preseat_periodic_finish": "pre-seat periodic 잔여 동작 대기", "lid_twist_turn_complete": "TCP 기준 상대 회전 완료", "lid_twist_release_force": "목표 힘 해제", "lid_twist_release_compliance": "컴플라이언스 해제", From a274d0a948f136ea69cbdbebe6fd7908f5b8e291 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Wed, 10 Jun 2026 15:13:33 +0900 Subject: [PATCH 40/88] Add script to stop all Azas field stack processes and clean up shared memory This commit introduces a new script, `stop_azas_all.sh`, which provides a comprehensive way to stop all processes related to the Azas field stack. The script handles the graceful termination of tmux sessions, ROS-related processes, and cleans up stale FastDDS shared memory segments. It includes options for dry runs and conditional stopping of the control panel. The script ensures that critical processes are protected from termination. --- contact | 0 docs/robot_pipeline_control.html | 2 +- pressed_z | 0 .../collision_scene_rviz_publisher.py | 380 ++++++++ .../rule_motion_joint_preview_node.py | 107 +++ src/azas_bringup/config/calibration.yaml | 44 +- .../calibration.yaml.bak-20260610-111504 | 157 ++++ .../calibration.yaml.bak-20260610-111645 | 157 ++++ .../calibration.yaml.bak-20260610-111836 | 157 ++++ .../calibration.yaml.bak-20260610-112001 | 157 ++++ .../config/measured_dispenser_collision.yaml | 64 +- ...spenser_collision.yaml.bak-20260610-110911 | 157 ++++ ...spenser_collision.yaml.bak-20260610-110944 | 157 ++++ ...spenser_collision.yaml.bak-20260610-111009 | 157 ++++ ...spenser_collision.yaml.bak-20260610-111027 | 157 ++++ .../launch/hardware_free_demo.launch.py | 134 +++ .../lid_sticker_grip_planning.launch.py | 5 + .../rviz/azas_cocktail_collision_preview.rviz | 10 + .../rviz/azas_collision_only.rviz | 72 ++ .../rviz/azas_dispenser_sequence_clean.rviz | 45 +- .../rviz/azas_rule_motion_preview.rviz | 148 ++++ src/azas_bringup/setup.py | 2 + .../azas_cup_uprighting/_base_node.py | 8 +- .../yolo_cup_uprighting_node.py | 2 +- .../launch/yolo_cup_uprighting.launch.py | 7 + .../dispenser_sequence_preview_node.py | 172 +++- src/azas_motion/azas_motion/lid_grip.py | 6 + .../azas_motion/lid_grip_planner_node.py | 11 +- .../azas_motion/side_grasp_ik_preview_node.py | 21 +- src/azas_motion/test/test_alignment.py | 31 + .../dsr_practice/yolo_cup_pick_node.py | 125 ++- ...lish_color_recipe_sequence_rviz_preview.py | 394 +++++++++ tools/run/record_dispenser_press_pose.py | 173 ++++ tools/run/robot_pipeline_control_server.py | 235 ++++- tools/run/run_changhyun_side_grip_direct.sh | 5 +- tools/run/run_color_recipe_sequence.py | 150 +++- .../run_course_dispenser_press_cycle_rviz.sh | 53 +- tools/run/run_kang_lid_grip_close_direct.sh | 9 +- .../run_measured_dispenser_recipe_sequence.py | 785 +++++++++++++++-- tools/run/run_minimal_dispenser_cycle.py | 834 ++++++++++++++++++ .../run/run_somyeong_cup_uprighting_direct.sh | 4 +- tools/run/start_azas_tmux_stack.sh | 37 +- tools/run/stop_azas_all.sh | 115 +++ 43 files changed, 5131 insertions(+), 315 deletions(-) create mode 100644 contact create mode 100644 pressed_z create mode 100644 src/azas_bringup/azas_bringup/collision_scene_rviz_publisher.py create mode 100644 src/azas_bringup/azas_bringup/rule_motion_joint_preview_node.py create mode 100644 src/azas_bringup/config/calibration.yaml.bak-20260610-111504 create mode 100644 src/azas_bringup/config/calibration.yaml.bak-20260610-111645 create mode 100644 src/azas_bringup/config/calibration.yaml.bak-20260610-111836 create mode 100644 src/azas_bringup/config/calibration.yaml.bak-20260610-112001 create mode 100644 src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-110911 create mode 100644 src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-110944 create mode 100644 src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-111009 create mode 100644 src/azas_bringup/config/measured_dispenser_collision.yaml.bak-20260610-111027 create mode 100644 src/azas_bringup/rviz/azas_collision_only.rviz create mode 100644 src/azas_bringup/rviz/azas_rule_motion_preview.rviz create mode 100644 tools/run/publish_color_recipe_sequence_rviz_preview.py create mode 100644 tools/run/record_dispenser_press_pose.py create mode 100755 tools/run/run_minimal_dispenser_cycle.py create mode 100755 tools/run/stop_azas_all.sh diff --git a/contact b/contact new file mode 100644 index 0000000..e69de29 diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index cb3b5e4..a43f1b3 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -1094,7 +1094,7 @@

RealSense 카메라 화면

diff --git a/tools/run/handover_cup_to_palm.py b/tools/run/handover_cup_to_palm.py new file mode 100755 index 0000000..ebcc66f --- /dev/null +++ b/tools/run/handover_cup_to_palm.py @@ -0,0 +1,368 @@ +#!/usr/bin/env python3 +"""Pattern-A human handover: place the side-gripped cup onto an open palm. + +This is an HRI motion (the robot moves toward a person). It follows +docs/post_shake_human_handover_plan.md with every gate kept explicit: + + 1. PERCEPTION sample /azas/human_hand_detection (run the detector first: + bash tools/run/run_human_hand_detection.sh) and transform the + palm into base frame via live TF base_link->link_6 and the + measured T_gripper2camera hand-eye calibration. + 2. PLAN compute LIFT -> ABOVE_HIGH -> ABOVE_PALM -> staged descent + -> RELEASE -> RETREAT, all with the CURRENT side-grip + orientation preserved (--use-current-rpy on every MoveLine). + 3. GATES default is dry-run. --execute needs --confirm, a typed + operator approval before any motion, a hand re-check right + before the descent, force-monitored descent steps, and a + second typed approval before the gripper opens. + +Every Cartesian move is delegated to tools/run/direct_movel_xyz.py, which +enforces workspace bounds, IK precheck, and target verification on its own. + +First-run advice: validate with a foam block or an empty palm-height surface +before any person, and tune --release-tcp-above-palm-m from that test. + +Usage: + python3 tools/run/handover_cup_to_palm.py # dry-run plan + python3 tools/run/handover_cup_to_palm.py --execute --confirm ENABLE_HUMAN_PALM_HANDOVER +""" +from __future__ import annotations + +import argparse +import math +import os +import subprocess +import sys +import time +from pathlib import Path + +import numpy as np + +ROOT = Path(__file__).resolve().parents[2] +DIRECT_MOVEL = ROOT / "tools" / "run" / "direct_movel_xyz.py" +RG2_OPEN = ROOT / "tools" / "run" / "rg2_full_open_verify.sh" +DEFAULT_HAND_EYE = ROOT / "src" / "azas_perception" / "config" / "T_gripper2camera.npy" +HAND_TOPIC = "/azas/human_hand_detection" +CONFIRM_PHRASE = "ENABLE_HUMAN_PALM_HANDOVER" +MOTION_APPROVAL_PHRASE = "ENABLE_HUMAN_PALM_HANDOVER_MOTION" +RELEASE_APPROVAL_PHRASE = "RELEASE_CUP_NOW" +DIRECT_CONFIRM_PHRASE = "ENABLE_DIRECT_MOVEL" + + +class HandoverPerception: + """rclpy helpers: palm sampling, live TCP pose, tool force. No motion.""" + + def __init__(self, args: argparse.Namespace) -> None: + import rclpy + import tf2_ros + from dsr_msgs2.srv import GetCurrentPosx, GetToolForce + from geometry_msgs.msg import PointStamped + + self.args = args + self.rclpy = rclpy + rclpy.init(args=None) + self.node = rclpy.create_node("azas_handover_cup_to_palm") + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + prefix = args.service_prefix + self.get_posx = self.node.create_client(GetCurrentPosx, f"/{prefix}/aux_control/get_current_posx") + self.get_tool_force = self.node.create_client(GetToolForce, f"/{prefix}/aux_control/get_tool_force") + self.hand_points: list[tuple[float, list[float]]] = [] + self.node.create_subscription(PointStamped, HAND_TOPIC, self._on_hand, 10) + self.gripper2cam = np.load(str(args.hand_eye_npy)).astype(float) + if abs(self.gripper2cam[:3, 3]).max() > 10.0: + self.gripper2cam[:3, 3] /= 1000.0 + + def close(self) -> None: + self.node.destroy_node() + if self.rclpy.ok(): + self.rclpy.shutdown() + + def _on_hand(self, msg) -> None: + self.hand_points.append((time.monotonic(), [msg.point.x, msg.point.y, msg.point.z])) + + def _call(self, client, request, *, label: str, retries: int = 2): + if not client.wait_for_service(timeout_sec=self.args.wait_service_sec): + raise RuntimeError(f"{label} service unavailable") + # Doosan aux services can time out on the first cold call; retry once. + for attempt in range(1, retries + 1): + future = client.call_async(request) + self.rclpy.spin_until_future_complete(self.node, future, timeout_sec=self.args.wait_service_sec) + response = future.result() + if response is not None: + return response + print(f"[Azas] {label} attempt {attempt}/{retries} timed out; retrying", file=sys.stderr) + raise RuntimeError(f"{label} timed out after {retries} attempts") + + def current_posx(self) -> list[float]: + from dsr_msgs2.srv import GetCurrentPosx + + req = GetCurrentPosx.Request() + req.ref = 0 # DR_BASE + response = self._call(self.get_posx, req, label="GetCurrentPosx") + if not response.success or not response.task_pos_info: + raise RuntimeError("GetCurrentPosx returned success=false") + return [float(v) for v in list(response.task_pos_info[0].data)[:6]] + + def tool_force_n(self) -> list[float]: + from dsr_msgs2.srv import GetToolForce + + req = GetToolForce.Request() + req.ref = 0 + response = self._call(self.get_tool_force, req, label="GetToolForce") + if not response.success: + raise RuntimeError("GetToolForce returned success=false") + return [float(v) for v in list(response.tool_force)[:3]] + + def base_to_camera(self) -> np.ndarray: + import rclpy.time + + deadline = time.monotonic() + self.args.wait_service_sec + last_error = "" + while time.monotonic() < deadline: + self.rclpy.spin_once(self.node, timeout_sec=0.05) + try: + t = self.tf_buffer.lookup_transform("base_link", "link_6", rclpy.time.Time()) + break + except Exception as exc: # tf2 exception types vary by install + last_error = str(exc) + else: + raise RuntimeError(f"TF base_link->link_6 unavailable: {last_error}") + q = t.transform.rotation + tr = t.transform.translation + xx, yy, zz, ww = q.x, q.y, q.z, q.w + rot = np.array( + [ + [1 - 2 * (yy * yy + zz * zz), 2 * (xx * yy - zz * ww), 2 * (xx * zz + yy * ww)], + [2 * (xx * yy + zz * ww), 1 - 2 * (xx * xx + zz * zz), 2 * (yy * zz - xx * ww)], + [2 * (xx * zz - yy * ww), 2 * (yy * zz + xx * ww), 1 - 2 * (xx * xx + yy * yy)], + ] + ) + base2ee = np.eye(4) + base2ee[:3, :3] = rot + base2ee[:3, 3] = [tr.x, tr.y, tr.z] + return base2ee @ self.gripper2cam + + def sample_palm_base(self, *, label: str) -> list[float]: + """Collect stable hand detections and return the palm in base frame (m).""" + if self.args.test_hand_xyz_m: + xyz = [float(v) for v in self.args.test_hand_xyz_m.split(",")] + print(f"[Azas] {label}: TEST palm injected at base xyz={xyz} (no camera sample)") + return xyz + self.hand_points.clear() + deadline = time.monotonic() + self.args.hand_sample_timeout_sec + while time.monotonic() < deadline and len(self.hand_points) < self.args.hand_sample_count: + self.rclpy.spin_once(self.node, timeout_sec=0.1) + if len(self.hand_points) < self.args.hand_sample_count: + raise RuntimeError( + f"{label}: only {len(self.hand_points)}/{self.args.hand_sample_count} stable hand " + f"detections within {self.args.hand_sample_timeout_sec:.1f}s; is " + "run_human_hand_detection.sh running and the palm open and steady?" + ) + base2cam = self.base_to_camera() + base_points = [] + for _, cam_xyz in self.hand_points[-self.args.hand_sample_count:]: + base_points.append((base2cam @ np.array([*cam_xyz, 1.0]))[:3]) + base_points = np.array(base_points) + spread = float(np.max(np.linalg.norm(base_points - base_points.mean(axis=0), axis=1))) + palm = base_points.mean(axis=0).tolist() + print( + f"[Azas] {label}: palm_base_m=[{palm[0]:.3f}, {palm[1]:.3f}, {palm[2]:.3f}] " + f"samples={len(base_points)} spread={spread * 1000.0:.1f}mm" + ) + if spread > self.args.hand_sample_spread_max_m: + raise RuntimeError( + f"{label}: palm samples spread {spread * 1000.0:.1f}mm exceeds " + f"{self.args.hand_sample_spread_max_m * 1000.0:.1f}mm; hand or robot is moving" + ) + return palm + + +def run_movel(args: argparse.Namespace, xyz_m: list[float], *, label: str, velocity: float, acceleration: float) -> None: + cmd = [ + sys.executable, str(DIRECT_MOVEL), + "--service-prefix", args.service_prefix, + "--x", f"{xyz_m[0]:.6f}", "--y", f"{xyz_m[1]:.6f}", "--z", f"{xyz_m[2]:.6f}", + "--use-current-rpy", + "--velocity", f"{velocity:.3f}", + "--acceleration", f"{acceleration:.3f}", + "--timeout-sec", f"{args.move_timeout_sec:.1f}", + "--wait-service-sec", f"{args.wait_service_sec:.1f}", + "--x-min", f"{args.x_min:.3f}", "--x-max", f"{args.x_max:.3f}", + "--y-min", f"{args.y_min:.3f}", "--y-max", f"{args.y_max:.3f}", + "--z-min", f"{args.z_min:.3f}", "--z-max", f"{args.z_max:.3f}", + ] + if args.execute: + cmd += ["--precheck-ikin", "--verify-target", "--execute", "--confirm", DIRECT_CONFIRM_PHRASE] + print(f"[Azas] MOVE {label}: xyz_m=[{xyz_m[0]:.3f}, {xyz_m[1]:.3f}, {xyz_m[2]:.3f}] vel={velocity:.1f}") + rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode + if rc != 0: + raise RuntimeError(f"MoveLine step failed: {label} (rc={rc})") + + +def open_gripper(args: argparse.Namespace) -> None: + env = os.environ.copy() + env.setdefault("RG2_OPEN_TIMEOUT_SEC", "20.0") + rc = subprocess.run([str(RG2_OPEN)], cwd=str(ROOT), env=env, check=False).returncode + if rc != 0: + raise RuntimeError(f"RG2 open failed (rc={rc})") + + +def require_typed_approval(phrase: str, *, prompt: str) -> None: + print(prompt) + entered = input(f"Type {phrase} to continue: ").strip() + if entered != phrase: + raise RuntimeError(f"operator approval mismatch; expected {phrase}") + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--hand-eye-npy", type=Path, default=DEFAULT_HAND_EYE) + parser.add_argument("--hand-sample-count", type=int, default=10) + parser.add_argument("--hand-sample-timeout-sec", type=float, default=20.0) + parser.add_argument("--hand-sample-spread-max-m", type=float, default=0.03) + parser.add_argument("--hand-recheck-tolerance-m", type=float, default=0.05, + help="abort if the palm moved more than this between plan and descent") + parser.add_argument("--transit-z-m", type=float, default=0.45) + parser.add_argument("--above-palm-m", type=float, default=0.12, + help="TCP height above the palm before the staged descent") + parser.add_argument("--release-tcp-above-palm-m", type=float, default=0.08, + help="TCP height above the palm at release. TUNE WITH A FOAM-BLOCK " + "DRY TEST FIRST: depends on where the side grip holds the cup") + parser.add_argument("--descent-step-m", type=float, default=0.02) + parser.add_argument("--force-abort-delta-n", type=float, default=10.0, + help="abort descent when |tool force| rises this much over the pre-descent baseline") + parser.add_argument("--retreat-lift-m", type=float, default=0.20) + parser.add_argument("--transit-velocity", type=float, default=10.0) + parser.add_argument("--transit-acceleration", type=float, default=14.0) + parser.add_argument("--descent-velocity", type=float, default=4.0) + parser.add_argument("--descent-acceleration", type=float, default=6.0) + # Palm workspace bounds (base frame). The palm itself must be inside these. + parser.add_argument("--x-min", type=float, default=0.25) + parser.add_argument("--x-max", type=float, default=0.75) + parser.add_argument("--y-min", type=float, default=-0.45) + parser.add_argument("--y-max", type=float, default=0.45) + parser.add_argument("--z-min", type=float, default=0.05) + parser.add_argument("--z-max", type=float, default=0.60) + parser.add_argument("--palm-z-max-m", type=float, default=0.40, + help="reject palms higher than this (likely a mis-detection)") + parser.add_argument("--move-timeout-sec", type=float, default=60.0) + parser.add_argument("--wait-service-sec", type=float, default=10.0) + parser.add_argument("--auto-release", action="store_true", + help="skip the final typed release approval (NOT recommended)") + parser.add_argument("--test-hand-xyz-m", default="", + help="debug: skip camera sampling and use this base-frame palm 'x,y,z' (meters)") + parser.add_argument("--execute", action="store_true") + parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} with --execute") + return parser.parse_args() + + +def main() -> int: + args = parse_args() + if args.execute and args.confirm != CONFIRM_PHRASE: + print(f"[BLOCKED] --execute requires --confirm {CONFIRM_PHRASE}") + return 2 + if not args.hand_eye_npy.is_file(): + print(f"[FAIL] hand-eye calibration not found: {args.hand_eye_npy}") + return 2 + if args.release_tcp_above_palm_m >= args.above_palm_m: + print("[BLOCKED] --release-tcp-above-palm-m must be below --above-palm-m") + return 2 + if not args.execute: + print("[DRY-RUN] --execute not set; perception + plan only, no robot command sent.") + + perception = HandoverPerception(args) + try: + # --- PERCEPTION + PLAN (no motion) --- + current = perception.current_posx() + current_m = [v / 1000.0 for v in current[:3]] + print( + f"[Azas] current TCP: xyz_m=[{current_m[0]:.3f}, {current_m[1]:.3f}, {current_m[2]:.3f}] " + f"rpy_deg=[{current[3]:.1f}, {current[4]:.1f}, {current[5]:.1f}] (orientation is preserved)" + ) + palm = perception.sample_palm_base(label="palm plan sample") + if not (args.x_min <= palm[0] <= args.x_max and args.y_min <= palm[1] <= args.y_max + and args.z_min <= palm[2] <= min(args.z_max, args.palm_z_max_m)): + print(f"[BLOCKED] palm outside handover workspace bounds; refusing: palm={palm}") + return 1 + + lift = [current_m[0], current_m[1], max(current_m[2], args.transit_z_m)] + above_high = [palm[0], palm[1], max(args.transit_z_m, palm[2] + args.above_palm_m)] + above_palm = [palm[0], palm[1], palm[2] + args.above_palm_m] + release = [palm[0], palm[1], palm[2] + args.release_tcp_above_palm_m] + retreat = [palm[0], palm[1], palm[2] + args.retreat_lift_m] + for name, pose in (("LIFT", lift), ("ABOVE_HIGH", above_high), ("ABOVE_PALM", above_palm), + ("RELEASE", release), ("RETREAT", retreat)): + print(f"[PLAN] {name}: xyz_m=[{pose[0]:.3f}, {pose[1]:.3f}, {pose[2]:.3f}]") + print( + "[PLAN] descent ABOVE_PALM -> RELEASE in " + f"{math.ceil((above_palm[2] - release[2]) / max(args.descent_step_m, 0.005))} steps of " + f"{args.descent_step_m * 1000.0:.0f}mm with force abort delta {args.force_abort_delta_n:.1f}N" + ) + if not args.execute: + return 0 + + # --- GATED EXECUTION --- + require_typed_approval( + MOTION_APPROVAL_PHRASE, + prompt=( + "[Azas] HRI MOTION APPROVAL REQUIRED. Confirm ALL:\n" + " - e-stop within reach\n" + " - only the receiving person is near the robot, arm steady, palm open\n" + " - first run was validated on a foam block, not a person\n" + " - speeds/bounds above were reviewed" + ), + ) + run_movel(args, lift, label="LIFT to transit height (Z-only)", + velocity=args.transit_velocity, acceleration=args.transit_acceleration) + run_movel(args, above_high, label="ABOVE_HIGH over palm at transit height", + velocity=args.transit_velocity, acceleration=args.transit_acceleration) + run_movel(args, above_palm, label="ABOVE_PALM vertical pre-descent", + velocity=args.descent_velocity, acceleration=args.descent_acceleration) + + # Hand must still be where we planned; people move. + recheck = perception.sample_palm_base(label="palm re-check before descent") + moved = math.dist(recheck, palm) + if moved > args.hand_recheck_tolerance_m: + print(f"[ABORT] palm moved {moved * 1000.0:.0f}mm since planning; retreating without descent") + run_movel(args, retreat, label="RETREAT after palm moved", + velocity=args.transit_velocity, acceleration=args.transit_acceleration) + return 1 + + baseline = perception.tool_force_n() + baseline_mag = math.sqrt(sum(v * v for v in baseline)) + z = above_palm[2] + while z > release[2] + 1e-6: + z = max(z - max(args.descent_step_m, 0.005), release[2]) + run_movel(args, [palm[0], palm[1], z], label=f"descent step to z={z:.3f}m", + velocity=args.descent_velocity, acceleration=args.descent_acceleration) + force = perception.tool_force_n() + force_mag = math.sqrt(sum(v * v for v in force)) + print(f"[Azas] tool force {force_mag:.1f}N (baseline {baseline_mag:.1f}N)") + if force_mag - baseline_mag > args.force_abort_delta_n: + print("[ABORT] force spike during descent (palm contact or obstruction); retreating with cup") + run_movel(args, retreat, label="RETREAT after force abort", + velocity=args.transit_velocity, acceleration=args.transit_acceleration) + return 1 + + if not args.auto_release: + require_typed_approval( + RELEASE_APPROVAL_PHRASE, + prompt="[Azas] Cup is at release height. Confirm the palm is directly under the cup.", + ) + open_gripper(args) + time.sleep(1.0) + run_movel(args, retreat, label="RETREAT vertical after release", + velocity=args.transit_velocity, acceleration=args.transit_acceleration) + print("[PASS] palm handover sequence completed") + return 0 + except RuntimeError as exc: + print(f"[FAIL] {exc}") + return 1 + finally: + perception.close() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index fc7aef4..21c6825 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -914,6 +914,32 @@ class Step: "lid_grip_close", "shake_rviz_preview", } +PANEL_HIDDEN_STEP_KEYS = { + "rviz_cocktail_collision_preview", + "rviz_color_scan_pose_preview", + "shake_rviz_preview", + "stop_cocktail_motion_preview", + "check_one_click_cocktail_ready", + "check_one_click_cocktail_result", + "run_cocktail_now_real", + "start_camera_view", + "detect_cup_lid", + "voice_input", + "listen_stt_recipe", + "run_one_click_cocktail_real", + "move_to_dispenser_1", + "move_to_dispenser_2", + "move_to_dispenser_3", + "move_to_dispenser_4", + "press_dispenser_1", + "press_dispenser_2", + "press_dispenser_3", + "press_dispenser_4", + "pick_from_dispenser_1", + "pick_from_dispenser_2", + "pick_from_dispenser_3", + "pick_from_dispenser_4", +} PANEL_DIRECT_TMUX_STEPS = { # Match the successful field workflow: the panel opens the same tmux launch # command and does not pre-block on slow ROS graph/service introspection. @@ -1008,12 +1034,16 @@ class Step: ) SIDE_GRIP_STACK_PATTERNS = ( + "run_changhyun_side_grip_direct.sh", "yolo_cup_pick_node.launch.py", "yolo_cup_pick_node_legacy.launch.py", "yolo_cup_pick_legacy_node", "dsr_practice/yolo_cup_pick_node", "yolo_cup_pick_node --ros-args", "yolo_cup_pick_moveit_py", + "hand_eye_static_tf_node", + "link6_gripper_collision_node", + "--frame-id world --child-frame-id base_link", ) CUP_UPRIGHTING_STACK_PATTERNS = ( @@ -2234,6 +2264,37 @@ def wait_for_camera_topic_samples( ) +def realsense_usb_visible() -> tuple[bool, str]: + """Return whether an Intel RealSense device is visible to the OS.""" + try: + result = subprocess.run( + ["lsusb"], + cwd=str(ROOT), + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + timeout=3.0, + check=False, + ) + except (OSError, subprocess.TimeoutExpired) as exc: + return False, f"[FAIL] lsusb check failed: {exc}" + output = result.stdout.strip() + if result.returncode != 0: + return False, "[FAIL] lsusb returned non-zero\n" + output + visible = any( + ("Intel" in line and "RealSense" in line) + or "8086:0b" in line.lower() + for line in output.splitlines() + ) + if visible: + return True, "[OK] RealSense USB device visible\n" + output + return ( + False, + "[FAIL] RealSense USB device is not visible to lsusb. " + "카메라 ROS 재시작으로는 복구되지 않습니다.\n" + output, + ) + + def wait_for_tf_transform( *, env: dict[str, str], @@ -2532,6 +2593,12 @@ def side_grip_preflight(env: dict[str, str], service_prefix: str) -> tuple[bool, + ", ".join(str(path) for path in calibration_candidates) ) + usb_ok, usb_output = realsense_usb_visible() + checks.append("--- RealSense USB ---\n" + usb_output) + if not usb_ok: + ok = False + return ok, "\n".join(checks) + camera_ready, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=5.0) if not camera_ready: # 카메라가 depth 없이 켜져 있을 수 있음 → 자동 재시작 @@ -2602,6 +2669,12 @@ def cup_uprighting_preflight(env: dict[str, str], service_prefix: str) -> tuple[ ok = False checks.append(f"[FAIL] cup_uprighting YOLO model missing: {CUP_UPRIGHTING_YOLO_MODEL_PATH}") + usb_ok, usb_output = realsense_usb_visible() + checks.append("--- RealSense USB ---\n" + usb_output) + if not usb_ok: + ok = False + return ok, "\n".join(checks) + camera_ready, camera_output = wait_for_camera_topic_samples(env=env, timeout_sec=5.0) checks.append("--- camera topics ---\n" + camera_output) if not camera_ready: @@ -3127,7 +3200,7 @@ def shell_env(payload: dict[str, Any]) -> dict[str, str]: env["CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M"] = str( payload.get("cup_holder_place_final_z_offset_m") or env.get("CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M") - or "-0.020" + or "-0.030" ) env["CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M"] = str( payload.get("cup_holder_place_final_y_offset_m") @@ -3570,7 +3643,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe place_final_z_offset_m = str( payload.get("cup_holder_place_final_z_offset_m") or os.environ.get("CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M") - or "-0.020" + or "-0.030" ).strip() place_final_y_offset_m = str( payload.get("cup_holder_place_final_y_offset_m") @@ -3831,9 +3904,19 @@ def run_step(step: Step, payload: dict[str, Any]) -> dict[str, Any]: if step.kind == "background": restart_output = "" if step.key in {"connect_robot", "start_tmux_stack"}: - restart_output = ( - "[Azas] tmux 통합 재연결: stop_azas_all.sh가 azas-logic tmux 세션을 종료하므로 " - "패널 서버가 tmux 밖에서 터미널과 같은 stop -> start 명령을 직접 실행합니다." + cleanup_events: list[str] = [] + cleanup_events.extend(cleanup_side_grip_stack(grace_sec=3.0)) + cleanup_events.extend(cleanup_camera_stack(grace_sec=3.0)) + cleanup_events.extend(cleanup_rg2_stack(grace_sec=3.0)) + restart_output = "\n".join( + part + for part in ( + "[Azas] tmux 통합 재연결: stop_azas_all.sh가 azas-logic tmux 세션을 종료하므로 " + "패널 서버가 tmux 밖에서 터미널과 같은 stop -> start 명령을 직접 실행합니다.", + "[Azas] reconnect pre-cleanup: 이전 side_grip/camera/RG2 잔여 프로세스를 먼저 정리합니다.", + "\n".join(cleanup_events), + ) + if part ) elif step.key == "connect_gripper": cleanup_events = cleanup_rg2_stack() @@ -4189,11 +4272,13 @@ def do_GET(self) -> None: "CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M", "-0.010" ), "cup_holder_place_final_z_offset_m": os.environ.get( - "CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M", "-0.020" + "CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M", "-0.030" ), } data = [] for step in STEPS: + if step.key in PANEL_HIDDEN_STEP_KEYS: + continue item = asdict(step) item["resolved_command"] = command_for(step, preview_payload) if step.implemented else "" item["command_saved"] = step.key in command_overrides @@ -4207,6 +4292,17 @@ def do_GET(self) -> None: self.send_json(dispenser_color_map_status()) return if path == "/api/camera_snapshot.jpg": + if os.environ.get("AZAS_PANEL_ENABLE_CAMERA_SNAPSHOT", "0") not in {"1", "true", "TRUE"}: + message = b"camera snapshot endpoint disabled in field panel" + try: + self.send_response(404) + self.send_header("Content-Type", "text/plain; charset=utf-8") + self.send_header("Content-Length", str(len(message))) + self.end_headers() + self.wfile.write(message) + except BrokenPipeError: + pass + return ok, body, error = camera_snapshot_jpeg() if not ok: self.send_response(503) @@ -4256,6 +4352,15 @@ def do_POST(self) -> None: steps_by_key = {step.key: step for step in STEPS} results = [] for key in selected: + if key in PANEL_HIDDEN_STEP_KEYS: + results.append( + { + "key": key, + "status": "blocked", + "output": "이 단계는 패널에서 제거된 내부/구버전 단계라 실행하지 않았습니다.", + } + ) + break step = steps_by_key.get(key) if step is None: continue diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 3fd32dd..e42d26d 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -243,18 +243,18 @@ def main() -> int: parser.add_argument("--regrasp-reset-joint-velocity", default="80.0") parser.add_argument("--regrasp-reset-joint-acceleration", default="35.0") parser.add_argument("--press-min-transit-z-m", default="0.500") - parser.add_argument("--press-line-velocity", default="80.0") - parser.add_argument("--press-line-acceleration", default="25.0") - parser.add_argument("--press-travel-velocity", default="80.0") - parser.add_argument("--press-travel-acceleration", default="60.0") - parser.add_argument("--press-contact-joint-velocity", default="80.0") - parser.add_argument("--press-contact-joint-acceleration", default="30.0") + parser.add_argument("--press-line-velocity", default="25.0") + parser.add_argument("--press-line-acceleration", default="10.0") + parser.add_argument("--press-travel-velocity", default="40.0") + parser.add_argument("--press-travel-acceleration", default="20.0") + parser.add_argument("--press-contact-joint-velocity", default="35.0") + parser.add_argument("--press-contact-joint-acceleration", default="15.0") parser.add_argument("--press-contact-entry-lift-m", default="0.050") parser.add_argument( "--press-reset-before-press", action=argparse.BooleanOptionalAction, default=True, - help="컵을 놓은 뒤 CONTACT_ENTRY_LIFT 전에 PRESS_COMMON_PRE/HOME joint waypoint를 경유", + help="컵을 놓은 뒤 CONTACT_ENTRY_LIFT 전에 PRESS_COMMON_PRE/HOME joint waypoint를 경유. 기본 false", ) parser.add_argument("--press-reset-joints-deg", default="0,0,90,0,90,0") parser.add_argument("--press-reset-joint-velocity", default="80.0") @@ -286,9 +286,15 @@ def main() -> int: parser.add_argument("--move-release-offset-x-m", default="-0.020") parser.add_argument("--move-release-offset-y-m", default="0.0") parser.add_argument("--move-release-offset-z-m", default="0.0") - parser.add_argument("--cup-pre-from-place-x-offset-m", default="-0.070") + parser.add_argument("--cup-pre-from-place-x-offset-m", default="-0.090") parser.add_argument("--cup-pre-from-place-z-offset-m", default="0.030") parser.add_argument("--generated-cup-pre-max-joint-delta-deg", default="190.0") + parser.add_argument( + "--press-contact-use-joint-move", + action=argparse.BooleanOptionalAction, + default=False, + help="measured PRESS_CONTACT movej 사용. 기본 false: PRESS_CONTACT FK까지 Cartesian Z-only 하강", + ) parser.add_argument( "--use-cup-common-pre", action=argparse.BooleanOptionalAction, @@ -361,7 +367,7 @@ def main() -> int: default=True, help="마지막 디스펜서 처리 후 컵홀더에 컵을 놓음", ) - parser.add_argument("--cup-holder-place-final-z-offset-m", default="-0.020") + parser.add_argument("--cup-holder-place-final-z-offset-m", default="-0.030") parser.add_argument("--cup-holder-place-final-y-offset-m", default="-0.010") parser.add_argument("--cup-holder-approach-velocity", default="80.0") parser.add_argument("--cup-holder-approach-acceleration", default="20.0") @@ -487,6 +493,11 @@ def main() -> int: sequence_extra_args.append( "--press-reset-before-press" if args.press_reset_before_press else "--no-press-reset-before-press" ) + sequence_extra_args.append( + "--press-contact-use-joint-move" + if args.press_contact_use_joint_move + else "--no-press-contact-use-joint-move" + ) sequence_extra_args.append( "--regrasp-reset-before-cup" if args.regrasp_reset_before_cup else "--no-regrasp-reset-before-cup" ) diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index dc4d415..94d7f73 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -2218,7 +2218,8 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: f"(entry_lift={entry_lift_m * 1000.0:.1f}mm) " f"z_overdrive_m={press_drop_m:.3f}" + ( - " source=PRESS_CONTACT_FK; measured pre-contact joints skipped" + " source=PRESS_CONTACT_FK; measured pre-contact joints skipped; " + f"press_contact_use_joint_move={str(self.args.press_contact_use_joint_move).lower()}" if skip_measured_press_pre else f" PRESS_PRE={format_joints_deg(pre_joints)}" ) @@ -2228,12 +2229,22 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: contact_entry_posx, label="CONTACT_ENTRY_LIFT above measured PRESS_CONTACT", ) - self.movej( - contact_joints, - label="PRESS_CONTACT measured contact joints", - velocity=self.args.press_contact_joint_velocity, - acceleration=self.args.press_contact_joint_acceleration, - ) + if self.args.press_contact_use_joint_move: + self.movej( + contact_joints, + label="PRESS_CONTACT measured contact joints", + velocity=self.args.press_contact_joint_velocity, + acceleration=self.args.press_contact_joint_acceleration, + ) + else: + self.move_posx( + list(contact_fk_posx[:6]), + label="Z-only descend to measured PRESS_CONTACT FK", + velocity=self.args.press_line_velocity, + acceleration=self.args.press_line_acceleration, + timeout_sec=self.args.press_timeout_sec, + verify_tolerance_mm=max(self.args.target_tolerance_mm, 25.0), + ) contact_posx = self.current_posx(timeout_sec=self.args.wait_service_sec) for press_index in range(1, max(int(press_count), 1) + 1): suffix = f" {press_index}/{press_count}" if press_count > 1 else "" @@ -2872,10 +2883,10 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--cup-pre-from-place-x-offset-m", type=float, - default=-0.070, + default=-0.090, help=( "Generate DISP_PRE from the latest measured cup_place pose by changing only X. " - "Default -0.070m." + "Default -0.090m." ), ) parser.add_argument( @@ -3187,10 +3198,10 @@ def parse_args() -> argparse.Namespace: default=0.500, help="Minimum absolute TCP Z before moving from cup release toward dispenser press joints.", ) - parser.add_argument("--press-line-velocity", type=float, default=80.0) - parser.add_argument("--press-line-acceleration", type=float, default=25.0) - parser.add_argument("--press-travel-velocity", type=float, default=80.0) - parser.add_argument("--press-travel-acceleration", type=float, default=60.0) + parser.add_argument("--press-line-velocity", type=float, default=25.0) + parser.add_argument("--press-line-acceleration", type=float, default=10.0) + parser.add_argument("--press-travel-velocity", type=float, default=40.0) + parser.add_argument("--press-travel-acceleration", type=float, default=20.0) parser.add_argument("--press-timeout-sec", type=float, default=120.0) parser.add_argument("--press-hold-seconds", type=float, default=0.25) parser.add_argument("--press-gripper-close-width-m", type=float, default=0.0) @@ -3202,7 +3213,8 @@ def parse_args() -> argparse.Namespace: help=( "After cup release, safe lift, and empty-gripper close, move through " "calibration.yaml press_common_pre_joints_deg before CONTACT_ENTRY_LIFT. " - "If that key is missing, falls back to --press-reset-joints-deg." + "If that key is missing, falls back to --press-reset-joints-deg. " + "Default false because the measured common pre/HOME joint waypoint can choose a large wrist branch." ), ) parser.add_argument( @@ -3214,8 +3226,17 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--press-reset-joint-acceleration", type=float, default=25.0) parser.add_argument("--press-pre-joint-velocity", type=float, default=80.0) parser.add_argument("--press-pre-joint-acceleration", type=float, default=25.0) - parser.add_argument("--press-contact-joint-velocity", type=float, default=80.0) - parser.add_argument("--press-contact-joint-acceleration", type=float, default=30.0) + parser.add_argument("--press-contact-joint-velocity", type=float, default=35.0) + parser.add_argument("--press-contact-joint-acceleration", type=float, default=15.0) + parser.add_argument( + "--press-contact-use-joint-move", + action=argparse.BooleanOptionalAction, + default=False, + help=( + "Use measured PRESS_CONTACT movej after CONTACT_ENTRY_LIFT. Default false: " + "PRESS_CONTACT joints are used only for FK, then the robot descends Z-only by Cartesian MoveLine." + ), + ) parser.add_argument( "--press-contact-entry-lift-m", type=float, @@ -3367,20 +3388,20 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--press-generated-pre-use-joint", action=argparse.BooleanOptionalAction, - default=True, + default=False, help=( - "Approach the generated high PRESS_PRE pose with IK MoveJoint by default. " - "This avoids long Cartesian orientation interpolation from cup-place posture near the dispenser. " - "The actual contact/press stroke remains Z-only MoveLine." + "Approach the generated CONTACT_ENTRY_LIFT/PRESS_PRE pose with IK MoveJoint. " + "Default false: press entry uses Cartesian MoveLine to avoid large wrist/joint branch changes." ), ) parser.add_argument( "--press-generated-pre-joint-fallback", action=argparse.BooleanOptionalAction, - default=True, + default=False, help=( "When MoveLine to generated PRESS_PRE returns complete but target verification stalls, " - "retry that high approach pose with IK MoveJoint. The actual contact/press stroke remains Z-only MoveLine." + "retry that high approach pose with IK MoveJoint. Default false because press entry should not " + "fall back to a large joint branch unless explicitly requested." ), ) parser.add_argument("--gripper-service", default="/jarvis/rg2/set_width") @@ -3408,7 +3429,7 @@ def parse_args() -> argparse.Namespace: default=True, help="After the final dispenser re-grasp, place the held cup into calibration.yaml cup_holder.side_grip_place.", ) - parser.add_argument("--cup-holder-place-final-z-offset-m", type=float, default=-0.020) + parser.add_argument("--cup-holder-place-final-z-offset-m", type=float, default=-0.030) parser.add_argument("--cup-holder-place-final-y-offset-m", type=float, default=-0.010) parser.add_argument("--cup-holder-approach-velocity", type=float, default=80.0) parser.add_argument("--cup-holder-approach-acceleration", type=float, default=20.0) @@ -3561,7 +3582,8 @@ def main() -> int: if args.skip_measured_press_pre: print( "[Azas] source=calibration.yaml generated DISP_PRE from DISP_PLACE X/Z offset, measured DISP_PLACE, " - "PRESS_COMMON_PRE, and PRESS_CONTACT joint teaching; press_pre_joints_deg ignored by default" + "and PRESS_CONTACT FK teaching; press_pre_joints_deg ignored by default; " + "PRESS_COMMON_PRE is used only with --press-reset-before-press" ) else: print("[Azas] source=calibration.yaml generated DISP_PRE from DISP_PLACE X/Z offset, measured DISP_PLACE and PRESS_PRE/PRESS_CONTACT joint teaching") diff --git a/tools/run/stop_azas_all.sh b/tools/run/stop_azas_all.sh index ee6678a..1be0ecd 100755 --- a/tools/run/stop_azas_all.sh +++ b/tools/run/stop_azas_all.sh @@ -39,8 +39,11 @@ self_and_ancestors() { PROTECTED_PIDS=" $(self_and_ancestors | tr '\n' ' ') " collect_pids() { - ps -eo pid=,args= | grep -E "${ROS_PATTERN}" | grep -Ev "${PROTECT_PATTERN}" \ - | while read -r pid args; do + ps -eo pid=,stat=,args= | grep -E "${ROS_PATTERN}" | grep -Ev "${PROTECT_PATTERN}" \ + | while read -r pid stat args; do + # Defunct children cannot be killed; counting them as live ROS processes + # prevents FastDDS SHM cleanup and makes reconnect look stuck. + [[ "${stat}" == Z* ]] && continue [[ "${PROTECTED_PIDS}" == *" ${pid} "* ]] && continue echo "${pid}" done From dad9f3d2298ed003b9b3d5b26cb6fdbb4fc1868a Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Thu, 11 Jun 2026 14:09:27 +0900 Subject: [PATCH 60/88] feat: Update robot pipeline control and handover logic - Refactor robot pipeline control HTML to improve clarity and flow of actions. - Enhance handover logic in Python scripts to support non-interactive approvals. - Modify shell scripts to log success and failure states for side-grip and shake operations. - Improve smoke test scripts for cocktail sequences to ensure proper publisher and subscriber counts. --- docs/robot_pipeline_control.html | 48 ++---- tools/run/handover_cup_to_palm.py | 12 +- tools/run/robot_pipeline_control_server.py | 150 ++++++++++-------- tools/run/run_changhyun_side_grip_direct.sh | 20 ++- tools/run/run_kang_lid_grip_close_direct.sh | 35 ++-- tools/run/run_rule_based_shake_real.sh | 28 +++- tools/smoke/smoke_cocktail_dryrun_sequence.py | 18 ++- .../smoke_one_click_cocktail_no_motion.sh | 4 +- .../smoke/smoke_voice_cocktail_no_hardware.py | 13 +- 9 files changed, 190 insertions(+), 138 deletions(-) diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index f7b0e2f..2e9cf3b 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -841,22 +841,22 @@

Azas Robot Pipeline Control

-
실험 순서: 연결 준비1 창현 side-grip → 성공 확인 후 2 소명 누운 컵 → 성공 확인 후 3 강개발자 뚜껑. 로직 버튼은 한 번에 묶지 말고 단독 실행합니다.
+
현재 시험 순서: 연결 준비색상 JSON창현 side-grip 성공 후 자동 디스펜서 → 성공 확인 후 ArUco 뚜껑 체결. 전체 묶기는 창현/소명 선택 확정 전까지 보류합니다.
- + - + - +
@@ -1304,36 +1304,7 @@

RealSense 카메라 화면

} function withCollisionScenePrereq(queue) { - const items = queue.map((item) => ({...item})); - function ensureBefore(targetKey, prereqKeys) { - let targetIndex = items.findIndex((item) => item.key === targetKey); - if (targetIndex < 0) return; - for (const prereq of prereqKeys) { - const existingIndex = items.findIndex((item) => item.key === prereq); - if (existingIndex >= 0) { - if (existingIndex < targetIndex) continue; - items.splice(existingIndex, 1); - if (existingIndex < targetIndex) targetIndex -= 1; - } - } - targetIndex = items.findIndex((item) => item.key === targetKey); - const missing = prereqKeys.filter((prereq) => !items.slice(0, targetIndex).some((item) => item.key === prereq)); - missing.forEach((prereq, offset) => { - items.splice(targetIndex + offset, 0, {id: `q${nextQueueId++}`, key: prereq, injected: true}); - }); - } - // 창현/소명/강개발자 단독 테스트는 이미 열린 tmux/ROS 세션을 재사용한다. - // 전체 준비 묶음은 각 통합 플로우 버튼에서 명시적으로 넣는다. - ensureBefore("color_scan", ["connect_robot", "status_check", "move_to_color_scan_pose", "start_camera"]); - const firstCollisionIndex = items.findIndex((item) => needsCollisionScene(item.key)); - if (firstCollisionIndex < 0) return items; - const existingSceneIndex = items.findIndex((item) => item.key === "start_collision_scene"); - if (existingSceneIndex >= 0 && existingSceneIndex <= firstCollisionIndex) return items; - const withoutScene = items.filter((item) => item.key !== "start_collision_scene"); - const insertIndex = withoutScene.findIndex((item) => needsCollisionScene(item.key)); - if (insertIndex < 0) return withoutScene; - withoutScene.splice(insertIndex, 0, {id: `q${nextQueueId++}`, key: "start_collision_scene", injected: true}); - return withoutScene; + return queue.map((item) => ({...item})); } function renderSummary() { @@ -1945,13 +1916,13 @@

RealSense 카메라 화면

queueOnly(["cup_uprighting"], "2단계 소명/누운 컵 직립화 로직만 단독으로 큐에 추가했습니다. 1단계 성공 확인 후 실행하세요. OpenCV 창에서 컵을 확인한 뒤 p 키로 실행합니다."); }); document.getElementById("sideGripBtn")?.addEventListener("click", () => { - queueOnly(["side_grip"], "1단계 창현/PR #20 RealSense side-grip만 단독으로 큐에 추가했습니다. 카메라 화면에서 컵을 확인한 뒤 p 키로 직접 잡습니다. 실패하거나 로봇이 안 움직이면 이 단계만 다시 실행하세요."); + queueOnly(["side_grip"], "창현/PR #20 RealSense side-grip을 큐에 추가했습니다. OpenCV 창에서 p 키로 컵 잡기가 성공하면 통합 디스펜서 레시피가 자동 실행됩니다."); }); document.getElementById("pickLidBtn")?.addEventListener("click", () => { queueOnly(["start_camera", "pick_lid"], "뚜껑 grip pose 계획 로직을 큐에 추가했습니다. 실제 로봇 모션은 실행하지 않습니다."); }); document.getElementById("lidGripCloseBtn")?.addEventListener("click", () => { - queueOnly(["lid_grip_close"], "3단계 강개발자 lid_grip_close만 단독으로 큐에 추가했습니다. 1/2단계 성공 확인 후 실행하세요. ArUco 확인 후 p 키 흐름으로 실행합니다."); + queueOnly(["lid_grip_close"], "ArUco 뚜껑 체결 시험을 큐에 추가했습니다. side-grip 후 통합 디스펜서가 성공한 다음 실행하세요. 스크립트가 뚜껑 보기 자세로 이동한 뒤 ArUco 확인/p 키 흐름으로 진행합니다."); }); document.getElementById("colorScanJsonBtn")?.addEventListener("click", () => { @@ -1971,7 +1942,8 @@

RealSense 카메라 화면

queueOnly(["place_cup_holder"], "컵홀더 배치를 큐에 추가했습니다. 이 단계는 MoveItPy 경로계획으로 pre_place→place_final→RG2 open→retreat를 실행합니다."); }); document.getElementById("fullCocktailRealBtn")?.addEventListener("click", async () => { - queueOnly([...PREP_STEPS, "side_grip_camera_home", "side_grip", "move_to_color_scan_pose", "color_scan", "run_color_recipe_sequence", "place_cup_holder"], "전체 플로우를 큐에 추가했습니다. (tmux 연결 준비 → 카메라 홈→컵 side-grip → 검증자세 색상 핸들 JSON → 레시피 사이클 → MoveIt 컵홀더 배치)", {clear: true}); + log.textContent = "전체 묶기는 창현/소명 side-grip 선택이 확정될 때까지 보류합니다. 지금 시험은 색상 JSON → 창현 side-grip 성공 후 자동 디스펜서 → ArUco 뚜껑 체결 순서로 단독 버튼을 사용하세요."; + focusLog(); }); document.getElementById("run").addEventListener("click", async () => { if (isRunning) { @@ -1979,7 +1951,7 @@

RealSense 카메라 화면

focusLog(); return; } - const selected = withCollisionScenePrereq(selectedQueue); + const selected = selectedQueue.map((item) => ({...item})); selectedQueue = selected; resetResultBadges(); isRunning = Boolean(selected.length); diff --git a/tools/run/handover_cup_to_palm.py b/tools/run/handover_cup_to_palm.py index ebcc66f..11c0fbd 100755 --- a/tools/run/handover_cup_to_palm.py +++ b/tools/run/handover_cup_to_palm.py @@ -208,8 +208,11 @@ def open_gripper(args: argparse.Namespace) -> None: raise RuntimeError(f"RG2 open failed (rc={rc})") -def require_typed_approval(phrase: str, *, prompt: str) -> None: +def require_typed_approval(phrase: str, *, prompt: str, preapproved: str = "") -> None: print(prompt) + if preapproved.strip() == phrase: + print(f"[Azas] approval {phrase} supplied non-interactively (panel/wrapper mode)") + return entered = input(f"Type {phrase} to continue: ").strip() if entered != phrase: raise RuntimeError(f"operator approval mismatch; expected {phrase}") @@ -255,6 +258,11 @@ def parse_args() -> argparse.Namespace: help="debug: skip camera sampling and use this base-frame palm 'x,y,z' (meters)") parser.add_argument("--execute", action="store_true") parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} with --execute") + parser.add_argument("--approve-motion", default="", + help=f"non-interactive operator approval; must equal {MOTION_APPROVAL_PHRASE} " + "(for panel/wrapper use where stdin is unavailable)") + parser.add_argument("--approve-release", default="", + help=f"non-interactive release approval; must equal {RELEASE_APPROVAL_PHRASE}") return parser.parse_args() @@ -313,6 +321,7 @@ def main() -> int: " - first run was validated on a foam block, not a person\n" " - speeds/bounds above were reviewed" ), + preapproved=args.approve_motion, ) run_movel(args, lift, label="LIFT to transit height (Z-only)", velocity=args.transit_velocity, acceleration=args.transit_acceleration) @@ -350,6 +359,7 @@ def main() -> int: require_typed_approval( RELEASE_APPROVAL_PHRASE, prompt="[Azas] Cup is at release height. Confirm the palm is directly under the cup.", + preapproved=args.approve_release, ) open_gripper(args) time.sleep(1.0) diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 21c6825..a981b2e 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -333,7 +333,8 @@ def chain_recipe_after_manual_command(manual_cmd: str, payload: dict[str, Any], f"( {manual_cmd} ); " "manual_rc=$?; " "if [ ${manual_rc} -eq 0 ]; then " - f"echo '[Azas] {label} 성공 종료 -> 통합 디스펜서 색상 레시피를 즉시 실행합니다.'; " + f"echo '[Azas] {label} 성공 메시지 확인 -> 통합 디스펜서 색상 레시피를 자동 실행합니다.'; " + "echo '[Azas] auto_integrated_dispenser_recipe=true'; " f"{recipe_cmd}; " "else " f"echo '[Azas] {label} 실패/중단 rc='${{manual_rc}}' -> 디스펜서 레시피 실행을 건너뜁니다.'; " @@ -887,6 +888,25 @@ class Step: "실제 로봇 미사용: 별도 ROS_DOMAIN_ID에서 쉐이킹 궤적/마커를 RViz로 표시", ), Step("shake_closed_cup", "컵홀더 컵 다시 잡기 후 쉐이킹", "run", "tools/run/pick_from_cup_holder_side_grip.py && tools/run/run_rule_based_shake_real.sh", True, True, "시작 시 컵홀더에 놓인 닫힌 컵을 측정된 cup_holder.side_grip_place pose로 다시 side-grip 픽업한 뒤, J3 양수 고정 및 J4/J5/J6 트위스트 쉐이킹을 실행"), + Step( + "start_hand_detection", + "손 검출 시작 / 무모션", + "background", + "bash tools/run/run_human_hand_detection.sh", + True, + False, + "perception 전용: MediaPipe로 펼친 손바닥을 추적해 /azas/human_hand_detection으로 발행. 로봇 모션 없음", + ), + Step( + "handover_cup_to_palm", + "쉐이킹 후 손바닥에 컵 건네기", + "run", + "tools/run/handover_cup_to_palm.py", + True, + True, + "실제모션 HRI: 손 검출이 먼저 켜져 있어야 함. 손바닥 위로 이동 후 외력 감시하며 저속 하강, 컵 release. " + "첫 사용 전 스펀지 테스트로 --release-tcp-above-palm-m 튜닝 필수", + ), ] processes: dict[str, subprocess.Popen[str]] = {} @@ -913,6 +933,7 @@ class Step: "cup_uprighting", "lid_grip_close", "shake_rviz_preview", + "start_hand_detection", } PANEL_HIDDEN_STEP_KEYS = { "rviz_cocktail_collision_preview", @@ -954,6 +975,7 @@ class Step: # are not enough for real-motion GUI workflows. "side_grip", "cup_uprighting", + "lid_grip_close", } DOOSAN_STACK_PATTERNS = ( @@ -1043,6 +1065,7 @@ class Step: "yolo_cup_pick_moveit_py", "hand_eye_static_tf_node", "link6_gripper_collision_node", + "workspace_collision_scene_node", "--frame-id world --child-frame-id base_link", ) @@ -2056,6 +2079,16 @@ def required_services_for_step(step: Step, service_prefix: str) -> list[str]: f"/{clean}/system/get_robot_state", "/check_state_validity", ] + if step.key == "handover_cup_to_palm": + return [ + "/jarvis/rg2/set_width", + f"/{clean}/motion/move_line", + f"/{clean}/motion/ikin", + f"/{clean}/motion/check_motion", + f"/{clean}/aux_control/get_current_posx", + f"/{clean}/aux_control/get_tool_force", + f"/{clean}/system/get_robot_state", + ] return [] @@ -2077,7 +2110,7 @@ def required_service_wait_timeout(step: Step) -> float: or step.key == "lid_grip_close" ): return 35.0 - if step.key in {"home_robot", "lift_robot", "side_grip_camera_home", "lid_view_pose", "move_to_color_scan_pose", "side_grip", "shake_closed_cup"}: + if step.key in {"home_robot", "lift_robot", "side_grip_camera_home", "lid_view_pose", "move_to_color_scan_pose", "side_grip", "shake_closed_cup", "handover_cup_to_palm"}: return 30.0 if step.key == "gripper_soft_grasp": return 12.0 @@ -3072,73 +3105,28 @@ def requires_collision_scene_step(key: str) -> bool: def with_collision_scene_prereq(selected: list[str]) -> list[str]: - ordered = list(dict.fromkeys(selected)) - - def ensure_before(target: str, prerequisites: list[str]) -> None: - if target not in ordered: - return - target_index = ordered.index(target) - for prereq in prerequisites: - if prereq in ordered: - prereq_index = ordered.index(prereq) - if prereq_index < target_index: - continue - ordered.pop(prereq_index) - if prereq_index < target_index: - target_index -= 1 - target_index = ordered.index(target) - missing = [ - prereq - for prereq in prerequisites - if prereq not in ordered[:target_index] - ] - for offset, prereq in enumerate(missing): - ordered.insert(target_index + offset, prereq) - - # 창현/소명/강개발자 수동 OpenCV 로직은 검증된 tmux stack을 사용한 뒤 - # 단독 버튼으로 실행한다. 서버가 connect/status/camera 단계를 다시 끼워 - # 넣으면 패널 실행 경로가 수동 tmux 경로와 달라지고 느려진다. - - # Color classification must aim the robot at the measured color-scan pose - # before sampling the dispenser image. If the camera is already running from - # side-grip, keep it there; otherwise start it before color_scan. - ensure_before( - "color_scan", - ["start_tmux_stack", "status_check", "move_to_color_scan_pose"], - ) - - # The measured dispenser recipe assumes the cup has already been grasped by - # side_grip or an equivalent operator-verified step. Here we only ensure the - # real-motion services and gripper service are available before the cycle. - ensure_before( - "run_color_recipe_sequence", - ["start_tmux_stack", "status_check"], - ) - - first_collision_index = next( - ( - index - for index, key in enumerate(ordered) - if requires_collision_scene_step(key) - ), - -1, - ) - if first_collision_index >= 0: - existing_scene_index = ( - ordered.index("start_collision_scene") if "start_collision_scene" in ordered else -1 - ) - if existing_scene_index < 0 or existing_scene_index > first_collision_index: - ordered = [key for key in ordered if key != "start_collision_scene"] - first_collision_index = next( - ( - index - for index, key in enumerate(ordered) - if requires_collision_scene_step(key) - ), - 0, - ) - ordered.insert(first_collision_index, "start_collision_scene") - return list(dict.fromkeys(ordered)) + ordered: list[str] = [] + + def append_once(key: str) -> None: + if key not in ordered: + ordered.append(key) + + for key in selected: + if key == "color_scan": + for prereq in ( + "connect_robot", + "status_check", + "start_collision_scene", + "move_to_color_scan_pose", + "start_camera", + ): + append_once(prereq) + elif requires_collision_scene_step(key): + for prereq in ("connect_robot", "status_check", "start_collision_scene"): + append_once(prereq) + append_once(key) + + return ordered def configure_manual_recipe_chain(selected: list[str], payload: dict[str, Any]) -> list[str]: @@ -3499,7 +3487,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " f"bash {shlex.quote(str(direct_script))}" ) - if payload.get("_auto_recipe_after_manual_logic"): + if payload.get("_auto_recipe_after_manual_logic", True): return chain_recipe_after_manual_command(manual_cmd, payload, "창현 side_grip") return manual_cmd if step.key == "gripper_soft_grasp": @@ -3722,6 +3710,28 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true " "tools/run/run_rule_based_shake_real.sh" ) + if step.key == "handover_cup_to_palm": + release_height_m = str( + payload.get("handover_release_tcp_above_palm_m") + or os.environ.get("HANDOVER_RELEASE_TCP_ABOVE_PALM_M") + or "0.08" + ).strip() + return ( + f"cd {ROOT} && " + f"{ROS_SETUP} && " + "echo '[Azas] HANDOVER START: 펼친 손바닥을 추적해 컵을 손 위에 내려놓습니다.' && " + "echo '[Azas] 전제: 손 검출 시작 버튼이 켜져 있고, 받는 사람이 손바닥을 펴고 멈춰 있어야 합니다.' && " + "echo '[Azas] 안전: 하강은 2cm 스텝마다 외력을 확인하고, 손이 움직이면 자동 후퇴합니다.' && " + "python3 tools/run/handover_cup_to_palm.py " + f"--service-prefix {service_prefix} " + f"--release-tcp-above-palm-m {shlex.quote(release_height_m)} " + "--transit-velocity 10.0 --transit-acceleration 14.0 " + "--descent-velocity 4.0 --descent-acceleration 6.0 " + "--force-abort-delta-n 10.0 " + "--execute --confirm ENABLE_HUMAN_PALM_HANDOVER " + "--approve-motion ENABLE_HUMAN_PALM_HANDOVER_MOTION " + "--approve-release RELEASE_CUP_NOW" + ) if step.command.strip(): return f"cd {ROOT} && {ROS_SETUP} && {step.command}" return "" diff --git a/tools/run/run_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh index ed81e63..0e4a8e0 100755 --- a/tools/run/run_changhyun_side_grip_direct.sh +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -105,6 +105,8 @@ if [[ "${should_start_relay}" == "true" ]]; then ) & fi +side_grip_success_log="$(mktemp /tmp/azas_changhyun_side_grip.XXXXXX.log)" +set +e ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ model_path:="${ROOT}/local_models/best.pt" \ conf:=0.35 imgsz:=640 device:=cpu target_class:=cup \ @@ -125,4 +127,20 @@ ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ workspace_boundary_collision_enabled:=true dispenser_collision_enabled:=true dispenser_collision_publish_objects:=true \ dispenser_collision_publish_markers:=true link6_gripper_collision_enabled:=false \ dispenser_collision_config_path:="${ROOT}/src/azas_bringup/config/measured_dispenser_collision.yaml" \ - moveit_controller_name:=/"${SERVICE_PREFIX}"/dsr_moveit_controller start_joint_state_relay:=false + moveit_controller_name:=/"${SERVICE_PREFIX}"/dsr_moveit_controller start_joint_state_relay:=false \ + 2>&1 | tee "${side_grip_success_log}" +launch_rc="${PIPESTATUS[0]}" +set -e + +if [[ "${launch_rc}" -eq 0 ]] && grep -q "exit_after_pick=true and one pick completed" "${side_grip_success_log}"; then + echo "[Azas] CHANGHYUN_SIDE_GRIP_SUCCESS: pick completed; downstream integrated dispenser recipe may start." + exit 0 +fi + +if [[ "${launch_rc}" -eq 0 ]]; then + echo "[Azas] CHANGHYUN_SIDE_GRIP_NO_SUCCESS: node exited without completed-pick success marker; integrated dispenser recipe will not start." + exit 3 +fi + +echo "[Azas] CHANGHYUN_SIDE_GRIP_FAILED: ros2 launch exited rc=${launch_rc}; integrated dispenser recipe will not start." +exit "${launch_rc}" diff --git a/tools/run/run_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh index a323a7d..fec0f5d 100755 --- a/tools/run/run_kang_lid_grip_close_direct.sh +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -10,10 +10,10 @@ ARUCO_DICTIONARY="${ARUCO_DICTIONARY:-DICT_4X4_50}" ARUCO_MARKER_ID="${ARUCO_MARKER_ID:-14}" ARUCO_FALLBACK_MARKERS="${ARUCO_FALLBACK_MARKERS:-}" ARUCO_MARKER_LENGTH_M="${ARUCO_MARKER_LENGTH_M:-0.03}" -ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +ROS_DOMAIN_ID="${LID_ROS_DOMAIN_ID:-${ROS_DOMAIN_ID:-9}}" ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" -MOVE_TO_LID_VIEW_POSE="${MOVE_TO_LID_VIEW_POSE:-true}" +MOVE_TO_LID_VIEW_POSE="${MOVE_TO_LID_VIEW_POSE:-false}" cd "${ROOT}" @@ -46,6 +46,7 @@ echo "[Azas] OpenCV window: confirm lid ArUco, then press p. Quit with q/Esc." echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XAUTHORITY}" echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY} FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS}" echo "[Azas] aruco=${ARUCO_DICTIONARY}:${ARUCO_MARKER_ID} fallback=${ARUCO_FALLBACK_MARKERS} length_m=${ARUCO_MARKER_LENGTH_M}" +echo "[Azas] note: use_j6_yaw_for_pick/pick_j6_* are not supported by this Azas launch; using supported ArUco-axis orientation parameters." if [[ ! -f "${MODEL_PATH}" ]]; then echo "[Azas][WARN] model_path not found: ${MODEL_PATH}" @@ -78,32 +79,36 @@ launch_args=( aruco_dictionary:="${ARUCO_DICTIONARY}" aruco_marker_id:="${ARUCO_MARKER_ID}" \ aruco_marker_length_m:="${ARUCO_MARKER_LENGTH_M}" \ use_aruco_axis_for_orientation:=true aruco_finger_axis_quarter_turns:=0 \ - use_lid_pose_yaw_for_pick:=true lid_pose_yaw_axis:=y lid_pose_yaw_offset_deg:=0.0 lid_pose_yaw_equivalence_deg:=180.0 \ - visual_refine_before_grasp:=true visual_refine_sample_count:=5 visual_refine_timeout_sec:=3.0 visual_refine_max_yaw_std_deg:=3.0 \ + use_lid_pose_yaw_for_pick:=false lid_pose_yaw_axis:=y lid_pose_yaw_offset_deg:=0.0 lid_pose_yaw_equivalence_deg:=360.0 \ + visual_refine_before_grasp:=true visual_refine_sample_count:=5 visual_refine_timeout_sec:=3.0 visual_refine_max_yaw_std_deg:=5.0 \ visual_refine_max_position_std_m:=0.005 visual_refine_apply_xy:=true visual_refine_apply_yaw:=true visual_refine_fallback_to_initial_plan:=true \ enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION allow_service_control_without_moveit:=true service_prefix:="${SERVICE_PREFIX}" \ - approach_lid_with_movej:=true approach_movej_velocity:=20.0 approach_movej_acceleration:=20.0 \ - lid_overhead_approach_enabled:=true lid_overhead_min_z_m:=0.260 \ + approach_lid_with_movej:=false approach_movej_velocity:=20.0 approach_movej_acceleration:=20.0 \ + lid_overhead_approach_enabled:=false lid_overhead_min_z_m:=0.260 \ rx:=108.41 ry:=-176.32 rz:=175.98 offset_axis:=base_z surface_offset_m:=0.0 \ - tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:=0.160 min_grasp_z_m:=0.180 \ - approach_offset_m:=0.08 min_approach_z_m:=0.260 lift_offset_m:=0.10 settle_seconds_before_grasp:=0.5 hold_seconds_after_grasp:=3.0 \ - line_velocity:=15.0 line_acceleration:=8.0 move_timeout_sec:=90.0 \ + tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:=-0.040 min_grasp_z_m:=0.025 \ + approach_offset_m:=0.08 min_approach_z_m:=0.0 lift_offset_m:=0.10 settle_seconds_before_grasp:=0.5 hold_seconds_after_grasp:=3.0 \ + line_velocity:=30.0 line_acceleration:=10.0 move_timeout_sec:=90.0 \ enable_gripper_service_calls:=true gripper_set_service:=/jarvis/rg2/set_width \ gripper_preopen_width_m:=0.110 gripper_grasp_width_m:=0.020 gripper_force_n:=12.0 \ continue_after_gripper_grasp_failure:=true gripper_grasp_failure_wait_sec:=2.0 \ enable_lid_twist_after_grasp:=true \ lid_twist_target_x_m:=0.422959106 lid_twist_target_y_m:=0.223224869 lid_twist_target_z_m:=0.166827988 \ lid_twist_rx:=73.901489 lid_twist_ry:=-178.542740 lid_twist_rz:=117.385612 \ - lid_twist_transfer_clearance_m:=0.12 lid_twist_transfer_max_z_m:=0.60 \ - lid_twist_use_force_control:=false lid_twist_force_rotation_mode:=j6 \ + lid_twist_transfer_clearance_m:=0.20 lid_twist_transfer_max_z_m:=0.60 \ + lid_twist_use_force_control:=false lid_twist_use_force_spiral:=true lid_twist_force_rotation_mode:=j6 \ + lid_twist_down_force_n:=2.0 lid_twist_force_ref:=base lid_twist_force_service_timeout_sec:=20.0 \ + lid_twist_force_settle_seconds:=0.2 lid_twist_force_release_time:=0.2 \ lid_twist_preseat_periodic_before_turn:=true \ lid_twist_preseat_periodic_x_amp_mm:=0.0 lid_twist_preseat_periodic_y_amp_mm:=0.0 lid_twist_preseat_periodic_z_amp_mm:=1.0 \ lid_twist_preseat_periodic_rx_amp_deg:=0.0 lid_twist_preseat_periodic_ry_amp_deg:=0.0 lid_twist_preseat_periodic_rz_amp_deg:=10.0 \ lid_twist_preseat_periodic_period_sec:=3.6 lid_twist_preseat_periodic_acc_time_sec:=1.0 lid_twist_preseat_periodic_repeat:=2 \ - lid_twist_preseat_periodic_ref:=tool lid_twist_rz_delta_deg:=300.0 lid_twist_turn_step_deg:=50.0 \ - lid_twist_release_lift_m:=0.03 lid_twist_min_z_m:=0.140 lid_twist_max_z_m:=0.220 \ - lid_twist_transfer_velocity:=25.0 lid_twist_press_velocity:=5.0 lid_twist_turn_velocity:=30.0 lid_twist_acceleration:=15.0 \ - lid_twist_hold_seconds_before_turn:=0.0 lid_twist_hold_seconds_after_turn:=0.5 + lid_twist_preseat_periodic_ref:=tool lid_twist_rz_delta_deg:=360.0 lid_twist_turn_step_deg:=60.0 \ + lid_twist_release_lift_m:=0.03 lid_twist_min_z_m:=0.140 lid_twist_max_z_m:=0.260 \ + lid_twist_transfer_velocity:=25.0 lid_twist_press_velocity:=10.0 lid_twist_turn_velocity:=40.0 lid_twist_acceleration:=15.0 \ + lid_twist_hold_seconds_before_turn:=0.2 lid_twist_hold_seconds_after_turn:=0.5 \ + lid_twist_compliance_x_stiffness:=3000.0 lid_twist_compliance_y_stiffness:=3000.0 lid_twist_compliance_z_stiffness:=300.0 \ + lid_twist_compliance_rx_stiffness:=200.0 lid_twist_compliance_ry_stiffness:=200.0 lid_twist_compliance_rz_stiffness:=200.0 ) if [[ -n "${ARUCO_FALLBACK_MARKERS}" ]]; then diff --git a/tools/run/run_rule_based_shake_real.sh b/tools/run/run_rule_based_shake_real.sh index 299acd6..b47781d 100755 --- a/tools/run/run_rule_based_shake_real.sh +++ b/tools/run/run_rule_based_shake_real.sh @@ -288,7 +288,22 @@ else echo "[Azas] Cup-holder pick skipped only because SKIP_CUP_HOLDER_PICK=true was set by a wrapper that already completed it." fi -exec ros2 launch azas_bringup tumbler_shake_sequence.launch.py \ +# Stale side-grip workspace walls left by earlier pick stages repeatedly fail +# the shake-ready MoveIt state-validity check (link_2 <-> ..._x_min_wall). +# workspace_collision_scene_node keeps re-publishing the walls every cycle, so +# stop it first; a plain scene removal would be overwritten within seconds. +if pgrep -f "workspace_collision_scene_node" >/dev/null 2>&1; then + echo "[Azas] Stopping leftover workspace_collision_scene_node (it re-publishes the side-grip walls)." + pkill -f "workspace_collision_scene_node" || true + sleep 1.5 +fi +echo "[Azas] Removing stale side-grip workspace walls from MoveIt scene (best-effort)." +python3 "${ROOT_DIR}/tools/run/remove_moveit_collision_objects.py" \ + --ids side_grip_workspace_x_min_wall,side_grip_workspace_x_max_wall,side_grip_workspace_y_min_wall,side_grip_workspace_y_max_wall \ + || echo "[Azas] workspace wall removal returned non-zero (walls likely absent already); continuing." + +SHAKE_RUN_LOG="$(mktemp /tmp/azas_shake_run.XXXXXX.log)" +ros2 launch azas_bringup tumbler_shake_sequence.launch.py \ enable_hardware:=true \ hardware_confirm:=ENABLE_REAL_ROBOT_MOTION \ allow_service_control_without_moveit:=true \ @@ -353,4 +368,13 @@ exec ros2 launch azas_bringup tumbler_shake_sequence.launch.py \ joint_target_poll_sec:="${JOINT_TARGET_POLL_SEC}" \ require_state_validity_for_joint_shake:="${REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE}" \ state_validity_service:="${STATE_VALIDITY_SERVICE}" \ - planning_group:="${PLANNING_GROUP}" + planning_group:="${PLANNING_GROUP}" 2>&1 | tee "${SHAKE_RUN_LOG}" + +# ros2 launch exits 0 even when the shake node refused/aborted the motion, so +# the panel used to report success on a failed shake. Fail closed on the node's +# own failure markers. +if grep -q "refusing MoveJoint\|tumbler_shake_sequence_node.*FAILED" "${SHAKE_RUN_LOG}"; then + echo "[FAIL] shake sequence reported failure (see log above); exiting non-zero." + exit 1 +fi +echo "[Azas] shake sequence finished without failure markers." diff --git a/tools/smoke/smoke_cocktail_dryrun_sequence.py b/tools/smoke/smoke_cocktail_dryrun_sequence.py index 1921219..05163b9 100755 --- a/tools/smoke/smoke_cocktail_dryrun_sequence.py +++ b/tools/smoke/smoke_cocktail_dryrun_sequence.py @@ -81,10 +81,14 @@ def main() -> int: deadline = time.monotonic() + 8.0 # Let discovery connect before publishing the one-shot inputs. - while time.monotonic() < deadline and node.count_publishers("/azas/cocktail/status") == 0: + while time.monotonic() < deadline and ( + node.count_publishers("/azas/cocktail/status") == 0 + or node.count_subscribers("/azas/cup_detection") == 0 + or node.count_subscribers("/azas/voice/recipe_decision") == 0 + ): rclpy.spin_once(node, timeout_sec=0.1) - for _ in range(5): + for _ in range(10): # The cocktail dry-run planner consumes symbolic cup/lid presence only. # Motion-facing cup poses are produced by the perception bridge from # live statuses that start with "detected:upright"; this smoke does not @@ -93,8 +97,14 @@ def main() -> int: node.publish_detection("detected:lid bbox=80x80 depth_raw=260.0") rclpy.spin_once(node, timeout_sec=0.1) - node.publish_decision() + next_publish = 0.0 while time.monotonic() < deadline: + now = time.monotonic() + if now >= next_publish: + node.publish_detection("detected:cup bbox=100x100 depth_raw=300.0") + node.publish_detection("detected:lid bbox=80x80 depth_raw=260.0") + node.publish_decision() + next_publish = now + 0.5 rclpy.spin_once(node, timeout_sec=0.1) if node.saw_complete(): required_phases = { @@ -118,7 +128,7 @@ def main() -> int: node.destroy_node() rclpy.shutdown() return 0 - if node.saw_blocked(): + if node.saw_blocked() and not node.latest_plan_phases(): print("[FAIL] cocktail dry-run sequence blocked") for item in node._statuses: print(json.dumps(item, ensure_ascii=False)) diff --git a/tools/smoke/smoke_one_click_cocktail_no_motion.sh b/tools/smoke/smoke_one_click_cocktail_no_motion.sh index 9efc1c9..aaa7201 100755 --- a/tools/smoke/smoke_one_click_cocktail_no_motion.sh +++ b/tools/smoke/smoke_one_click_cocktail_no_motion.sh @@ -128,8 +128,8 @@ for dispenser_id in ("1", "2", "3", "4"): recipe_source = Path('tools/run/run_measured_dispenser_recipe_sequence.py').read_text() assert 'default=False' in recipe_source and '--press-reset-before-press' in recipe_source assert 'PRESS_Z_OVERDRIVE -> PRESS_CONTACT -> PRESS_PRE' in recipe_source -assert 'contact_joints is not None and pre_joints is not None' in recipe_source -assert 'Fallback-only generated Cartesian PRE' in recipe_source +assert 'contact_joints is not None and (skip_measured_press_pre or pre_joints is not None)' in recipe_source +assert 'Fallback-only option for old contact-joint mode' in recipe_source print('[Azas smoke] measured PRE/CONTACT joint-first press path OK') path = Path('tools/run/robot_pipeline_control_server.py') diff --git a/tools/smoke/smoke_voice_cocktail_no_hardware.py b/tools/smoke/smoke_voice_cocktail_no_hardware.py index 6f191da..a200dd1 100644 --- a/tools/smoke/smoke_voice_cocktail_no_hardware.py +++ b/tools/smoke/smoke_voice_cocktail_no_hardware.py @@ -61,14 +61,17 @@ def main() -> int: while time.monotonic() < deadline and ( node.count_subscribers("/stt_result") == 0 or node.count_publishers("/azas/cocktail/status") == 0 + or node.count_publishers("/azas/voice/recipe_decision") == 0 + or node.count_subscribers("/azas/voice/recipe_decision") < 2 ): rclpy.spin_once(node, timeout_sec=0.1) - for _ in range(3): - node.publish_stt(text) - rclpy.spin_once(node, timeout_sec=0.1) - + next_publish = 0.0 while time.monotonic() < deadline: + now = time.monotonic() + if now >= next_publish: + node.publish_stt(text) + next_publish = now + 0.5 rclpy.spin_once(node, timeout_sec=0.1) if node.saw_complete(): decision = node.latest_decision() @@ -83,7 +86,7 @@ def main() -> int: node.destroy_node() rclpy.shutdown() return 0 - if node.saw_blocked(): + if node.saw_blocked() and not node._plans: print("[FAIL] cocktail dry-run blocked") for item in node._statuses: print(json.dumps(item, ensure_ascii=False)) From d01c2631ef858e8f08aa3ebdbeeb129da58bf75b Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Thu, 11 Jun 2026 14:22:37 +0900 Subject: [PATCH 61/88] Make RG2 mesh collision verifiable in M0609 planning Constraint: origin/develop adds the RG2FT mesh URDF, but the active M0609 MoveIt config must wire it into robot_description before collision checking can see the gripper. Rejected: Depend only on the link6 attached collision publisher | It can race startup or be skipped while MoveIt still plans with a bare robot model. Confidence: high Scope-risk: moderate Directive: Rebuild azas_description and the active dsr_moveit_config_m0609 overlay after changing gripper URDF geometry. Tested: colcon build --packages-select azas_description dsr_moveit_config_m0609 azas_perception --symlink-install; colcon build --packages-select dsr_moveit_config_m0609 --symlink-install in ~/ros2_ws; check_rg2_moveit_description.py passes for both Azas vendor and ~/ros2_ws install xacros; MoveItConfigsBuilder includes rg2_quick_changer, rg2_gripper_body, gripper_tcp, and the OnRobot RG2FT collision mesh; PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest -q src/azas_perception/test/test_depth_and_detection_logic.py. Not-tested: Live real-robot obstacle planning and hardware motion with the robot/camera/gripper connected. --- .../azas_perception/lid_marker.py | 65 +-------- third_party/ros2_src/doosan-robot2 | 2 +- tools/checks/check_rg2_moveit_description.py | 125 ++++++++++++++++++ 3 files changed, 132 insertions(+), 60 deletions(-) create mode 100755 tools/checks/check_rg2_moveit_description.py diff --git a/src/azas_perception/azas_perception/lid_marker.py b/src/azas_perception/azas_perception/lid_marker.py index 1dbbda4..ad1e90b 100644 --- a/src/azas_perception/azas_perception/lid_marker.py +++ b/src/azas_perception/azas_perception/lid_marker.py @@ -144,7 +144,6 @@ def detect_aruco_marker( gray = cv2.cvtColor(patch, cv2.COLOR_BGR2GRAY) dictionary = _create_aruco_dictionary(dictionary_id) -<<<<<<< refactor/rg2_collision2 if dictionary is None: return None parameters = _create_aruco_detector_parameters() @@ -159,17 +158,6 @@ def detect_aruco_marker( dictionary, parameters, ) -======= - parameters = _create_aruco_detector_parameters() - - # The lid marker appears small and oblique in the wrist-camera view. Try - # conservative contrast/scale variants, but keep the dictionary/id filter - # strict so a noisy table feature cannot become a false lid marker. - best: ArucoMarker | None = None - best_score = -1.0 - for candidate_gray, scale in _aruco_detection_images(gray): - corners_list, ids, _rejected = _detect_aruco_markers(candidate_gray, dictionary, parameters) ->>>>>>> develop candidate = _select_aruco_marker_from_detections( corners_list, ids, @@ -184,18 +172,14 @@ def detect_aruco_marker( def _aruco_dictionary_id(dictionary_name: str) -> int | None: -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) if aruco is None: return None -======= ->>>>>>> develop name = str(dictionary_name).strip().upper() if not name: return None if not name.startswith("DICT_"): name = f"DICT_{name}" -<<<<<<< refactor/rg2_collision2 return getattr(aruco, name, None) @@ -220,33 +204,14 @@ def _create_aruco_detector_parameters(): parameters = aruco.DetectorParameters_create() else: return None -======= - return getattr(cv2.aruco, name, None) - - -def _create_aruco_dictionary(dictionary_id: int): - if hasattr(cv2.aruco, "getPredefinedDictionary"): - return cv2.aruco.getPredefinedDictionary(dictionary_id) - return cv2.aruco.Dictionary_get(dictionary_id) - - -def _create_aruco_detector_parameters(): - if hasattr(cv2.aruco, "DetectorParameters"): - parameters = cv2.aruco.DetectorParameters() - else: - parameters = cv2.aruco.DetectorParameters_create() ->>>>>>> develop return _tune_lid_aruco_detector_parameters(parameters) def _tune_lid_aruco_detector_parameters(parameters): -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) -======= # The lid marker is small in the wrist-camera overview image and often seen # at an angle. Keep the expected marker-id filter strict, but make candidate # extraction and perspective sampling tolerant enough for the measured setup. ->>>>>>> develop tuned_values = { "adaptiveThreshWinSizeMin": 3, "adaptiveThreshWinSizeMax": 53, @@ -258,11 +223,7 @@ def _tune_lid_aruco_detector_parameters(parameters): "perspectiveRemovePixelPerCell": 8, "perspectiveRemoveIgnoredMarginPerCell": 0.20, "errorCorrectionRate": 0.75, -<<<<<<< refactor/rg2_collision2 "cornerRefinementMethod": getattr(aruco, "CORNER_REFINE_SUBPIX", 1), -======= - "cornerRefinementMethod": getattr(cv2.aruco, "CORNER_REFINE_SUBPIX", 1), ->>>>>>> develop "cornerRefinementWinSize": 3, } for name, value in tuned_values.items(): @@ -272,9 +233,6 @@ def _tune_lid_aruco_detector_parameters(parameters): def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: -<<<<<<< refactor/rg2_collision2 - """Return grayscale variants for small/low-contrast lid ArUco detection.""" -======= """Return grayscale variants for small/low-contrast lid ArUco detection. OpenCV returns corners in the coordinate system of the image it receives, @@ -282,7 +240,6 @@ def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: ROI. Variants are intentionally limited to deterministic contrast/scale transforms; no dictionary or marker-id relaxation is performed. """ ->>>>>>> develop variants: list[tuple[np.ndarray, float]] = [(gray, 1.0)] equalized = cv2.equalizeHist(gray) variants.append((equalized, 1.0)) @@ -291,23 +248,19 @@ def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: sharpened = cv2.addWeighted(gray, 1.6, blur, -0.6, 0) variants.append((sharpened, 1.0)) -<<<<<<< refactor/rg2_collision2 - for source in (gray, equalized, sharpened): - variants.append(( - cv2.resize(source, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_CUBIC), - 2.0, - )) -======= # Upscaling materially helps when the marker body is only a few tens of # pixels wide in the RealSense overview frame. for source in (gray, equalized, sharpened): - variants.append((cv2.resize(source, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_CUBIC), 2.0)) ->>>>>>> develop + variants.append( + ( + cv2.resize(source, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_CUBIC), + 2.0, + ) + ) return variants def _detect_aruco_markers(gray: np.ndarray, dictionary, parameters): -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) if aruco is None: return [], None, [] @@ -318,12 +271,6 @@ def _detect_aruco_markers(gray: np.ndarray, dictionary, parameters): kwargs = {"parameters": parameters} if parameters is not None else {} return aruco.detectMarkers(gray, dictionary, **kwargs) return [], None, [] -======= - if hasattr(cv2.aruco, "ArucoDetector"): - detector = cv2.aruco.ArucoDetector(dictionary, parameters) - return detector.detectMarkers(gray) - return cv2.aruco.detectMarkers(gray, dictionary, parameters=parameters) ->>>>>>> develop def _select_aruco_marker_from_detections( diff --git a/third_party/ros2_src/doosan-robot2 b/third_party/ros2_src/doosan-robot2 index 0a908f3..d0e39f9 160000 --- a/third_party/ros2_src/doosan-robot2 +++ b/third_party/ros2_src/doosan-robot2 @@ -1 +1 @@ -Subproject commit 0a908f31222e795e2e39ab5f187109ebbd85541b +Subproject commit d0e39f9132f47726ba35d34a59089e97c0baad33 diff --git a/tools/checks/check_rg2_moveit_description.py b/tools/checks/check_rg2_moveit_description.py new file mode 100755 index 0000000..6ab8fec --- /dev/null +++ b/tools/checks/check_rg2_moveit_description.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +"""Verify that the M0609 MoveIt description includes the RG2 collision mesh. + +This is a no-hardware check. It expands the MoveIt URDF xacro and verifies that +the robot model contains the vendored OnRobot RG2FT collision links instead of +planning with the bare robot flange only. +""" + +from __future__ import annotations + +import argparse +import os +import subprocess +import sys +import xml.etree.ElementTree as ET +from pathlib import Path + + +ROOT = Path(__file__).resolve().parents[2] +MOVEIT_XACRO = ( + ROOT + / "third_party" + / "ros2_src" + / "doosan-robot2" + / "dsr_moveit2" + / "dsr_moveit_config_m0609" + / "config" + / "m0609.urdf.xacro" +) + +REQUIRED_LINKS = { + "rg2_quick_changer", + "rg2_gripper_body", + "rg2_angle_bracket", + "rg2_left_inner_finger", + "rg2_right_inner_finger", + "gripper_tcp", +} + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Check that M0609 MoveIt robot_description contains RG2 mesh collisions." + ) + parser.add_argument("--xacro", type=Path, default=MOVEIT_XACRO) + return parser.parse_args() + + +def run_xacro(path: Path) -> str: + env = os.environ.copy() + env.setdefault("AMENT_PREFIX_PATH", str(ROOT / "install")) + try: + completed = subprocess.run( + ["xacro", str(path)], + check=True, + text=True, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + env=env, + ) + except FileNotFoundError: + print("[FAIL] xacro executable not found", file=sys.stderr) + raise SystemExit(1) + except subprocess.CalledProcessError as exc: + print(exc.stderr, file=sys.stderr) + print(f"[FAIL] failed to expand xacro: {path}", file=sys.stderr) + raise SystemExit(exc.returncode) + return completed.stdout + + +def main() -> int: + args = parse_args() + xacro_path = args.xacro.expanduser().resolve() + if not xacro_path.is_file(): + print(f"[FAIL] missing MoveIt xacro: {xacro_path}") + return 1 + + source = xacro_path.read_text(encoding="utf-8") + required_source = [ + "rg2_parametric.xacro", + "xacro:azas_rg2_parametric", + 'name="rg2_parent_link" default="tool0"', + 'name="rg2_mount_rpy" default="3.141592654 -1.570796327 0"', + ] + missing_source = [needle for needle in required_source if needle not in source] + if missing_source: + print("[FAIL] MoveIt xacro does not wire the RG2 description:") + for needle in missing_source: + print(f"missing={needle}") + return 1 + + root = ET.fromstring(run_xacro(xacro_path)) + links = {link.attrib["name"] for link in root.findall("link")} + missing_links = sorted(REQUIRED_LINKS - links) + if missing_links: + print(f"[FAIL] expanded robot_description missing RG2 links: {missing_links}") + return 1 + + collision_meshes = [ + mesh.attrib.get("filename", "") + for mesh in root.findall(".//collision/geometry/mesh") + ] + rg2_collision_meshes = [ + filename + for filename in collision_meshes + if filename.startswith("package://azas_description/meshes/onrobot_rg2ft/collision/") + ] + if len(rg2_collision_meshes) < 8: + print(f"[FAIL] expected RG2 collision meshes, found {len(rg2_collision_meshes)}") + return 1 + + quick_changer_joint = root.find("./joint[@name='rg2_quick_changer_joint']") + parent = quick_changer_joint.find("parent").attrib.get("link") if quick_changer_joint is not None else None + if parent != "tool0": + print(f"[FAIL] rg2_quick_changer_joint parent should be tool0, found {parent!r}") + return 1 + + print("[PASS] M0609 MoveIt robot_description includes RG2 mesh collision links.") + print(f"rg2_collision_meshes={len(rg2_collision_meshes)}") + print("rg2_parent=tool0") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) From e5d18c89fd2a15c5fe2a8dbc5691fa2046c7a039 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Thu, 11 Jun 2026 14:27:22 +0900 Subject: [PATCH 62/88] Make the RG2 M0609 description load from link_6 Constraint: dsr_description2/xacro/m0609.urdf.xacro exposes link_6 but not tool0, so the gripper-equipped Azas URDF must attach to link_6 when used by dsr_bringup2_rviz. Rejected: Leave rg2_parent_link defaulted to tool0 | robot_state_publisher can receive a disconnected gripper tree for the xacro-based Doosan description. Confidence: high Scope-risk: narrow Directive: Use rg2_parent_link:=link_6 for m0609_rg2_parametric unless the included Doosan source explicitly defines tool0. Tested: xacro m0609_rg2_parametric.urdf.xacro rg2_parent_link:=link_6 expands with RG2 links, 9 RG2 collision meshes, parent link_6, and one ros2_control tag; installed dsr_bringup2_rviz launch references m0609_rg2_parametric and parses with --show-args. Not-tested: Live RViz session connected to the real robot. --- src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro | 2 +- third_party/ros2_src/doosan-robot2 | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro b/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro index c46ed5a..6bb1041 100644 --- a/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro +++ b/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro @@ -1,6 +1,6 @@ - + diff --git a/third_party/ros2_src/doosan-robot2 b/third_party/ros2_src/doosan-robot2 index d0e39f9..a759bea 160000 --- a/third_party/ros2_src/doosan-robot2 +++ b/third_party/ros2_src/doosan-robot2 @@ -1 +1 @@ -Subproject commit d0e39f9132f47726ba35d34a59089e97c0baad33 +Subproject commit a759bea7b789e968e82c82539518c676d06c9eaa From 49a36f24d80cf9dc35fb9cfe3dcc037e6cec37ee Mon Sep 17 00:00:00 2001 From: chris3471 Date: Thu, 11 Jun 2026 14:53:47 +0900 Subject: [PATCH 63/88] Add auto cup flow router --- .../launch/auto_cup_flow_router.launch.py | 50 ++ .../azas_task_manager/auto_cup_flow_router.py | 436 ++++++++++++++++++ src/azas_task_manager/package.xml | 3 + src/azas_task_manager/setup.py | 1 + 4 files changed, 490 insertions(+) create mode 100644 src/azas_bringup/launch/auto_cup_flow_router.launch.py create mode 100644 src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py new file mode 100644 index 0000000..0a74653 --- /dev/null +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -0,0 +1,50 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument("enable_real_motion", default_value="false"), + DeclareLaunchArgument("router_confirm", default_value=""), + DeclareLaunchArgument("service_prefix", default_value=""), + DeclareLaunchArgument("moveit_controller_name", default_value="/dsr_moveit_controller"), + DeclareLaunchArgument("controller_action_name", default_value="/dsr_moveit_controller/follow_joint_trajectory"), + DeclareLaunchArgument("yolo_model_path", default_value="/home/ssu/Azas/local_models/best.pt"), + DeclareLaunchArgument("classifier_path", default_value="/home/ssu/Azas/cup_classifier_best.pth"), + DeclareLaunchArgument("classifier_arch", default_value="resnet18"), + DeclareLaunchArgument("classifier_min_confidence", default_value="0.70"), + DeclareLaunchArgument("route_timeout_sec", default_value="30.0"), + DeclareLaunchArgument("route_hold_sec", default_value="2.0"), + DeclareLaunchArgument("route_stable_required_samples", default_value="5"), + DeclareLaunchArgument("route_stable_min_sec", default_value="0.8"), + DeclareLaunchArgument("show_classification_window", default_value="true"), + DeclareLaunchArgument("side_extra_args", default_value=""), + DeclareLaunchArgument("cup_uprighting_extra_args", default_value=""), + Node( + package="azas_task_manager", + executable="auto_cup_flow_router", + name="auto_cup_flow_router", + output="screen", + parameters=[{ + "enable_real_motion": ParameterValue(LaunchConfiguration("enable_real_motion"), value_type=bool), + "router_confirm": LaunchConfiguration("router_confirm"), + "service_prefix": LaunchConfiguration("service_prefix"), + "moveit_controller_name": LaunchConfiguration("moveit_controller_name"), + "controller_action_name": LaunchConfiguration("controller_action_name"), + "yolo_model_path": LaunchConfiguration("yolo_model_path"), + "classifier_path": LaunchConfiguration("classifier_path"), + "classifier_arch": LaunchConfiguration("classifier_arch"), + "classifier_min_confidence": ParameterValue(LaunchConfiguration("classifier_min_confidence"), value_type=float), + "route_timeout_sec": ParameterValue(LaunchConfiguration("route_timeout_sec"), value_type=float), + "route_hold_sec": ParameterValue(LaunchConfiguration("route_hold_sec"), value_type=float), + "route_stable_required_samples": ParameterValue(LaunchConfiguration("route_stable_required_samples"), value_type=int), + "route_stable_min_sec": ParameterValue(LaunchConfiguration("route_stable_min_sec"), value_type=float), + "show_classification_window": ParameterValue(LaunchConfiguration("show_classification_window"), value_type=bool), + "side_extra_args": LaunchConfiguration("side_extra_args"), + "cup_uprighting_extra_args": LaunchConfiguration("cup_uprighting_extra_args"), + }], + ), + ]) diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py new file mode 100644 index 0000000..0270820 --- /dev/null +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -0,0 +1,436 @@ +from __future__ import annotations + +import os +import signal +import subprocess +import sys +import threading +import time +from dataclasses import dataclass +from typing import Optional + +import cv2 +import numpy as np +import rclpy +from azas_interfaces.msg import CupDetection +from dsr_msgs2.srv import MoveJoint, MoveWait +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_srvs.srv import Trigger + + +@dataclass(frozen=True) +class RouteDecision: + route: str + status: str + confidence: float + + +class AutoCupFlowRouter(Node): + def __init__(self) -> None: + super().__init__("auto_cup_flow_router") + + self.declare_parameter("router_confirm", "") + self.declare_parameter("enable_real_motion", False) + self.declare_parameter("observe_joints_deg", [3.0, -12.7, 44.0, -9.0, 133.0, 90.0]) + self.declare_parameter("observe_vel", 30.0) + self.declare_parameter("observe_acc", 30.0) + self.declare_parameter("observe_time", 0.0) + self.declare_parameter("motion_timeout_sec", 25.0) + self.declare_parameter("service_prefix", "") + self.declare_parameter("gripper_open_service", "/jarvis/rg2/open") + + self.declare_parameter("detection_topic", "/azas/cup_detection") + self.declare_parameter("color_topic", "/camera/camera/color/image_raw") + self.declare_parameter("perception_launch", "azas_bringup yolo_perception.launch.py") + self.declare_parameter("yolo_model_path", "/home/ssu/Azas/local_models/best.pt") + self.declare_parameter("classifier_path", "/home/ssu/Azas/cup_classifier_best.pth") + self.declare_parameter("classifier_arch", "resnet18") + self.declare_parameter("classifier_min_confidence", 0.70) + self.declare_parameter("route_timeout_sec", 30.0) + self.declare_parameter("route_stable_required_samples", 5) + self.declare_parameter("route_stable_min_sec", 0.8) + self.declare_parameter("route_hold_sec", 2.0) + self.declare_parameter("show_classification_window", True) + self.declare_parameter("window_name", "Azas cup route classifier") + + self.declare_parameter("side_launch", "dsr_practice yolo_cup_pick_node.launch.py") + self.declare_parameter("cup_uprighting_launch", "azas_cup_uprighting yolo_cup_uprighting.launch.py") + self.declare_parameter("moveit_controller_name", "/dsr_moveit_controller") + self.declare_parameter("controller_action_name", "/dsr_moveit_controller/follow_joint_trajectory") + self.declare_parameter("side_extra_args", "") + self.declare_parameter("cup_uprighting_extra_args", "") + + self._latest_detection: Optional[CupDetection] = None + self._latest_image: Optional[np.ndarray] = None + self._image_lock = threading.Lock() + self._window_enabled = bool(self.get_parameter("show_classification_window").value) + self._children: list[subprocess.Popen[str]] = [] + + self.create_subscription( + CupDetection, + str(self.get_parameter("detection_topic").value), + self._on_detection, + 10, + ) + self.create_subscription( + Image, + str(self.get_parameter("color_topic").value), + self._on_image, + 10, + ) + + def run(self) -> int: + if not self._confirmed(): + return 2 + + self.get_logger().info("auto cup router: observe -> open -> classify -> route") + perception = None + try: + if not self._move_observe("initial observe"): + return 1 + if not self._open_gripper("initial gripper full-open"): + return 1 + + perception = self._start_perception() + decision = self._wait_for_route_decision() + if decision is None: + self.get_logger().error("route decision failed: no stable upright/lying classification") + return 1 + self._destroy_window() + self._stop_process(perception, "perception") + perception = None + + if decision.route == "side_grasp": + success = self._run_side_grasp(decision) + else: + success = self._run_cup_uprighting(decision) + if not success: + return 1 + self.get_logger().info("auto cup router: selected flow completed; router exiting") + return 0 + finally: + self._stop_process(perception, "perception") + self._stop_all_children() + self._destroy_window() + + def _confirmed(self) -> bool: + if not bool(self.get_parameter("enable_real_motion").value): + self.get_logger().error("enable_real_motion must be true for this router") + return False + token = str(self.get_parameter("router_confirm").value) + if token != "ENABLE_AUTO_CUP_ROUTER": + self.get_logger().error("router_confirm must be ENABLE_AUTO_CUP_ROUTER") + return False + return True + + def _on_detection(self, msg: CupDetection) -> None: + self._latest_detection = msg + + def _on_image(self, msg: Image) -> None: + if not self._window_enabled: + return + try: + image = self._image_to_bgr(msg) + except Exception as exc: + self.get_logger().warn(f"classification window disabled: image conversion failed: {exc}") + self._window_enabled = False + return + with self._image_lock: + self._latest_image = image + + def _start_perception(self) -> subprocess.Popen[str]: + classifier_path = str(self.get_parameter("classifier_path").value) + cmd = self._launch_command(str(self.get_parameter("perception_launch").value)) + cmd.extend([ + f"model_path:={self.get_parameter('yolo_model_path').value}", + f"orientation_classifier_path:={classifier_path}", + f"orientation_classifier_arch:={self.get_parameter('classifier_arch').value}", + f"orientation_classifier_min_confidence:={self.get_parameter('classifier_min_confidence').value}", + ]) + self.get_logger().info("starting perception with cup classifier: " + " ".join(cmd)) + proc = self._popen(cmd, "perception") + return proc + + def _wait_for_route_decision(self) -> Optional[RouteDecision]: + timeout = float(self.get_parameter("route_timeout_sec").value) + required = max(1, int(self.get_parameter("route_stable_required_samples").value)) + stable_min_sec = max(0.0, float(self.get_parameter("route_stable_min_sec").value)) + hold_sec = max(2.0, float(self.get_parameter("route_hold_sec").value)) + end_time = time.monotonic() + timeout + stable_route: Optional[str] = None + stable_since = 0.0 + stable_count = 0 + last_reported_count = 0 + decided: Optional[RouteDecision] = None + hold_until: Optional[float] = None + + self.get_logger().info( + f"waiting for stable route: samples={required}, min_sec={stable_min_sec:.2f}, view_hold={hold_sec:.2f}s" + ) + while rclpy.ok() and time.monotonic() < end_time: + rclpy.spin_once(self, timeout_sec=0.05) + detection = self._latest_detection + if detection is not None: + route = self._route_from_status(detection.status) + if route is not None: + decision = RouteDecision(route, detection.status, float(detection.confidence)) + now = time.monotonic() + if stable_route == route: + stable_count += 1 + else: + stable_route = route + stable_since = now + stable_count = 1 + last_reported_count = 0 + stable_elapsed = now - stable_since + if stable_count >= required and stable_count != last_reported_count: + last_reported_count = stable_count + self.get_logger().info( + f"route candidate stable: {route} samples={stable_count} elapsed={stable_elapsed:.2f}s" + ) + if stable_count >= required and stable_elapsed >= stable_min_sec: + decided = decision + if hold_until is None: + hold_until = time.monotonic() + hold_sec + self.get_logger().info( + f"route decided: {decided.route} confidence={decided.confidence:.3f} status={decided.status}" + ) + + self._show_classification_frame(decided) + if decided is not None and hold_until is not None and time.monotonic() >= hold_until: + return decided + return decided + + @staticmethod + def _route_from_status(status: str) -> Optional[str]: + normalized = status.strip().lower() + if normalized.startswith("detected:upright"): + return "side_grasp" + if normalized.startswith("rejected:lying"): + return "cup_uprighting" + return None + + def _show_classification_frame(self, decision: Optional[RouteDecision]) -> None: + if not self._window_enabled: + return + with self._image_lock: + image = None if self._latest_image is None else self._latest_image.copy() + if image is None: + return + status = self._latest_detection.status if self._latest_detection is not None else "waiting" + route = decision.route if decision else "stabilizing" + cv2.putText(image, f"route: {route}", (20, 36), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 255), 2) + cv2.putText(image, status[:100], (20, 72), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 0), 2) + try: + cv2.imshow(str(self.get_parameter("window_name").value), image) + cv2.waitKey(1) + except Exception as exc: + self.get_logger().warn(f"classification window disabled: {exc}") + self._window_enabled = False + + def _run_side_grasp(self, decision: RouteDecision) -> bool: + self.get_logger().info(f"route=side_grasp: launching existing side grasp flow ({decision.status})") + cmd = self._launch_command(str(self.get_parameter("side_launch").value)) + cmd.extend([ + "auto_pick:=true", + "grasp_mode:=side", + "exit_after_pick:=true", + "move_to_camera_home:=false", + "skip_initial_home_move:=true", + "return_home_after_task:=false", + "return_to_camera_home_after_attempt:=false", + "center_check_enabled:=false", + "redetect_on_approach:=false", + "verify_motion:=true", + "side_fixed_grasp_z_enabled:=true", + "side_fixed_grasp_z:=0.07", + "side_project_bbox_center_to_fixed_z:=true", + "min_motion_z:=0.07", + "side_candidate_plan_check_enabled:=true", + "side_far_stage_enabled:=false", + "side_short_stage_backoff_m:=0.08", + "side_approach_offset:=0.18", + "side_grasp_stop_backoff_m:=0.04", + "side_close_underreach_m:=0.03", + "side_final_slide_enabled:=false", + "side_move_to_initial_center_before_close:=false", + "side_linear_approach_enabled:=true", + "side_low_retry_lift_m:=0.03", + "side_low_retry_attempts:=5", + "workspace_xy_clamp_enabled:=false", + "table_collision_enabled:=true", + "table_surface_z:=0.0", + "table_thickness:=0.04", + "table_size_x:=1.10", + "table_size_y:=0.65", + "table_center_x:=0.29", + "table_center_y:=0.0", + "dispenser_collision_enabled:=true", + f"moveit_controller_name:={self.get_parameter('moveit_controller_name').value}", + "start_joint_state_relay:=false", + f"model_path:={self.get_parameter('yolo_model_path').value}", + ]) + cmd.extend(self._split_extra_args(str(self.get_parameter("side_extra_args").value))) + return self._run_process(cmd, "side_grasp") + + def _run_cup_uprighting(self, decision: RouteDecision) -> bool: + self.get_logger().info(f"route=cup_uprighting: launching optimized cup-uprighting flow ({decision.status})") + cmd = self._launch_command(str(self.get_parameter("cup_uprighting_launch").value)) + cmd.extend([ + "auto_pick:=true", + "exit_after_pick:=true", + "skip_initial_home_move:=true", + f"model_path:={self.get_parameter('yolo_model_path').value}", + f"moveit_controller_name:={self.get_parameter('moveit_controller_name').value}", + f"controller_action_name:={self.get_parameter('controller_action_name').value}", + ]) + cmd.extend(self._split_extra_args(str(self.get_parameter("cup_uprighting_extra_args").value))) + return self._run_process(cmd, "cup_uprighting") + + def _move_observe(self, label: str) -> bool: + prefix = str(self.get_parameter("service_prefix").value).strip().strip("/") + base = f"/{prefix}/motion" if prefix else "/motion" + service = f"{base}/move_joint" + wait_service = f"{base}/move_wait" + client = self.create_client(MoveJoint, service) + timeout = float(self.get_parameter("motion_timeout_sec").value) + if not client.wait_for_service(timeout_sec=5.0): + self.get_logger().error(f"{label}: service unavailable: {service}") + return False + req = MoveJoint.Request() + req.pos = [float(v) for v in self.get_parameter("observe_joints_deg").value] + req.vel = float(self.get_parameter("observe_vel").value) + req.acc = float(self.get_parameter("observe_acc").value) + req.time = float(self.get_parameter("observe_time").value) + req.radius = 0.0 + req.mode = 0 + req.blend_type = 0 + req.sync_type = 0 + self.get_logger().info(f"{label}: MoveJoint via {service}: " + ", ".join(f"{v:.1f}" for v in req.pos)) + future = client.call_async(req) + if not self._spin_future(future, timeout): + self.get_logger().error(f"{label}: MoveJoint timed out after {timeout:.1f}s") + return False + if not bool(future.result().success): + self.get_logger().error(f"{label}: MoveJoint returned failure") + return False + wait_client = self.create_client(MoveWait, wait_service) + if wait_client.wait_for_service(timeout_sec=2.0): + wait_future = wait_client.call_async(MoveWait.Request()) + if self._spin_future(wait_future, timeout) and bool(wait_future.result().success): + self.get_logger().info(f"{label}: MoveWait completed") + return True + + def _open_gripper(self, label: str) -> bool: + service = str(self.get_parameter("gripper_open_service").value) + client = self.create_client(Trigger, service) + if not client.wait_for_service(timeout_sec=5.0): + self.get_logger().error(f"{label}: service unavailable: {service}") + return False + self.get_logger().info(f"{label}: opening RG2 via {service}") + future = client.call_async(Trigger.Request()) + if not self._spin_future(future, 8.0): + self.get_logger().error(f"{label}: RG2 open timed out") + return False + result = future.result() + if not bool(result.success): + self.get_logger().error(f"{label}: RG2 open failed: {result.message}") + return False + self.get_logger().info(f"{label}: {result.message}") + return True + + def _spin_future(self, future, timeout_sec: float) -> bool: + end_time = time.monotonic() + timeout_sec + while rclpy.ok() and not future.done() and time.monotonic() < end_time: + rclpy.spin_once(self, timeout_sec=0.05) + return future.done() + + @staticmethod + def _launch_command(spec: str) -> list[str]: + parts = spec.split() + if len(parts) != 2: + raise ValueError(f"launch spec must be ' ': {spec!r}") + return ["ros2", "launch", parts[0], parts[1]] + + @staticmethod + def _split_extra_args(raw: str) -> list[str]: + return [part for part in raw.split() if part] + + def _popen(self, cmd: list[str], label: str) -> subprocess.Popen[str]: + proc = subprocess.Popen( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + bufsize=1, + preexec_fn=os.setsid, + ) + self._children.append(proc) + threading.Thread(target=self._forward_output, args=(proc, label), daemon=True).start() + return proc + + def _run_process(self, cmd: list[str], label: str) -> bool: + self.get_logger().info(f"{label}: " + " ".join(cmd)) + proc = self._popen(cmd, label) + code = proc.wait() + if code == 0: + self.get_logger().info(f"{label}: completed successfully") + return True + self.get_logger().error(f"{label}: process exited with code {code}") + return False + + def _forward_output(self, proc: subprocess.Popen[str], label: str) -> None: + if proc.stdout is None: + return + for line in proc.stdout: + self.get_logger().info(f"{label}> {line.rstrip()}") + + def _stop_process(self, proc: Optional[subprocess.Popen[str]], label: str) -> None: + if proc is None or proc.poll() is not None: + return + self.get_logger().info(f"stopping {label}") + try: + os.killpg(os.getpgid(proc.pid), signal.SIGINT) + proc.wait(timeout=5.0) + except Exception: + try: + os.killpg(os.getpgid(proc.pid), signal.SIGTERM) + except Exception: + pass + + def _stop_all_children(self) -> None: + for proc in list(self._children): + self._stop_process(proc, "child") + + def _destroy_window(self) -> None: + if not self._window_enabled: + return + try: + cv2.destroyWindow(str(self.get_parameter("window_name").value)) + except Exception: + pass + + @staticmethod + def _image_to_bgr(msg: Image) -> np.ndarray: + dtype = np.uint8 if msg.encoding.lower() in {"rgb8", "bgr8", "8uc3"} else np.uint8 + channels = 3 + array = np.frombuffer(msg.data, dtype=dtype).reshape((msg.height, msg.width, channels)) + if msg.encoding.lower() == "rgb8": + return cv2.cvtColor(array, cv2.COLOR_RGB2BGR) + return array.copy() + + +def main(args: Optional[list[str]] = None) -> None: + rclpy.init(args=args) + node = AutoCupFlowRouter() + try: + code = node.run() + finally: + node.destroy_node() + rclpy.shutdown() + sys.exit(code) + + +if __name__ == "__main__": + main() diff --git a/src/azas_task_manager/package.xml b/src/azas_task_manager/package.xml index 0b25c0a..25d7a7c 100644 --- a/src/azas_task_manager/package.xml +++ b/src/azas_task_manager/package.xml @@ -8,7 +8,10 @@ rclpy azas_interfaces + dsr_msgs2 + sensor_msgs std_msgs + std_srvs ament_python diff --git a/src/azas_task_manager/setup.py b/src/azas_task_manager/setup.py index 2e4312a..64acd4d 100644 --- a/src/azas_task_manager/setup.py +++ b/src/azas_task_manager/setup.py @@ -18,6 +18,7 @@ license="MIT", entry_points={ "console_scripts": [ + "auto_cup_flow_router = azas_task_manager.auto_cup_flow_router:main", "pick_and_align_action_server = azas_task_manager.pick_and_align_action_server:main", "cocktail_dryrun_sequence_node = azas_task_manager.cocktail_dryrun_sequence_node:main", ], From 1a3b3258ad1724cfa06e108c6c54b7241e4336bf Mon Sep 17 00:00:00 2001 From: chris3471 Date: Thu, 11 Jun 2026 15:09:15 +0900 Subject: [PATCH 64/88] Improve cup route classifier overlay --- .../launch/auto_cup_flow_router.launch.py | 2 +- .../azas_task_manager/auto_cup_flow_router.py | 143 +++++++++++++++++- 2 files changed, 139 insertions(+), 6 deletions(-) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index 0a74653..a2cf835 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -17,7 +17,7 @@ def generate_launch_description(): DeclareLaunchArgument("classifier_arch", default_value="resnet18"), DeclareLaunchArgument("classifier_min_confidence", default_value="0.70"), DeclareLaunchArgument("route_timeout_sec", default_value="30.0"), - DeclareLaunchArgument("route_hold_sec", default_value="2.0"), + DeclareLaunchArgument("route_hold_sec", default_value="3.5"), DeclareLaunchArgument("route_stable_required_samples", default_value="5"), DeclareLaunchArgument("route_stable_min_sec", default_value="0.8"), DeclareLaunchArgument("show_classification_window", default_value="true"), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 0270820..302fef2 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -1,6 +1,7 @@ from __future__ import annotations import os +import re import signal import subprocess import sys @@ -26,6 +27,17 @@ class RouteDecision: confidence: float +@dataclass(frozen=True) +class DetectionOverlay: + center_u: int + center_v: int + width: int + height: int + orientation: str + class_name: str + classifier_confidence: Optional[float] + + class AutoCupFlowRouter(Node): def __init__(self) -> None: super().__init__("auto_cup_flow_router") @@ -50,7 +62,7 @@ def __init__(self) -> None: self.declare_parameter("route_timeout_sec", 30.0) self.declare_parameter("route_stable_required_samples", 5) self.declare_parameter("route_stable_min_sec", 0.8) - self.declare_parameter("route_hold_sec", 2.0) + self.declare_parameter("route_hold_sec", 3.5) self.declare_parameter("show_classification_window", True) self.declare_parameter("window_name", "Azas cup route classifier") @@ -97,7 +109,6 @@ def run(self) -> int: if decision is None: self.get_logger().error("route decision failed: no stable upright/lying classification") return 1 - self._destroy_window() self._stop_process(perception, "perception") perception = None @@ -156,7 +167,7 @@ def _wait_for_route_decision(self) -> Optional[RouteDecision]: timeout = float(self.get_parameter("route_timeout_sec").value) required = max(1, int(self.get_parameter("route_stable_required_samples").value)) stable_min_sec = max(0.0, float(self.get_parameter("route_stable_min_sec").value)) - hold_sec = max(2.0, float(self.get_parameter("route_hold_sec").value)) + hold_sec = max(3.0, float(self.get_parameter("route_hold_sec").value)) end_time = time.monotonic() + timeout stable_route: Optional[str] = None stable_since = 0.0 @@ -220,8 +231,10 @@ def _show_classification_frame(self, decision: Optional[RouteDecision]) -> None: return status = self._latest_detection.status if self._latest_detection is not None else "waiting" route = decision.route if decision else "stabilizing" - cv2.putText(image, f"route: {route}", (20, 36), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 255), 2) - cv2.putText(image, status[:100], (20, 72), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 0), 2) + overlay = self._overlay_from_status(status) + if overlay is not None: + self._draw_detection_overlay(image, overlay, route) + self._draw_text_panel(image, route, status) try: cv2.imshow(str(self.get_parameter("window_name").value), image) cv2.waitKey(1) @@ -229,6 +242,126 @@ def _show_classification_frame(self, decision: Optional[RouteDecision]) -> None: self.get_logger().warn(f"classification window disabled: {exc}") self._window_enabled = False + @staticmethod + def _overlay_from_status(status: str) -> Optional[DetectionOverlay]: + bbox_match = re.search(r"bbox=(\d+)x(\d+)", status) + center_match = re.search(r"center=\((\d+),(\d+)\)", status) + orientation_match = re.search(r"orientation=([^\s]+)", status) + if bbox_match is None or center_match is None: + return None + return DetectionOverlay( + center_u=int(center_match.group(1)), + center_v=int(center_match.group(2)), + width=int(bbox_match.group(1)), + height=int(bbox_match.group(2)), + orientation=orientation_match.group(1) if orientation_match else "unknown", + class_name=AutoCupFlowRouter._status_field(status, "class") or "cup", + classifier_confidence=AutoCupFlowRouter._status_float( + status, + "orientation_classifier_confidence", + ), + ) + + @staticmethod + def _status_field(status: str, key: str) -> Optional[str]: + match = re.search(rf"{re.escape(key)}=([^\s]+)", status) + return match.group(1) if match else None + + @staticmethod + def _status_float(status: str, key: str) -> Optional[float]: + value = AutoCupFlowRouter._status_field(status, key) + if value is None: + return None + try: + return float(value) + except ValueError: + return None + + @staticmethod + def _draw_detection_overlay(image: np.ndarray, overlay: DetectionOverlay, route: str) -> None: + image_h, image_w = image.shape[:2] + x1 = max(0, overlay.center_u - overlay.width // 2) + y1 = max(0, overlay.center_v - overlay.height // 2) + x2 = min(image_w - 1, overlay.center_u + overlay.width // 2) + y2 = min(image_h - 1, overlay.center_v + overlay.height // 2) + color = (50, 210, 90) if route == "side_grasp" or overlay.orientation == "upright" else (0, 150, 255) + if route == "stabilizing": + color = (0, 220, 255) + AutoCupFlowRouter._draw_corner_box(image, x1, y1, x2, y2, color) + cv2.circle(image, (overlay.center_u, overlay.center_v), 5, color, -1) + conf = "" if overlay.classifier_confidence is None else f" {overlay.classifier_confidence:.2f}" + label = f"{overlay.orientation}{conf} -> {route}" + AutoCupFlowRouter._draw_label(image, label, x1, max(30, y1 - 34), color) + + @staticmethod + def _draw_corner_box(image: np.ndarray, x1: int, y1: int, x2: int, y2: int, color: tuple[int, int, int]) -> None: + thickness = 3 + corner = max(18, min((x2 - x1) // 4, (y2 - y1) // 4, 44)) + cv2.rectangle(image, (x1, y1), (x2, y2), color, 1) + for start, end in [ + ((x1, y1), (x1 + corner, y1)), + ((x1, y1), (x1, y1 + corner)), + ((x2, y1), (x2 - corner, y1)), + ((x2, y1), (x2, y1 + corner)), + ((x1, y2), (x1 + corner, y2)), + ((x1, y2), (x1, y2 - corner)), + ((x2, y2), (x2 - corner, y2)), + ((x2, y2), (x2, y2 - corner)), + ]: + cv2.line(image, start, end, color, thickness) + + @staticmethod + def _draw_label(image: np.ndarray, text: str, x: int, y: int, color: tuple[int, int, int]) -> None: + font = cv2.FONT_HERSHEY_SIMPLEX + scale = 0.62 + thickness = 2 + (text_w, text_h), baseline = cv2.getTextSize(text, font, scale, thickness) + x2 = min(image.shape[1] - 8, x + text_w + 18) + y1 = max(8, y - text_h - 10) + cv2.rectangle(image, (x, y1), (x2, y + baseline + 8), color, -1) + cv2.putText(image, text, (x + 9, y), font, scale, (20, 20, 20), thickness) + + @staticmethod + def _draw_text_panel(image: np.ndarray, route: str, status: str) -> None: + panel = image.copy() + margin = 14 + panel_h = 96 + cv2.rectangle(panel, (margin, margin), (image.shape[1] - margin, panel_h), (12, 16, 20), -1) + cv2.addWeighted(panel, 0.78, image, 0.22, 0, image) + + route_color = (50, 210, 90) if route == "side_grasp" else (0, 150, 255) + if route == "stabilizing": + route_color = (0, 220, 255) + AutoCupFlowRouter._draw_pill(image, route.replace("_", " ").upper(), margin + 14, 48, route_color) + + orientation = AutoCupFlowRouter._status_field(status, "orientation") or "waiting" + cls = AutoCupFlowRouter._status_field(status, "class") or "cup" + conf = AutoCupFlowRouter._status_float(status, "orientation_classifier_confidence") + conf_text = "--" if conf is None else f"{conf:.2f}" + detail = f"{cls} / {orientation} / classifier {conf_text}" + cv2.putText(image, detail, (margin + 210, 46), cv2.FONT_HERSHEY_SIMPLEX, 0.62, (235, 245, 245), 2) + + compact_status = AutoCupFlowRouter._compact_status(status) + cv2.putText(image, compact_status, (margin + 18, 78), cv2.FONT_HERSHEY_SIMPLEX, 0.48, (170, 225, 205), 1) + + @staticmethod + def _draw_pill(image: np.ndarray, text: str, x: int, y: int, color: tuple[int, int, int]) -> None: + font = cv2.FONT_HERSHEY_SIMPLEX + scale = 0.56 + thickness = 2 + (text_w, text_h), baseline = cv2.getTextSize(text, font, scale, thickness) + cv2.rectangle(image, (x, y - text_h - 12), (x + text_w + 28, y + baseline + 10), color, -1) + cv2.putText(image, text, (x + 14, y), font, scale, (18, 22, 24), thickness) + + @staticmethod + def _compact_status(status: str) -> str: + parts = [] + for key in ["center", "orientation_classifier_result"]: + value = AutoCupFlowRouter._status_field(status, key) + if value: + parts.append(f"{key}={value}") + return " ".join(parts) if parts else status[:120] + def _run_side_grasp(self, decision: RouteDecision) -> bool: self.get_logger().info(f"route=side_grasp: launching existing side grasp flow ({decision.status})") cmd = self._launch_command(str(self.get_parameter("side_launch").value)) From dc5c5ba55b8d41cbd966b0ed461666b505ffc6b7 Mon Sep 17 00:00:00 2001 From: chris3471 Date: Thu, 11 Jun 2026 15:28:49 +0900 Subject: [PATCH 65/88] Fix cup uprighting auto execution --- .../yolo_cup_uprighting_node.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py index d79b21f..4379a59 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/yolo_cup_uprighting_node.py @@ -169,8 +169,8 @@ def detect_and_pick(self, frame: np.ndarray): self.picking = True try: # feature 브랜치의 핵심 목표인 홈 복귀 시퀀스 직접 호출 - self._pick_and_return_home(bx, by, bz, cup_theta) - 诚然: + return self._pick_and_return_home(bx, by, bz, cup_theta) + finally: self.picking = False def _pick_and_return_home(self, bx, by, bz, cup_theta): @@ -205,19 +205,19 @@ def _pick_and_return_home(self, bx, by, bz, cup_theta): # 1-1 단계 실패 시 예외 처리 및 탈출 if not self.plan_pose(bx, by, safe_z, current_ori): log.error("[1-1] 상공 진입 실패. 시퀀스 중단.") - return + return False time.sleep(1.0) log.info("[1-2] 상공에서 파지 방향 정렬") if not self.plan_pose(bx, by, safe_z, target_ori): log.error("[1-2] 방향 정렬 실패. 시퀀스 중단.") - return + return False time.sleep(1.0) log.info("[2] 컵 파지 위치 하강") if not self.plan_pose(bx, by, pick_z, target_ori): log.error("[2] 파지 위치 하강 실패. 시퀀스 중단.") - return + return False self.gripper.close_gripper() log.info("[2] 그리퍼 클로즈 완료") @@ -228,14 +228,16 @@ def _pick_and_return_home(self, bx, by, bz, cup_theta): log.error("[3] 리프트업 실패. 물체 탈락 위험으로 인한 안전 복구 가동.") self.gripper.open_gripper() log.info("=> 그리퍼 비상 강제 릴리즈 완료.") - return + return False time.sleep(1.0) log.info("[4] 홈 위치로 복귀 (파지 유지)") if self.go_home_pose(): log.info("=> 홈 복귀 성공. 전체 구출 시퀀스 완수.") + return True else: log.error("=> [치명적] 파지는 완료했으나 관절 한계 혹은 충돌 궤적으로 인해 홈 복귀 실패.") + return False def main(args=None): @@ -243,4 +245,4 @@ def main(args=None): if __name__ == "__main__": - main() \ No newline at end of file + main() From f08ad8fab5de4f966d8f7559f03de1d3c50dc276 Mon Sep 17 00:00:00 2001 From: chris3471 Date: Thu, 11 Jun 2026 15:51:38 +0900 Subject: [PATCH 66/88] Fix lid marker merge conflict markers --- .../azas_perception/lid_marker.py | 64 ++----------------- 1 file changed, 6 insertions(+), 58 deletions(-) diff --git a/src/azas_perception/azas_perception/lid_marker.py b/src/azas_perception/azas_perception/lid_marker.py index 1dbbda4..aefcf69 100644 --- a/src/azas_perception/azas_perception/lid_marker.py +++ b/src/azas_perception/azas_perception/lid_marker.py @@ -144,13 +144,13 @@ def detect_aruco_marker( gray = cv2.cvtColor(patch, cv2.COLOR_BGR2GRAY) dictionary = _create_aruco_dictionary(dictionary_id) -<<<<<<< refactor/rg2_collision2 if dictionary is None: return None parameters = _create_aruco_detector_parameters() # The lid marker appears small and oblique in the wrist-camera view. Try - # conservative contrast/scale variants while keeping dictionary/id strict. + # conservative contrast/scale variants, but keep the dictionary/id filter + # strict so a noisy table feature cannot become a false lid marker. best: ArucoMarker | None = None best_score = -1.0 for candidate_gray, scale in _aruco_detection_images(gray): @@ -159,17 +159,6 @@ def detect_aruco_marker( dictionary, parameters, ) -======= - parameters = _create_aruco_detector_parameters() - - # The lid marker appears small and oblique in the wrist-camera view. Try - # conservative contrast/scale variants, but keep the dictionary/id filter - # strict so a noisy table feature cannot become a false lid marker. - best: ArucoMarker | None = None - best_score = -1.0 - for candidate_gray, scale in _aruco_detection_images(gray): - corners_list, ids, _rejected = _detect_aruco_markers(candidate_gray, dictionary, parameters) ->>>>>>> develop candidate = _select_aruco_marker_from_detections( corners_list, ids, @@ -184,18 +173,14 @@ def detect_aruco_marker( def _aruco_dictionary_id(dictionary_name: str) -> int | None: -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) if aruco is None: return None -======= ->>>>>>> develop name = str(dictionary_name).strip().upper() if not name: return None if not name.startswith("DICT_"): name = f"DICT_{name}" -<<<<<<< refactor/rg2_collision2 return getattr(aruco, name, None) @@ -220,33 +205,16 @@ def _create_aruco_detector_parameters(): parameters = aruco.DetectorParameters_create() else: return None -======= - return getattr(cv2.aruco, name, None) - - -def _create_aruco_dictionary(dictionary_id: int): - if hasattr(cv2.aruco, "getPredefinedDictionary"): - return cv2.aruco.getPredefinedDictionary(dictionary_id) - return cv2.aruco.Dictionary_get(dictionary_id) - - -def _create_aruco_detector_parameters(): - if hasattr(cv2.aruco, "DetectorParameters"): - parameters = cv2.aruco.DetectorParameters() - else: - parameters = cv2.aruco.DetectorParameters_create() ->>>>>>> develop return _tune_lid_aruco_detector_parameters(parameters) def _tune_lid_aruco_detector_parameters(parameters): -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) -======= # The lid marker is small in the wrist-camera overview image and often seen # at an angle. Keep the expected marker-id filter strict, but make candidate # extraction and perspective sampling tolerant enough for the measured setup. ->>>>>>> develop + if parameters is None: + return None tuned_values = { "adaptiveThreshWinSizeMin": 3, "adaptiveThreshWinSizeMax": 53, @@ -258,11 +226,7 @@ def _tune_lid_aruco_detector_parameters(parameters): "perspectiveRemovePixelPerCell": 8, "perspectiveRemoveIgnoredMarginPerCell": 0.20, "errorCorrectionRate": 0.75, -<<<<<<< refactor/rg2_collision2 "cornerRefinementMethod": getattr(aruco, "CORNER_REFINE_SUBPIX", 1), -======= - "cornerRefinementMethod": getattr(cv2.aruco, "CORNER_REFINE_SUBPIX", 1), ->>>>>>> develop "cornerRefinementWinSize": 3, } for name, value in tuned_values.items(): @@ -272,9 +236,6 @@ def _tune_lid_aruco_detector_parameters(parameters): def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: -<<<<<<< refactor/rg2_collision2 - """Return grayscale variants for small/low-contrast lid ArUco detection.""" -======= """Return grayscale variants for small/low-contrast lid ArUco detection. OpenCV returns corners in the coordinate system of the image it receives, @@ -282,7 +243,6 @@ def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: ROI. Variants are intentionally limited to deterministic contrast/scale transforms; no dictionary or marker-id relaxation is performed. """ ->>>>>>> develop variants: list[tuple[np.ndarray, float]] = [(gray, 1.0)] equalized = cv2.equalizeHist(gray) variants.append((equalized, 1.0)) @@ -291,23 +251,17 @@ def _aruco_detection_images(gray: np.ndarray) -> list[tuple[np.ndarray, float]]: sharpened = cv2.addWeighted(gray, 1.6, blur, -0.6, 0) variants.append((sharpened, 1.0)) -<<<<<<< refactor/rg2_collision2 + # Upscaling materially helps when the marker body is only a few tens of + # pixels wide in the RealSense overview frame. for source in (gray, equalized, sharpened): variants.append(( cv2.resize(source, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_CUBIC), 2.0, )) -======= - # Upscaling materially helps when the marker body is only a few tens of - # pixels wide in the RealSense overview frame. - for source in (gray, equalized, sharpened): - variants.append((cv2.resize(source, None, fx=2.0, fy=2.0, interpolation=cv2.INTER_CUBIC), 2.0)) ->>>>>>> develop return variants def _detect_aruco_markers(gray: np.ndarray, dictionary, parameters): -<<<<<<< refactor/rg2_collision2 aruco = getattr(cv2, "aruco", None) if aruco is None: return [], None, [] @@ -318,12 +272,6 @@ def _detect_aruco_markers(gray: np.ndarray, dictionary, parameters): kwargs = {"parameters": parameters} if parameters is not None else {} return aruco.detectMarkers(gray, dictionary, **kwargs) return [], None, [] -======= - if hasattr(cv2.aruco, "ArucoDetector"): - detector = cv2.aruco.ArucoDetector(dictionary, parameters) - return detector.detectMarkers(gray) - return cv2.aruco.detectMarkers(gray, dictionary, parameters=parameters) ->>>>>>> develop def _select_aruco_marker_from_detections( From 4244253638032b50d6780db0f6094c2f18ed7613 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Thu, 11 Jun 2026 20:14:38 +0900 Subject: [PATCH 67/88] Refactor RG2 Gripper Configuration and Enhance Side Grip Logic - Updated RG2 launch files to disable gripper collision publishing for improved performance. - Changed default mount orientation for RG2 in URDF and launch files to identity (0, 0, 0). - Introduced a new parameter `side_target_x_offset_m` in the YoloCupPickNode for side-grip motion planning. - Enhanced side-grip logic to apply target offset during planning and added warnings for offset usage. - Updated launch files to include the new parameter and adjusted related commands for consistency. - Added a new script to wait for lid grip status, ensuring proper sequence execution after lid closure. - Improved error handling and logging in various scripts to enhance debugging and user feedback. --- docs/robot_pipeline_control.html | 26 +-- .../launch/rg2_link6_tcp.launch.py | 2 +- .../view_m0609_rg2_parametric.launch.py | 2 +- .../urdf/m0609_rg2_parametric.urdf.xacro | 2 +- .../dsr_practice/yolo_cup_pick_node.py | 28 +++- .../launch/yolo_cup_pick_node.launch.py | 62 +++++-- .../check_panel_service_discovery_race.py | 16 ++ tools/checks/check_rg2_moveit_description.py | 45 +++++- tools/run/robot_pipeline_control_server.py | 152 ++++++++++++------ tools/run/run_changhyun_side_grip_direct.sh | 14 +- .../run_course_dispenser_press_cycle_rviz.sh | 4 +- tools/run/run_tmux_logic_sequence.sh | 9 +- tools/run/wait_for_lid_grip_status.py | 93 +++++++++++ 13 files changed, 352 insertions(+), 103 deletions(-) create mode 100755 tools/run/wait_for_lid_grip_status.py diff --git a/docs/robot_pipeline_control.html b/docs/robot_pipeline_control.html index 2e9cf3b..003b3e4 100644 --- a/docs/robot_pipeline_control.html +++ b/docs/robot_pipeline_control.html @@ -841,20 +841,21 @@

Azas Robot Pipeline Control

-
현재 시험 순서: 연결 준비색상 JSON창현 side-grip 성공 후 자동 디스펜서 → 성공 확인 후 ArUco 뚜껑 체결. 전체 묶기는 창현/소명 선택 확정 전까지 보류합니다.
+
현재 후보 순서: tmux 연결 준비수빈 STT/주문 UI색상 스캔창현 side-grip / 소명 누운컵(현재 점검 필요)통합 디스펜서강모 ArUco 뚜껑 체결쉐이킹.
- - - - + + + + +
@@ -1103,8 +1104,6 @@

RealSense 카메라 화면

"run_cocktail_now_real", "start_camera_view", "detect_cup_lid", - "voice_input", - "listen_stt_recipe", "run_one_click_cocktail_real", "move_to_dispenser_1", "move_to_dispenser_2", @@ -1119,9 +1118,10 @@

RealSense 카메라 화면

"pick_from_dispenser_3", "pick_from_dispenser_4", ]); - const groupOrder = ["core", "grip", "move", "press", "shake", "blocked", "etc"]; + const groupOrder = ["core", "voice", "grip", "move", "press", "shake", "blocked", "etc"]; const groupLabels = { core: "연결/준비", + voice: "수빈 STT/주문 UI", grip: "그리퍼", move: "컵 이동/배치", press: "통합 디스펜서 (컵놓기→프레스→다시잡기)", @@ -1175,6 +1175,7 @@

RealSense 카메라 화면

function stepGroup(step) { const key = step.key || ""; if (core.has(key)) return "core"; + if (key.includes("voice") || key.includes("stt")) return "voice"; if (key === "start_camera" || key === "detect_cup_lid" || key.includes("color_scan")) return "camera"; if (key.includes("gripper")) return "grip"; if (key.startsWith("teach_front_hold_")) return "teach"; @@ -1912,8 +1913,11 @@

RealSense 카메라 화면

document.getElementById("startPrepBtn")?.addEventListener("click", () => { queuePrepBundle(); }); + document.getElementById("sttRecipeBtn")?.addEventListener("click", () => { + queueOnly(["voice_input", "listen_stt_recipe"], "수빈 STT/주문 UI 후보를 큐에 추가했습니다. voice screen에서 메뉴를 말하거나 입력한 뒤 '응'으로 확정하면 latest_recipe.json을 저장합니다."); + }); document.getElementById("cupUprightingBtn")?.addEventListener("click", () => { - queueOnly(["cup_uprighting"], "2단계 소명/누운 컵 직립화 로직만 단독으로 큐에 추가했습니다. 1단계 성공 확인 후 실행하세요. OpenCV 창에서 컵을 확인한 뒤 p 키로 실행합니다."); + queueOnly(["cup_uprighting"], "소명/누운 컵 직립화 후보를 큐에 추가했습니다. 현재 yolo_cup_uprighting_node.py 문법 오류가 있어 수정 전에는 실행 실패합니다."); }); document.getElementById("sideGripBtn")?.addEventListener("click", () => { queueOnly(["side_grip"], "창현/PR #20 RealSense side-grip을 큐에 추가했습니다. OpenCV 창에서 p 키로 컵 잡기가 성공하면 통합 디스펜서 레시피가 자동 실행됩니다."); @@ -1922,7 +1926,7 @@

RealSense 카메라 화면

queueOnly(["start_camera", "pick_lid"], "뚜껑 grip pose 계획 로직을 큐에 추가했습니다. 실제 로봇 모션은 실행하지 않습니다."); }); document.getElementById("lidGripCloseBtn")?.addEventListener("click", () => { - queueOnly(["lid_grip_close"], "ArUco 뚜껑 체결 시험을 큐에 추가했습니다. side-grip 후 통합 디스펜서가 성공한 다음 실행하세요. 스크립트가 뚜껑 보기 자세로 이동한 뒤 ArUco 확인/p 키 흐름으로 진행합니다."); + queueOnly(["lid_grip_close"], "ArUco 뚜껑 체결 시험을 큐에 추가했습니다. 뚜껑 보기 자세 이동 뒤 ArUco 확인/p 키로 성공하면 컵홀더 컵 재픽업→쉐이킹으로 바로 이어집니다."); }); document.getElementById("colorScanJsonBtn")?.addEventListener("click", () => { @@ -1942,7 +1946,7 @@

RealSense 카메라 화면

queueOnly(["place_cup_holder"], "컵홀더 배치를 큐에 추가했습니다. 이 단계는 MoveItPy 경로계획으로 pre_place→place_final→RG2 open→retreat를 실행합니다."); }); document.getElementById("fullCocktailRealBtn")?.addEventListener("click", async () => { - log.textContent = "전체 묶기는 창현/소명 side-grip 선택이 확정될 때까지 보류합니다. 지금 시험은 색상 JSON → 창현 side-grip 성공 후 자동 디스펜서 → ArUco 뚜껑 체결 순서로 단독 버튼을 사용하세요."; + log.textContent = "현재 후보 흐름은 tmux 연결 준비 → 수빈 STT/주문 UI → 색상 스캔 → 창현 side-grip 또는 소명 누운컵 → 통합 디스펜서 → 강모 ArUco 뚜껑 체결 → 쉐이킹입니다. 단, 소명 누운컵은 현재 문법 오류로 점검 필요합니다."; focusLog(); }); document.getElementById("run").addEventListener("click", async () => { diff --git a/src/azas_bringup/launch/rg2_link6_tcp.launch.py b/src/azas_bringup/launch/rg2_link6_tcp.launch.py index c562bc0..a268add 100644 --- a/src/azas_bringup/launch/rg2_link6_tcp.launch.py +++ b/src/azas_bringup/launch/rg2_link6_tcp.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): [ DeclareLaunchArgument("open_tcp_offset_m", default_value="0.15"), DeclareLaunchArgument("closed_tcp_offset_m", default_value="0.25"), - DeclareLaunchArgument("publish_gripper_collision", default_value="true"), + DeclareLaunchArgument("publish_gripper_collision", default_value="false"), Node( package="robot_state_publisher", executable="robot_state_publisher", diff --git a/src/azas_description/launch/view_m0609_rg2_parametric.launch.py b/src/azas_description/launch/view_m0609_rg2_parametric.launch.py index 6726106..3382337 100644 --- a/src/azas_description/launch/view_m0609_rg2_parametric.launch.py +++ b/src/azas_description/launch/view_m0609_rg2_parametric.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): [ DeclareLaunchArgument( "rg2_mount_rpy", - default_value="1.570796327 0 1.570796327", + default_value="0 0 0", ), Node( package="robot_state_publisher", diff --git a/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro b/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro index 6bb1041..56eaf1e 100644 --- a/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro +++ b/src/azas_description/urdf/m0609_rg2_parametric.urdf.xacro @@ -3,7 +3,7 @@ - + diff --git a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py index bbdd73c..819822b 100644 --- a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -225,6 +225,7 @@ def __init__(self): self.declare_parameter("side_short_stage_backoff_m", 0.06) self.declare_parameter("side_stage_y_min", SAFE_Y_MIN) self.declare_parameter("side_stage_y_max", SAFE_Y_MAX) + self.declare_parameter("side_target_x_offset_m", 0.0) self.declare_parameter("side_grasp_offset", 0.035) self.declare_parameter("side_grasp_z_offset", 0.05) self.declare_parameter("side_grasp_stop_backoff_m", 0.04) @@ -405,6 +406,9 @@ def __init__(self): ) self.side_stage_y_min = float(self.get_parameter("side_stage_y_min").value) self.side_stage_y_max = float(self.get_parameter("side_stage_y_max").value) + self.side_target_x_offset_m = float( + self.get_parameter("side_target_x_offset_m").value + ) self.side_grasp_offset = float(self.get_parameter("side_grasp_offset").value) self.side_grasp_z_offset = float( self.get_parameter("side_grasp_z_offset").value @@ -640,6 +644,11 @@ def __init__(self): "side_fixed_grasp_z is interpreted as a base_link Z target for " f"{EE_LINK}; table/cup/lid geometry is not inferred from it." ) + if abs(self.side_target_x_offset_m) > 1e-6: + self.get_logger().warning( + "side_target_x_offset_m applies only to side-grip motion planning; " + f"detected cup poses are left unchanged (offset={self.side_target_x_offset_m:.3f} m)." + ) if not self.table_collision_enabled: self.get_logger().warning( "table_collision_enabled=false: MoveIt will only clamp the EE target Z, " @@ -1839,6 +1848,20 @@ def log_side_grasp_plan(self, plan: SideGraspPlan, prefix="Side grasp target"): def side_final_approach_params(self): return self.pilz_lin_params if self.side_linear_approach_enabled else self.pilz_params + def apply_side_target_offset(self, cup_base_xyz): + adjusted = np.array([float(v) for v in cup_base_xyz], dtype=float) + if abs(self.side_target_x_offset_m) <= 1e-6: + return adjusted + raw_x = float(adjusted[0]) + adjusted[0] = raw_x + self.side_target_x_offset_m + self.get_logger().info( + "Side target X compensation: " + f"detected_x={raw_x:.3f} m, " + f"offset={self.side_target_x_offset_m:.3f} m, " + f"planning_x={adjusted[0]:.3f} m" + ) + return adjusted + def spin_for_camera_update(self, duration_sec): end_time = time.time() + max(0.0, duration_sec) while rclpy.ok() and time.time() < end_time: @@ -2031,7 +2054,8 @@ def pick_and_place_side(self, base_xyz): refined_base = self.center_check_redetect(initial_base) cup_base = initial_base if refined_base is None else np.array(refined_base, dtype=float) - candidates = self.build_side_grasp_candidates(cup_base) + planning_cup_base = self.apply_side_target_offset(cup_base) + candidates = self.build_side_grasp_candidates(planning_cup_base) if not candidates: log.error("No side-grasp candidates generated") return False @@ -2044,7 +2068,7 @@ def pick_and_place_side(self, base_xyz): f"close=({candidate.guarded_grasp_xy[0]:.3f}, {candidate.guarded_grasp_xy[1]:.3f}, {candidate.pre_z:.3f})" ) - if not self.move_to_side_prepose_if_configured(cup_base): + if not self.move_to_side_prepose_if_configured(planning_cup_base): return False if not self.move_joint1_clearance_before_side_grip(): return False diff --git a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py index 9401151..9034dbf 100644 --- a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py +++ b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py @@ -1,8 +1,16 @@ from copy import deepcopy from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, +) from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -167,16 +175,15 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params ) ) - nodes.append( - Node( - package="dsr_practice", - executable="yolo_cup_pick_node", - output="screen", - parameters=[ - runtime_moveit_params, - moveit_py_params, - side_prepose_params, - { + yolo_node = Node( + package="dsr_practice", + executable="yolo_cup_pick_node", + output="screen", + parameters=[ + runtime_moveit_params, + moveit_py_params, + side_prepose_params, + { "model_path": ParameterValue( LaunchConfiguration("model_path"), value_type=str, @@ -218,6 +225,9 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params ), "side_stage_y_min": LaunchConfiguration("side_stage_y_min"), "side_stage_y_max": LaunchConfiguration("side_stage_y_max"), + "side_target_x_offset_m": LaunchConfiguration( + "side_target_x_offset_m" + ), "side_grasp_offset": LaunchConfiguration("side_grasp_offset"), "side_grasp_z_offset": LaunchConfiguration("side_grasp_z_offset"), "side_grasp_stop_backoff_m": LaunchConfiguration( @@ -368,8 +378,22 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params "place_y": LaunchConfiguration("place_y"), "place_z": LaunchConfiguration("place_z"), "auto_pick": LaunchConfiguration("auto_pick"), - }, - ], + }, + ], + ) + nodes.append(yolo_node) + nodes.append( + RegisterEventHandler( + OnProcessExit( + target_action=yolo_node, + on_exit=[ + EmitEvent( + event=Shutdown( + reason="yolo_cup_pick_node exited; stopping helper nodes" + ) + ) + ], + ) ) ) return nodes @@ -471,6 +495,11 @@ def generate_launch_description(): default_value="0.35", description="Maximum preferred base_link Y for side staging; side direction is flipped if the configured direction leaves this range.", ) + side_target_x_offset_m_arg = DeclareLaunchArgument( + "side_target_x_offset_m", + default_value="0.0", + description="Planning-only base_link X compensation added to side-grip cup targets after vision/refinement.", + ) side_grasp_offset_arg = DeclareLaunchArgument( "side_grasp_offset", default_value="0.035" ) @@ -571,10 +600,10 @@ def generate_launch_description(): ) link6_gripper_collision_enabled_arg = DeclareLaunchArgument( "link6_gripper_collision_enabled", - default_value="true", + default_value="false", description=( - "Publish the RG2/link_6 attached collision envelope so MoveIt plans " - "with the mounted gripper, not only the bare robot flange." + "Legacy attached RG2/link_6 box envelope. Keep false when the " + "mesh-based RG2 is already in the MoveIt URDF." ), ) table_collision_enabled_arg = DeclareLaunchArgument( @@ -841,6 +870,7 @@ def generate_launch_description(): side_short_stage_backoff_m_arg, side_stage_y_min_arg, side_stage_y_max_arg, + side_target_x_offset_m_arg, side_grasp_offset_arg, side_grasp_z_offset_arg, side_grasp_stop_backoff_m_arg, diff --git a/tools/checks/check_panel_service_discovery_race.py b/tools/checks/check_panel_service_discovery_race.py index dcc8c8c..89def56 100755 --- a/tools/checks/check_panel_service_discovery_race.py +++ b/tools/checks/check_panel_service_discovery_race.py @@ -104,6 +104,22 @@ def main() -> int: print("[FAIL] lid_grip_close command does not use the direct runner") print(lid_grip_close_command) return 1 + if "MOVE_TO_LID_VIEW_POSE=true" not in lid_grip_close_command: + print("[FAIL] lid_grip_close command must move to lid camera view pose first") + print(lid_grip_close_command) + return 1 + if "wait_for_lid_grip_status.py" not in lid_grip_close_command: + print("[FAIL] lid_grip_close command must wait for ArUco success status") + print(lid_grip_close_command) + return 1 + if "pick_from_cup_holder_side_grip.py" not in lid_grip_close_command: + print("[FAIL] lid_grip_close command must chain to cup-holder re-pick") + print(lid_grip_close_command) + return 1 + if "run_rule_based_shake_real.sh" not in lid_grip_close_command: + print("[FAIL] lid_grip_close command must chain to real shake") + print(lid_grip_close_command) + return 1 non_tmux_background = [ step.key diff --git a/tools/checks/check_rg2_moveit_description.py b/tools/checks/check_rg2_moveit_description.py index 6ab8fec..11cbd91 100755 --- a/tools/checks/check_rg2_moveit_description.py +++ b/tools/checks/check_rg2_moveit_description.py @@ -27,6 +27,7 @@ / "config" / "m0609.urdf.xacro" ) +MOVEIT_SRDF = MOVEIT_XACRO.with_name("dsr.srdf") REQUIRED_LINKS = { "rg2_quick_changer", @@ -37,12 +38,23 @@ "gripper_tcp", } +REQUIRED_DISABLED_COLLISIONS = { + frozenset(("link_6", "rg2_quick_changer")), + frozenset(("rg2_quick_changer", "rg2_angle_bracket")), + frozenset(("rg2_angle_bracket", "rg2_gripper_body")), + frozenset(("rg2_gripper_body", "rg2_left_inner_knuckle")), + frozenset(("rg2_gripper_body", "rg2_right_inner_knuckle")), + frozenset(("rg2_left_inner_knuckle", "rg2_left_inner_finger")), + frozenset(("rg2_right_inner_knuckle", "rg2_right_inner_finger")), +} + def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser( description="Check that M0609 MoveIt robot_description contains RG2 mesh collisions." ) parser.add_argument("--xacro", type=Path, default=MOVEIT_XACRO) + parser.add_argument("--srdf", type=Path, default=MOVEIT_SRDF) return parser.parse_args() @@ -79,8 +91,8 @@ def main() -> int: required_source = [ "rg2_parametric.xacro", "xacro:azas_rg2_parametric", - 'name="rg2_parent_link" default="tool0"', - 'name="rg2_mount_rpy" default="3.141592654 -1.570796327 0"', + 'name="rg2_parent_link" default="link_6"', + 'name="rg2_mount_rpy" default="0 0 0"', ] missing_source = [needle for needle in required_source if needle not in source] if missing_source: @@ -111,13 +123,36 @@ def main() -> int: quick_changer_joint = root.find("./joint[@name='rg2_quick_changer_joint']") parent = quick_changer_joint.find("parent").attrib.get("link") if quick_changer_joint is not None else None - if parent != "tool0": - print(f"[FAIL] rg2_quick_changer_joint parent should be tool0, found {parent!r}") + if parent != "link_6": + print(f"[FAIL] rg2_quick_changer_joint parent should be link_6, found {parent!r}") + return 1 + origin = quick_changer_joint.find("origin") if quick_changer_joint is not None else None + mount_rpy = origin.attrib.get("rpy") if origin is not None else None + if mount_rpy != "0 0 0": + print(f"[FAIL] rg2_quick_changer_joint mount rpy should be identity, found {mount_rpy!r}") + return 1 + + srdf_path = args.srdf.expanduser().resolve() + if not srdf_path.is_file(): + print(f"[FAIL] missing MoveIt SRDF: {srdf_path}") + return 1 + srdf = ET.parse(srdf_path).getroot() + disabled_collisions = { + frozenset((entry.attrib.get("link1", ""), entry.attrib.get("link2", ""))) + for entry in srdf.findall("disable_collisions") + } + missing_disabled = REQUIRED_DISABLED_COLLISIONS - disabled_collisions + if missing_disabled: + print("[FAIL] MoveIt SRDF does not allow required RG2 internal self-collisions:") + for pair in sorted(tuple(sorted(pair)) for pair in missing_disabled): + print(f"missing={pair[0]} <-> {pair[1]}") return 1 print("[PASS] M0609 MoveIt robot_description includes RG2 mesh collision links.") print(f"rg2_collision_meshes={len(rg2_collision_meshes)}") - print("rg2_parent=tool0") + print("rg2_parent=link_6") + print("rg2_mount_rpy=0 0 0") + print(f"rg2_required_disabled_collisions={len(REQUIRED_DISABLED_COLLISIONS)}") return 0 diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index a981b2e..81c3239 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -343,6 +343,59 @@ def chain_recipe_after_manual_command(manual_cmd: str, payload: dict[str, Any], ) +def chain_shake_after_lid_command(lid_cmd: str, payload: dict[str, Any]) -> str: + """Run holder re-pick + shake immediately after ArUco lid close success. + + The lid-grip launch is an OpenCV/manual ROS launch that stays alive after a + successful `p`-triggered sequence. Waiting for the process to exit would + block the next motion indefinitely, so the panel chain watches the planner's + `/jarvis/lid_gripper/status` success event, terminates the lid preview + launch, then starts the existing measured cup-holder re-pick/shake command. + """ + + steps_by_key = {step.key: step for step in STEPS} + shake_cmd = command_for(steps_by_key["shake_closed_cup"], payload) + wait_script = ROOT / "tools" / "run" / "wait_for_lid_grip_status.py" + wait_cmd = ( + f"cd {ROOT} && {ROS_SETUP} && " + f"python3 {shlex.quote(str(wait_script))} " + "--timeout-sec 900 --success-status motion_sequence_requested" + ) + return ( + f"( {lid_cmd} ) & " + "lid_pid=$!; " + f"( {wait_cmd} ) & " + "wait_pid=$!; " + "while true; do " + "if ! kill -0 ${wait_pid} 2>/dev/null; then " + "wait ${wait_pid}; wait_rc=$?; break; " + "fi; " + "if ! kill -0 ${lid_pid} 2>/dev/null; then " + "wait ${lid_pid}; lid_rc=$?; " + "sleep 1; " + "if ! kill -0 ${wait_pid} 2>/dev/null; then " + "wait ${wait_pid}; wait_rc=$?; break; " + "fi; " + "echo '[Azas] lid_grip_close launch exited before ArUco success status; shake chain blocked.'; " + "kill -TERM ${wait_pid} 2>/dev/null || true; " + "wait ${wait_pid} 2>/dev/null || true; " + "if [ ${lid_rc} -eq 0 ]; then exit 1; else exit ${lid_rc}; fi; " + "fi; " + "sleep 1; " + "done; " + "kill -TERM ${lid_pid} 2>/dev/null || true; " + "wait ${lid_pid} 2>/dev/null || true; " + "if [ ${wait_rc} -eq 0 ]; then " + "echo '[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹으로 바로 넘어갑니다.'; " + "echo '[Azas] auto_holder_pick_then_shake=true'; " + f"{shake_cmd}; " + "else " + "echo '[Azas] ArUco lid_grip_close 실패/타임아웃 -> 컵홀더 재픽업/쉐이킹을 건너뜁니다.'; " + "exit ${wait_rc}; " + "fi" + ) + + def hand_eye_static_tf_command(*, compose_timeout_sec: float = 30.0) -> str: """Start the measured hand-eye TF publisher without inventing camera poses.""" return ( @@ -686,15 +739,15 @@ class Step: False, "카메라 화면의 visible colored handle blob을 직접 검출해 왼쪽→오른쪽을 디스펜서 1~4로 매핑하고 outputs/dispenser_color_map.json 저장. TF 투영은 보조 경로", ), - Step("voice_input", "음성 입력 (STT+LLM 노드 시작)", "background", "ros2 launch azas_voice azas_voice.launch.py", True, False, "STT → /stt_result → llm_recipe_mapper → /azas/voice/recipe_decision"), + Step("voice_input", "수빈 STT/주문 UI 시작", "background", "ros2 launch azas_voice azas_voice.launch.py", True, False, "voice screen(8090) + STT topic(/stt_result) → recipe mapper → conversation manager. 로봇 좌표/모션은 만들지 않음"), Step( "listen_stt_recipe", - "STT 레시피 수신 대기 (60초)", + "수빈 STT 레시피 확정 대기 (60초)", "run", "tools/run/listen_stt_recipe.py --timeout 60", True, False, - "사용자가 말하면 /azas/voice/recipe_decision 수신 → outputs/latest_recipe.json 저장", + "사용자가 메뉴를 말하고 '응'으로 확정하면 /azas/voice/confirmed_recipe_decision 수신 → outputs/latest_recipe.json 저장", ), Step( "run_color_recipe_sequence", @@ -709,10 +762,10 @@ class Step: "side_grip", "PR #20 RealSense 컵 인식 후 side grip", "background", - "ros2 launch dsr_practice yolo_cup_pick_node.launch.py auto_pick:=false exit_after_pick:=false grasp_mode:=side moveit_controller_name:=/dsr01/dsr_moveit_controller", + "SIDE_TARGET_X_OFFSET_M=-0.020 bash tools/run/run_changhyun_side_grip_direct.sh", True, True, - "OpenCV 창에서 컵 확인 후 p 키로 side-grip 실행. 장기 실행 GUI 노드라 패널에서는 tmux 창으로 분리 실행", + "OpenCV 창에서 컵 확인 후 p 키로 side-grip 실행. 패널은 direct runner를 tmux로 띄우며 기본 X 보정은 -20mm", ), Step( "cup_uprighting", @@ -887,7 +940,7 @@ class Step: False, "실제 로봇 미사용: 별도 ROS_DOMAIN_ID에서 쉐이킹 궤적/마커를 RViz로 표시", ), - Step("shake_closed_cup", "컵홀더 컵 다시 잡기 후 쉐이킹", "run", "tools/run/pick_from_cup_holder_side_grip.py && tools/run/run_rule_based_shake_real.sh", True, True, "시작 시 컵홀더에 놓인 닫힌 컵을 측정된 cup_holder.side_grip_place pose로 다시 side-grip 픽업한 뒤, J3 양수 고정 및 J4/J5/J6 트위스트 쉐이킹을 실행"), + Step("shake_closed_cup", "컵홀더 컵 다시 잡기 후 쉐이킹", "run", "tools/run/pick_from_cup_holder_side_grip.py && tools/run/run_rule_based_shake_real.sh", True, True, "시작 시 컵홀더에 놓인 닫힌 컵을 측정된 cup_holder.side_grip_place pose로 다시 side-grip 픽업한 뒤, J3 양수 고정 및 J4/J5/J6 트위스트 쉐이킹을 실행. 쉐이킹 성공 시 컵을 든 채 카메라 포즈(J=[3, -12.7, 44, -9, 133, 90])로 복귀해 손 검출/핸드오버 준비"), Step( "start_hand_detection", "손 검출 시작 / 무모션", @@ -897,6 +950,15 @@ class Step: False, "perception 전용: MediaPipe로 펼친 손바닥을 추적해 /azas/human_hand_detection으로 발행. 로봇 모션 없음", ), + Step( + "start_hand_detection_view", + "손 검출 화면 보기", + "background", + "rqt_image_view /azas/human_hand_detection/overlay", + True, + False, + "손 검출 overlay(랜드마크/STABLE 라벨)를 rqt_image_view 창으로 표시. 손 검출 시작 버튼이 먼저 켜져 있어야 영상이 나옴", + ), Step( "handover_cup_to_palm", "쉐이킹 후 손바닥에 컵 건네기", @@ -934,6 +996,7 @@ class Step: "lid_grip_close", "shake_rviz_preview", "start_hand_detection", + "start_hand_detection_view", } PANEL_HIDDEN_STEP_KEYS = { "rviz_cocktail_collision_preview", @@ -945,8 +1008,6 @@ class Step: "run_cocktail_now_real", "start_camera_view", "detect_cup_lid", - "voice_input", - "listen_stt_recipe", "run_one_click_cocktail_real", "move_to_dispenser_1", "move_to_dispenser_2", @@ -2193,12 +2254,10 @@ def wait_for_collision_object_sample( timeout_sec: float = 10.0, proc: subprocess.Popen[str] | None = None, ) -> tuple[bool, str]: - """Wait until workspace objects and the link_6 gripper attachment are visible.""" + """Wait until workspace collision objects are visible.""" deadline = time.monotonic() + max(timeout_sec, 0.1) last_collision_output = "" - last_attached_output = "" saw_collision = False - saw_attached_gripper = False while time.monotonic() < deadline: if proc is not None and proc.poll() is not None: return False, "collision scene process exited while waiting\n" + tail_file(process_logs.get("start_collision_scene")) @@ -2216,38 +2275,17 @@ def wait_for_collision_object_sample( if collision_result.returncode == 0 and "id:" in collision_result.stdout: saw_collision = True - attached_result = subprocess.run( - ["bash", "-lc", "timeout 2s ros2 topic echo /attached_collision_object --once"], - cwd=str(ROOT), - env=env, - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT, - text=True, - timeout=3.0, - check=False, - ) - last_attached_output = attached_result.stdout[-2000:] - if ( - attached_result.returncode == 0 - and "azas_rg2_gripper_on_link6" in attached_result.stdout - and "link_name: link_6" in attached_result.stdout - ): - saw_attached_gripper = True - - if saw_collision and saw_attached_gripper: + if saw_collision: return ( True, "collision object sample observed on /collision_object\n" - + last_collision_output - + "\nattached RG2 gripper envelope observed on /attached_collision_object\n" - + last_attached_output, + + last_collision_output, ) time.sleep(0.5) return False, ( f"scene readiness incomplete within {timeout_sec:.1f}s " - f"(workspace={saw_collision}, link6_gripper={saw_attached_gripper})\n" - f"--- last /collision_object ---\n{last_collision_output}\n" - f"--- last /attached_collision_object ---\n{last_attached_output}" + f"(workspace={saw_collision})\n" + f"--- last /collision_object ---\n{last_collision_output}" ) @@ -3404,15 +3442,8 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "dispenser_collision_publish_markers:=true & " "ros2 launch azas_bringup rg2_link6_tcp.launch.py " "publish_gripper_collision:=false & " - "(timeout 12s ros2 run azas_motion link6_gripper_collision_node " + "timeout 12s ros2 run azas_motion link6_gripper_collision_node " "--ros-args -p operation:=remove -p publish_once:=true -p publish_markers:=false || true; " - "ros2 run azas_motion link6_gripper_collision_node " - "--ros-args " - "-p palm_size_x_m:=0.075 -p palm_size_y_m:=0.115 -p palm_size_z_m:=0.040 -p palm_z_m:=0.070 " - "-p finger_size_x_m:=0.030 -p finger_size_y_m:=0.014 -p finger_size_z_m:=0.120 " - "-p finger_y_m:=0.050 -p finger_z_m:=0.125 " - "-p pad_size_x_m:=0.022 -p pad_size_y_m:=0.010 -p pad_size_z_m:=0.025 " - "-p pad_y_m:=0.037 -p pad_z_m:=0.180) & " "ros2 run tf2_ros static_transform_publisher " "--x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 " "--frame-id world --child-frame-id base_link & " @@ -3446,6 +3477,13 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " "ros2 run rqt_image_view rqt_image_view /camera/camera/color/image_raw" ) + if step.key == "start_hand_detection_view": + return ( + f"cd {ROOT} && {ROS_SETUP} && " + "DISPLAY=${DISPLAY:-:0} " + "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " + "ros2 run rqt_image_view rqt_image_view /azas/human_hand_detection/overlay" + ) if step.key == "detect_cup_lid": return f"cd {ROOT} && {ROS_SETUP} && ros2 launch azas_bringup yolo_perception.launch.py" if step.key == "pick_lid": @@ -3455,13 +3493,17 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe ) if step.key == "lid_grip_close": direct_script = ROOT / "tools" / "run" / "run_kang_lid_grip_close_direct.sh" - return ( + manual_cmd = ( f"cd {ROOT} && " f"SERVICE_PREFIX={shlex.quote(service_prefix)} " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " + "MOVE_TO_LID_VIEW_POSE=true " f"bash {shlex.quote(str(direct_script))}" ) + if payload.get("_auto_shake_after_lid_grip_close", True): + return chain_shake_after_lid_command(manual_cmd, payload) + return manual_cmd if step.key == "cup_uprighting": direct_script = ROOT / "tools" / "run" / "run_somyeong_cup_uprighting_direct.sh" manual_cmd = ( @@ -3477,12 +3519,23 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe return chain_recipe_after_manual_command(manual_cmd, payload, "소명 cup_uprighting") return manual_cmd if step.key == "voice_input": - return f"cd {ROOT} && {ROS_SETUP} && ros2 launch azas_voice azas_voice.launch.py" + return ( + f"cd {ROOT} && {ROS_SETUP} && " + "echo '[Azas] 수빈 STT/주문 UI: voice screen http://localhost:8090' && " + "echo '[Azas] 메뉴를 말하거나 테스트 발화 입력 후, 응/시작으로 확정하면 listen_stt_recipe가 latest_recipe.json을 저장합니다.' && " + "ros2 launch azas_voice azas_voice.launch.py run_voice_screen:=true" + ) if step.key == "side_grip": direct_script = ROOT / "tools" / "run" / "run_changhyun_side_grip_direct.sh" + side_target_x_offset_m = str( + payload.get("side_target_x_offset_m") + or os.environ.get("SIDE_TARGET_X_OFFSET_M") + or "-0.020" + ) manual_cmd = ( f"cd {ROOT} && " f"SERVICE_PREFIX={shlex.quote(service_prefix)} " + f"SIDE_TARGET_X_OFFSET_M={shlex.quote(side_target_x_offset_m)} " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " f"bash {shlex.quote(str(direct_script))}" @@ -3709,6 +3762,13 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 " "REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true " "tools/run/run_rule_based_shake_real.sh" + " && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && " + "python3 tools/run/direct_movej_joints.py " + f"--service-prefix {service_prefix} " + "--j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 " + "--velocity 15 --acceleration 15 " + "--j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 " + "--execute --confirm ENABLE_DIRECT_MOVEJ" ) if step.key == "handover_cup_to_palm": release_height_m = str( diff --git a/tools/run/run_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh index 0e4a8e0..f406d45 100755 --- a/tools/run/run_changhyun_side_grip_direct.sh +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -71,19 +71,10 @@ ros2 run tf2_ros static_transform_publisher \ ros2 run azas_perception hand_eye_static_tf_node \ --ros-args -p compose_timeout_sec:=30.0 -p allow_direct_fallback:=false & -# Publish only the attached RG2/link_6 collision object before launching the -# picker. Keep the launch-side include disabled because it also starts an -# auxiliary robot_state_publisher and can stall MoveItPy initialization in the -# field tmux workflow. +# The RG2 mesh is part of the M0609 MoveIt URDF. Remove the older attached +# link_6 box envelope so it cannot duplicate the mesh in PlanningScene/RViz. timeout 12s ros2 run azas_motion link6_gripper_collision_node \ --ros-args -p operation:=remove -p publish_once:=true -p publish_markers:=false || true -ros2 run azas_motion link6_gripper_collision_node \ - --ros-args \ - -p palm_size_x_m:=0.075 -p palm_size_y_m:=0.115 -p palm_size_z_m:=0.040 -p palm_z_m:=0.070 \ - -p finger_size_x_m:=0.030 -p finger_size_y_m:=0.014 -p finger_size_z_m:=0.120 \ - -p finger_y_m:=0.050 -p finger_z_m:=0.125 \ - -p pad_size_x_m:=0.022 -p pad_size_y_m:=0.010 -p pad_size_z_m:=0.025 \ - -p pad_y_m:=0.037 -p pad_z_m:=0.180 & should_start_relay=false if [[ "${START_JOINT_STATE_RELAY:-auto}" == "true" ]]; then @@ -115,6 +106,7 @@ ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ redetect_on_approach:=false redetect_settle_sec:=0.5 \ grasp_mode:=side side_far_stage_enabled:=false side_approach_offset:=0.18 \ side_short_stage_backoff_m:=0.08 side_grasp_stop_backoff_m:=0.04 side_close_underreach_m:=0.03 \ + side_target_x_offset_m:="${SIDE_TARGET_X_OFFSET_M:--0.020}" \ side_low_retry_lift_m:=0.0 side_low_retry_attempts:=0 \ side_linear_approach_enabled:=true side_final_slide_enabled:=false \ side_fixed_grasp_z_enabled:=false side_grasp_z_offset:=0.05 side_project_bbox_center_to_fixed_z:=false \ diff --git a/tools/run/run_course_dispenser_press_cycle_rviz.sh b/tools/run/run_course_dispenser_press_cycle_rviz.sh index 721671b..9b16f9c 100755 --- a/tools/run/run_course_dispenser_press_cycle_rviz.sh +++ b/tools/run/run_course_dispenser_press_cycle_rviz.sh @@ -311,10 +311,10 @@ fi if [[ "${SHOW_LINK6_GRIPPER}" == "1" || "${SHOW_LINK6_GRIPPER}" == "true" ]]; then ros2 launch azas_bringup rg2_link6_tcp.launch.py \ - publish_gripper_collision:=true \ + publish_gripper_collision:=false \ >"${LOG_DIR}/rg2_link6_tcp.log" 2>&1 & PIDS+=("$!") - echo "[Azas] SHOW_LINK6_GRIPPER=${SHOW_LINK6_GRIPPER}: publishing RG2 link_6 TF/markers on /azas/link6_gripper/markers." + echo "[Azas] SHOW_LINK6_GRIPPER=${SHOW_LINK6_GRIPPER}: publishing RG2 link_6 TF only; MoveIt uses the mesh-based RG2 URDF." fi if [[ "${DISPENSER_COLLISION_OBJECTS}" == "0" || "${DISPENSER_COLLISION_OBJECTS}" == "false" ]]; then diff --git a/tools/run/run_tmux_logic_sequence.sh b/tools/run/run_tmux_logic_sequence.sh index 11fd41e..f33f6dc 100755 --- a/tools/run/run_tmux_logic_sequence.sh +++ b/tools/run/run_tmux_logic_sequence.sh @@ -129,15 +129,9 @@ ensure_collision_scene() { dispenser_collision_publish_objects:=true \ dispenser_collision_publish_markers:=true & ros2 launch azas_bringup rg2_link6_tcp.launch.py publish_gripper_collision:=false & + # The RG2 mesh is now in the MoveIt URDF; purge the legacy attached box. timeout 12s ros2 run azas_motion link6_gripper_collision_node \ --ros-args -p operation:=remove -p publish_once:=true -p publish_markers:=false || true - ros2 run azas_motion link6_gripper_collision_node \ - --ros-args \ - -p palm_size_x_m:=0.075 -p palm_size_y_m:=0.115 -p palm_size_z_m:=0.040 -p palm_z_m:=0.070 \ - -p finger_size_x_m:=0.030 -p finger_size_y_m:=0.014 -p finger_size_z_m:=0.120 \ - -p finger_y_m:=0.050 -p finger_z_m:=0.125 \ - -p pad_size_x_m:=0.022 -p pad_size_y_m:=0.010 -p pad_size_z_m:=0.025 \ - -p pad_y_m:=0.037 -p pad_z_m:=0.180 & ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 0 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id base_link & ros2 run azas_perception hand_eye_static_tf_node --ros-args -p compose_timeout_sec:=30.0 -p allow_direct_fallback:=false & python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=publish_detected -p object_id:=detected_tumbler -p use_lidded_height:=true @@ -163,6 +157,7 @@ run_side_grip() { redetect_on_approach:=false redetect_settle_sec:=0.5 \ grasp_mode:=side side_far_stage_enabled:=false side_approach_offset:=0.18 \ side_short_stage_backoff_m:=0.08 side_grasp_stop_backoff_m:=0.04 side_close_underreach_m:=0.03 \ + side_target_x_offset_m:="${SIDE_TARGET_X_OFFSET_M:--0.020}" \ side_low_retry_lift_m:=0.0 side_low_retry_attempts:=0 \ side_linear_approach_enabled:=true side_final_slide_enabled:=false \ side_fixed_grasp_z_enabled:=false side_grasp_z_offset:=0.05 side_project_bbox_center_to_fixed_z:=false \ diff --git a/tools/run/wait_for_lid_grip_status.py b/tools/run/wait_for_lid_grip_status.py new file mode 100755 index 0000000..16c998c --- /dev/null +++ b/tools/run/wait_for_lid_grip_status.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python3 +"""Wait for the supervised lid-grip/twist sequence to report success. + +The lid-grip launch keeps its OpenCV/perception nodes alive after a successful +`p`-triggered sequence. Panel shell chaining therefore needs a small ROS topic +gate that exits as soon as the planner publishes its terminal success/failure +status instead of waiting for the operator to close the preview window. +""" + +from __future__ import annotations + +import argparse +import json +import time + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser( + description="Wait for /jarvis/lid_gripper/status success/failure JSON." + ) + parser.add_argument("--topic", default="/jarvis/lid_gripper/status") + parser.add_argument("--timeout-sec", type=float, default=900.0) + parser.add_argument( + "--success-status", + action="append", + default=["motion_sequence_requested"], + help="status value that means the lid close sequence completed successfully", + ) + parser.add_argument( + "--failure-status", + action="append", + default=["failed"], + help="status value that means the lid close sequence failed", + ) + return parser.parse_args() + + +class LidGripStatusWaiter(Node): + def __init__(self, topic: str, success_statuses: set[str], failure_statuses: set[str]): + super().__init__("azas_wait_for_lid_grip_status") + self._success_statuses = success_statuses + self._failure_statuses = failure_statuses + self.result_code: int | None = None + self.result_text = "" + self.create_subscription(String, topic, self._on_status, 10) + print(f"[Azas] waiting for lid grip status on {topic}", flush=True) + + def _on_status(self, msg: String) -> None: + try: + payload = json.loads(msg.data) + except json.JSONDecodeError: + payload = {"status": msg.data} + status = str(payload.get("status", "")).strip() + if not status: + return + print(f"[Azas] lid_grip_status={status} payload={payload}", flush=True) + if status in self._success_statuses: + self.result_code = 0 + self.result_text = f"success status observed: {status}" + elif status in self._failure_statuses: + self.result_code = 1 + self.result_text = f"failure status observed: {status}" + + +def main() -> int: + args = parse_args() + success_statuses = {str(item) for item in args.success_status} + failure_statuses = {str(item) for item in args.failure_status} + timeout_sec = max(float(args.timeout_sec), 0.1) + + rclpy.init(args=None) + node = LidGripStatusWaiter(args.topic, success_statuses, failure_statuses) + deadline = time.monotonic() + timeout_sec + try: + while rclpy.ok() and node.result_code is None and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if node.result_code is not None: + print(f"[Azas] {node.result_text}", flush=True) + return node.result_code + print(f"[Azas][FAIL] lid grip status wait timed out after {timeout_sec:.1f}s", flush=True) + return 2 + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + raise SystemExit(main()) From d2342510fa8b9d692dde438fe06679a496ab4b92 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 08:48:43 +0900 Subject: [PATCH 68/88] =?UTF-8?q?QA=20=ED=85=8C=EC=8A=A4=ED=8A=B8=20?= =?UTF-8?q?=EA=B2=B0=EA=B3=BC=20side=20grip=EB=AF=B8=EC=9E=91=EB=8F=99,=20?= =?UTF-8?q?=EC=95=84=EB=A5=B4=EC=BD=94=20=EB=A7=88=EC=BB=A4=20=EC=9D=B8?= =?UTF-8?q?=EC=8B=9D=20=EC=9D=B4=EC=83=81=ED=95=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- omx_wiki/index.md | 4 +- omx_wiki/log.md | 8 ++ omx_wiki/session-log-2026-06-11-5-rp8btb.md | 18 +++ omx_wiki/session-log-2026-06-11-6-dnt3rj.md | 18 +++ tools/checks/check_hand_detection_status.py | 71 +++++++++++ tools/run/auto_handover_on_palm.py | 129 ++++++++++++++++++++ tools/run/run_color_recipe_sequence.py | 2 +- 7 files changed, 248 insertions(+), 2 deletions(-) create mode 100644 omx_wiki/session-log-2026-06-11-5-rp8btb.md create mode 100644 omx_wiki/session-log-2026-06-11-6-dnt3rj.md create mode 100755 tools/checks/check_hand_detection_status.py create mode 100755 tools/run/auto_handover_on_palm.py diff --git a/omx_wiki/index.md b/omx_wiki/index.md index e119ab6..345ce9a 100644 --- a/omx_wiki/index.md +++ b/omx_wiki/index.md @@ -1,6 +1,6 @@ # Wiki Index -> 6 pages | Last updated: 2026-06-10T10:52:52.609Z +> 8 pages | Last updated: 2026-06-11T23:23:12.255Z ## session-log @@ -10,3 +10,5 @@ - [Session Log 2026-06-05](session-log-2026-06-05-8-3rrp4t.md) — # Session Log 2026-06-05 - [Session Log 2026-06-07](session-log-2026-06-07-9-5l0d8p.md) — # Session Log 2026-06-07 - [Session Log 2026-06-10](session-log-2026-06-10-7-prx6mu.md) — # Session Log 2026-06-10 +- [Session Log 2026-06-11](session-log-2026-06-11-5-rp8btb.md) — # Session Log 2026-06-11 +- [Session Log 2026-06-11](session-log-2026-06-11-6-dnt3rj.md) — # Session Log 2026-06-11 diff --git a/omx_wiki/log.md b/omx_wiki/log.md index 78e9c0e..97ffc6a 100644 --- a/omx_wiki/log.md +++ b/omx_wiki/log.md @@ -77,3 +77,11 @@ - **Pages:** session-log-2026-06-10-7-prx6mu.md - **Summary:** Auto-captured session log for omx-1780826697197-prx6mu +## [2026-06-11T13:16:04.802Z] session-end +- **Pages:** session-log-2026-06-11-5-rp8btb.md +- **Summary:** Auto-captured session log for omx-1781173835835-rp8btb + +## [2026-06-11T23:23:12.252Z] session-end +- **Pages:** session-log-2026-06-11-6-dnt3rj.md +- **Summary:** Auto-captured session log for omx-1781218680276-dnt3rj + diff --git a/omx_wiki/session-log-2026-06-11-5-rp8btb.md b/omx_wiki/session-log-2026-06-11-5-rp8btb.md new file mode 100644 index 0000000..7aa8dc4 --- /dev/null +++ b/omx_wiki/session-log-2026-06-11-5-rp8btb.md @@ -0,0 +1,18 @@ +--- +title: "Session Log 2026-06-11" +tags: ["session-log", "auto-captured"] +created: 2026-06-11T13:16:04.802Z +updated: 2026-06-11T13:16:04.802Z +sources: ["omx-1781173835835-rp8btb"] +links: [] +category: session-log +confidence: medium +schemaVersion: 1 +--- + +# Session Log 2026-06-11 + +Auto-captured session metadata. +Session ID: omx-1781173835835-rp8btb + +Review and promote significant findings to curated wiki pages via `wiki_ingest`. diff --git a/omx_wiki/session-log-2026-06-11-6-dnt3rj.md b/omx_wiki/session-log-2026-06-11-6-dnt3rj.md new file mode 100644 index 0000000..93b14cb --- /dev/null +++ b/omx_wiki/session-log-2026-06-11-6-dnt3rj.md @@ -0,0 +1,18 @@ +--- +title: "Session Log 2026-06-11" +tags: ["session-log", "auto-captured"] +created: 2026-06-11T23:23:12.252Z +updated: 2026-06-11T23:23:12.252Z +sources: ["omx-1781218680276-dnt3rj"] +links: [] +category: session-log +confidence: medium +schemaVersion: 1 +--- + +# Session Log 2026-06-11 + +Auto-captured session metadata. +Session ID: omx-1781218680276-dnt3rj + +Review and promote significant findings to curated wiki pages via `wiki_ingest`. diff --git a/tools/checks/check_hand_detection_status.py b/tools/checks/check_hand_detection_status.py new file mode 100755 index 0000000..f39c903 --- /dev/null +++ b/tools/checks/check_hand_detection_status.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +"""Panel check: listen to /azas/human_hand_detection/status and summarize. + +Perception-only (no motion command). Listens for a fixed window and reports +how many frames detected a hand and how many passed the stability gate that +actually publishes coordinates for the palm handover. + +Exit codes: + 0 stable open-hand detections seen (handover can consume coordinates) + 1 status is flowing but no stable open hand in the window + 2 no status messages at all (detection node or camera is not running) +""" +from __future__ import annotations + +import argparse +import json +import time + +import rclpy +from std_msgs.msg import String + +STATUS_TOPIC = "/azas/human_hand_detection/status" + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--listen-sec", type=float, default=10.0) + args = parser.parse_args() + + messages: list[dict] = [] + rclpy.init() + node = rclpy.create_node("azas_check_hand_detection_status") + node.create_subscription( + String, STATUS_TOPIC, lambda m: messages.append(json.loads(m.data)), 10 + ) + print(f"[Azas] 손 검출 상태를 {args.listen_sec:.0f}초간 측정합니다 (로봇 모션 없음).") + deadline = time.monotonic() + args.listen_sec + while rclpy.ok() and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.2) + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + detected = [m for m in messages if m.get("detected")] + stable = [m for m in messages if m.get("stable")] + print(f"[Azas] 상태 메시지 {len(messages)}개 / 손 검출 {len(detected)}개 / STABLE {len(stable)}개") + + if not messages: + print("[FAIL] 상태 메시지가 없습니다. 카메라와 '손 검출 시작' 버튼이 켜져 있는지 확인하세요.") + return 2 + if not stable: + reasons = [str(m.get("reason", "")) for m in messages if m.get("reason")] + if reasons: + print(f"[Azas] 최근 사유: {reasons[-1]}") + print( + "[FAIL] STABLE 검출이 없습니다. 손바닥을 펴고(손가락 4개 이상), " + "카메라에서 0.3m 이상 떨어져 1초간 정지하세요. 텀블러가 가리는 화면 " + "오른쪽 아래를 피해 왼쪽/위쪽 영역에 손을 두세요." + ) + return 1 + + last = stable[-1] + xyz = last.get("camera_xyz_m") + depth = last.get("depth_m") + print(f"[Azas] 마지막 STABLE: depth={depth}m camera_xyz_m={xyz} palm_px={last.get('palm_px')}") + print("[PASS] 안정적인 손바닥 검출이 발행되고 있습니다. 핸드오버 진행 가능.") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/auto_handover_on_palm.py b/tools/run/auto_handover_on_palm.py new file mode 100755 index 0000000..1f04164 --- /dev/null +++ b/tools/run/auto_handover_on_palm.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +"""One-shot auto handover: wait for a stable open palm, then hand the cup over. + +Panel flow "손 보이면 자동 핸드오버": this watcher holds NO motion of its own. +It only listens to /azas/human_hand_detection (published by +run_human_hand_detection.sh ONLY while an open palm stays spatially stable) +and, once the palm has been continuously stable for the trigger window, runs +the existing gated handover script tools/run/handover_cup_to_palm.py exactly +once and exits with its return code. + +Layered safety (kept from the manual flow): + - trigger needs N stable detections inside a sliding window (person must + hold the palm open and still BEFORE the robot starts at all) + - handover_cup_to_palm.py then re-samples the palm itself, checks workspace + bounds, re-checks the palm before descent, and aborts on any force spike + - one-shot: after one attempt (success or abort) this watcher exits, so the + robot never re-launches at a hand by itself + +Usage: + python3 tools/run/auto_handover_on_palm.py # dry-run + python3 tools/run/auto_handover_on_palm.py --execute --confirm AUTO_HANDOVER_ON_PALM +""" +from __future__ import annotations + +import argparse +import subprocess +import sys +import time +from pathlib import Path + +ROOT = Path(__file__).resolve().parents[2] +HANDOVER_SCRIPT = ROOT / "tools" / "run" / "handover_cup_to_palm.py" +HAND_TOPIC = "/azas/human_hand_detection" +CONFIRM_PHRASE = "AUTO_HANDOVER_ON_PALM" + + +def wait_for_stable_palm(args: argparse.Namespace) -> bool: + """Spin a perception-only node until the palm trigger fires or we time out.""" + import rclpy + from geometry_msgs.msg import PointStamped + + stamps: list[float] = [] + rclpy.init() + node = rclpy.create_node("azas_auto_handover_watch") + node.create_subscription( + PointStamped, HAND_TOPIC, lambda _msg: stamps.append(time.monotonic()), 10 + ) + print( + f"[Azas] 손 대기 시작: {args.trigger_window_sec:.1f}초 안에 안정 검출 " + f"{args.trigger_stable_count}개가 쌓이면 핸드오버를 1회 실행합니다 " + f"(최대 {args.wait_timeout_sec:.0f}초 대기, 대기 중 로봇 모션 없음)." + ) + deadline = time.monotonic() + args.wait_timeout_sec + last_report = 0.0 + triggered = False + try: + while rclpy.ok() and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.2) + now = time.monotonic() + stamps[:] = [t for t in stamps if now - t <= args.trigger_window_sec] + if len(stamps) >= args.trigger_stable_count: + triggered = True + break + if now - last_report >= 5.0: + last_report = now + remain = deadline - now + print( + f"[Azas] 대기 중... 최근 {args.trigger_window_sec:.1f}초 안정 검출 " + f"{len(stamps)}/{args.trigger_stable_count}개 (남은 시간 {remain:.0f}초). " + "손바닥을 펴고 정지해 주세요." + ) + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return triggered + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--trigger-stable-count", type=int, default=12, + help="trigger when this many stable detections land inside the window") + parser.add_argument("--trigger-window-sec", type=float, default=3.0) + parser.add_argument("--wait-timeout-sec", type=float, default=180.0, + help="give up (exit 3, no motion) when no stable palm appears in time") + parser.add_argument("--release-tcp-above-palm-m", default="0.08") + parser.add_argument("--execute", action="store_true") + parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} with --execute") + args = parser.parse_args() + + if args.execute and args.confirm != CONFIRM_PHRASE: + print(f"[BLOCKED] --execute requires --confirm {CONFIRM_PHRASE}") + return 2 + if not args.execute: + print("[DRY-RUN] --execute 미지정: 손 트리거 후 핸드오버도 dry-run(인식+계획만)으로 실행합니다.") + + if not wait_for_stable_palm(args): + print( + f"[FAIL] {args.wait_timeout_sec:.0f}초 안에 안정적인 손바닥이 없어 종료합니다 (로봇 모션 없음). " + "손 검출 화면에서 STABLE이 뜨는 위치를 확인한 뒤 다시 실행하세요." + ) + return 3 + + print("[Azas] 손 트리거 충족. 핸드오버를 1회 실행합니다 (이후 자동 재시도 없음).") + cmd = [ + sys.executable, str(HANDOVER_SCRIPT), + "--service-prefix", args.service_prefix, + "--release-tcp-above-palm-m", str(args.release_tcp_above_palm_m), + "--transit-velocity", "10.0", "--transit-acceleration", "14.0", + "--descent-velocity", "4.0", "--descent-acceleration", "6.0", + "--force-abort-delta-n", "10.0", + ] + if args.execute: + cmd += [ + "--execute", "--confirm", "ENABLE_HUMAN_PALM_HANDOVER", + "--approve-motion", "ENABLE_HUMAN_PALM_HANDOVER_MOTION", + "--approve-release", "RELEASE_CUP_NOW", + ] + rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode + if rc == 0: + print("[PASS] 자동 핸드오버 완료.") + else: + print(f"[FAIL] 핸드오버가 비정상 종료했습니다 (rc={rc}); 위 로그를 확인하세요.") + return rc + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index e42d26d..04f0f75 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -285,7 +285,7 @@ def main() -> int: parser.add_argument("--press-pre-lift-retreat-y-m", default="0.0") parser.add_argument("--move-release-offset-x-m", default="-0.020") parser.add_argument("--move-release-offset-y-m", default="0.0") - parser.add_argument("--move-release-offset-z-m", default="0.0") + parser.add_argument("--move-release-offset-z-m", default="0.010") parser.add_argument("--cup-pre-from-place-x-offset-m", default="-0.090") parser.add_argument("--cup-pre-from-place-z-offset-m", default="0.030") parser.add_argument("--generated-cup-pre-max-joint-delta-deg", default="190.0") From aed286c52fe76d4b64d52e90cc05bf280b266344 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 09:33:52 +0900 Subject: [PATCH 69/88] =?UTF-8?q?=EC=BB=B5=20=EC=83=81=ED=83=9C=20?= =?UTF-8?q?=ED=8C=90=EB=8B=A8=ED=9B=84=20=EC=82=AC=EC=9D=B4=EB=93=9C=20?= =?UTF-8?q?=EA=B7=B8=EB=A6=BD=20=EC=95=88=EB=90=98=EB=8A=94=20=EC=98=A4?= =?UTF-8?q?=EB=A5=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../azas_cup_uprighting/_base_node.py | 59 +++++++++++++++++-- 1 file changed, 55 insertions(+), 4 deletions(-) diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py index 336fae6..fe23838 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -31,7 +31,6 @@ from scipy.spatial.transform import Rotation from sensor_msgs.msg import CameraInfo, Image -from cv_bridge import CvBridge from moveit.core.robot_state import RobotState from moveit.planning import MoveItPy, PlanRequestParameters @@ -67,7 +66,6 @@ def __init__(self): log = self.get_logger() # ── 카메라 상태 ── - self.bridge = CvBridge() self.color_image = None self.depth_image = None self.intrinsics = None @@ -166,10 +164,63 @@ def _cam_info_cb(self, msg): } def _color_cb(self, msg): - self.color_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + self.color_image = self._imgmsg_to_bgr(msg) def _depth_cb(self, msg): - self.depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") + self.depth_image = self._imgmsg_to_array(msg) + + # cv_bridge는 NumPy 1.x ABI로 빌드되어 ~/.local의 NumPy 2.x와 충돌 + # (_ARRAY_API not found → segfault)하므로 직접 변환한다. + @staticmethod + def _imgmsg_to_array(msg: Image) -> np.ndarray: + encoding = msg.encoding.lower() + dtype_by_encoding = { + "8uc1": np.uint8, + "mono8": np.uint8, + "8uc3": np.uint8, + "rgb8": np.uint8, + "bgr8": np.uint8, + "16uc1": np.uint16, + "mono16": np.uint16, + "32fc1": np.float32, + } + channels_by_encoding = { + "8uc1": 1, + "mono8": 1, + "8uc3": 3, + "rgb8": 3, + "bgr8": 3, + "16uc1": 1, + "mono16": 1, + "32fc1": 1, + } + if encoding not in dtype_by_encoding: + raise ValueError(f"unsupported image encoding: {msg.encoding}") + + dtype = dtype_by_encoding[encoding] + channels = channels_by_encoding[encoding] + itemsize = np.dtype(dtype).itemsize + row_values = msg.step // itemsize + data = np.frombuffer(msg.data, dtype=dtype) + if msg.is_bigendian != (data.dtype.byteorder == ">"): + data = data.byteswap().view(data.dtype.newbyteorder()) + if channels == 1: + image = data.reshape((msg.height, row_values))[:, : msg.width] + else: + image = data.reshape((msg.height, row_values // channels, channels))[:, : msg.width, :] + return np.ascontiguousarray(image) + + @classmethod + def _imgmsg_to_bgr(cls, msg: Image) -> np.ndarray: + image = cls._imgmsg_to_array(msg) + encoding = msg.encoding.lower() + if encoding in {"bgr8", "8uc3"}: + return image + if encoding == "rgb8": + return cv2.cvtColor(image, cv2.COLOR_RGB2BGR) + if encoding in {"mono8", "8uc1"}: + return cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) + raise ValueError(f"unsupported color encoding: {msg.encoding}") # ════════════════════════════════════════════ # Perception 래퍼 From 795c722ec6b79aad4e28227652cd26b216ecf9d0 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 10:33:21 +0900 Subject: [PATCH 70/88] =?UTF-8?q?side=20grip=20dispenser=20cycle=20lid=20g?= =?UTF-8?q?rip=20and=20put=20=EC=97=B0=EA=B2=B0=EC=99=84=EB=A3=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 8 ++ .../azas_task_manager/auto_cup_flow_router.py | 117 +++++++++++++++++- tools/run/run_color_scan_stage.sh | 6 + tools/run/run_lid_close_then_shake_chain.sh | 6 + 4 files changed, 136 insertions(+), 1 deletion(-) create mode 100755 tools/run/run_color_scan_stage.sh create mode 100755 tools/run/run_lid_close_then_shake_chain.sh diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index a2cf835..d7f99e3 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -23,6 +23,10 @@ def generate_launch_description(): DeclareLaunchArgument("show_classification_window", default_value="true"), DeclareLaunchArgument("side_extra_args", default_value=""), DeclareLaunchArgument("cup_uprighting_extra_args", default_value=""), + DeclareLaunchArgument("side_target_x_offset_m", default_value="-0.02"), + DeclareLaunchArgument("color_scan_before_recipe", default_value="true"), + DeclareLaunchArgument("recipe_after_success", default_value="true"), + DeclareLaunchArgument("lid_shake_after_recipe", default_value="true"), Node( package="azas_task_manager", executable="auto_cup_flow_router", @@ -45,6 +49,10 @@ def generate_launch_description(): "show_classification_window": ParameterValue(LaunchConfiguration("show_classification_window"), value_type=bool), "side_extra_args": LaunchConfiguration("side_extra_args"), "cup_uprighting_extra_args": LaunchConfiguration("cup_uprighting_extra_args"), + "side_target_x_offset_m": ParameterValue(LaunchConfiguration("side_target_x_offset_m"), value_type=float), + "color_scan_before_recipe": ParameterValue(LaunchConfiguration("color_scan_before_recipe"), value_type=bool), + "recipe_after_success": ParameterValue(LaunchConfiguration("recipe_after_success"), value_type=bool), + "lid_shake_after_recipe": ParameterValue(LaunchConfiguration("lid_shake_after_recipe"), value_type=bool), }], ), ]) diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 302fef2..bc784a4 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -72,12 +72,41 @@ def __init__(self) -> None: self.declare_parameter("controller_action_name", "/dsr_moveit_controller/follow_joint_trajectory") self.declare_parameter("side_extra_args", "") self.declare_parameter("cup_uprighting_extra_args", "") + # 사이드 그립에서 base x가 +20mm 정도 어긋나는 실측 보정값 + self.declare_parameter("side_target_x_offset_m", -0.02) + + self.declare_parameter("color_scan_before_recipe", True) + self.declare_parameter( + "color_scan_command", + "bash /home/ssu/Azas/tools/run/run_color_scan_stage.sh", + ) + self.declare_parameter("recipe_after_success", True) + self.declare_parameter( + "recipe_command", + "cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && " + "mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && " + "export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && " + "export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && " + "export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && " + "if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && " + "if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && " + "if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; " + "else source /home/ssu/Azas/install/local_setup.bash; fi && " + "export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && " + "python3 tools/run/run_color_recipe_sequence.py --execute --confirm", + ) + self.declare_parameter("lid_shake_after_recipe", True) + self.declare_parameter( + "lid_shake_command", + "bash /home/ssu/Azas/tools/run/run_lid_close_then_shake_chain.sh", + ) self._latest_detection: Optional[CupDetection] = None self._latest_image: Optional[np.ndarray] = None self._image_lock = threading.Lock() self._window_enabled = bool(self.get_parameter("show_classification_window").value) self._children: list[subprocess.Popen[str]] = [] + self._child_node_failures: dict[str, list[str]] = {} self.create_subscription( CupDetection, @@ -118,6 +147,12 @@ def run(self) -> int: success = self._run_cup_uprighting(decision) if not success: return 1 + if not self._run_color_scan_sequence(): + return 1 + if not self._run_recipe_sequence(): + return 1 + if not self._run_lid_shake_sequence(): + return 1 self.get_logger().info("auto cup router: selected flow completed; router exiting") return 0 finally: @@ -401,6 +436,7 @@ def _run_side_grasp(self, decision: RouteDecision) -> bool: "table_center_y:=0.0", "dispenser_collision_enabled:=true", f"moveit_controller_name:={self.get_parameter('moveit_controller_name').value}", + f"side_target_x_offset_m:={float(self.get_parameter('side_target_x_offset_m').value)}", "start_joint_state_relay:=false", f"model_path:={self.get_parameter('yolo_model_path').value}", ]) @@ -421,6 +457,39 @@ def _run_cup_uprighting(self, decision: RouteDecision) -> bool: cmd.extend(self._split_extra_args(str(self.get_parameter("cup_uprighting_extra_args").value))) return self._run_process(cmd, "cup_uprighting") + def _run_color_scan_sequence(self) -> bool: + if not bool(self.get_parameter("color_scan_before_recipe").value): + self.get_logger().info("color_scan_before_recipe=false; skipping dispenser color scan") + return True + command = str(self.get_parameter("color_scan_command").value).strip() + if not command: + self.get_logger().warning("color_scan_command is empty; skipping dispenser color scan") + return True + self.get_logger().info("pick flow succeeded; moving to color_scan_pose and scanning dispensers") + return self._run_process(["bash", "-c", command], "color_scan") + + def _run_recipe_sequence(self) -> bool: + if not bool(self.get_parameter("recipe_after_success").value): + self.get_logger().info("recipe_after_success=false; skipping dispenser recipe sequence") + return True + command = str(self.get_parameter("recipe_command").value).strip() + if not command: + self.get_logger().warning("recipe_command is empty; skipping dispenser recipe sequence") + return True + self.get_logger().info("pick flow succeeded; starting integrated dispenser recipe sequence") + return self._run_process(["bash", "-c", command], "recipe") + + def _run_lid_shake_sequence(self) -> bool: + if not bool(self.get_parameter("lid_shake_after_recipe").value): + self.get_logger().info("lid_shake_after_recipe=false; skipping lid close / shake chain") + return True + command = str(self.get_parameter("lid_shake_command").value).strip() + if not command: + self.get_logger().warning("lid_shake_command is empty; skipping lid close / shake chain") + return True + self.get_logger().info("recipe succeeded; starting lid close -> holder re-pick -> shake chain") + return self._run_process(["bash", "-c", command], "lid_shake") + def _move_observe(self, label: str) -> bool: prefix = str(self.get_parameter("service_prefix").value).strip().strip("/") base = f"/{prefix}/motion" if prefix else "/motion" @@ -490,6 +559,31 @@ def _launch_command(spec: str) -> list[str]: def _split_extra_args(raw: str) -> list[str]: return [part for part in raw.split() if part] + @staticmethod + def _subprocess_env() -> dict[str, str]: + # 다른 워크스페이스(예: ~/ros2_ws)에 같은 이름의 stale 패키지가 있으면 + # 터미널 source 순서에 따라 자식 launch가 엉뚱한 사본을 잡을 수 있다. + # 이 라우터가 설치된 워크스페이스의 경로를 검색 변수 맨 앞으로 올려서 + # 자식 프로세스가 항상 같은 워크스페이스의 패키지를 먼저 찾게 한다. + env = os.environ.copy() + try: + from ament_index_python.packages import get_package_prefix + # install/ 두 단계 위 = 워크스페이스 루트. symlink 설치는 egg-info가 + # build/에 있으므로 install/만 올리면 entry point 탐색이 또 어긋난다. + ws_root = os.path.dirname(os.path.dirname(get_package_prefix("azas_task_manager"))) + os.sep + except Exception: + return env + for var in ("AMENT_PREFIX_PATH", "COLCON_PREFIX_PATH", "CMAKE_PREFIX_PATH", + "PYTHONPATH", "PATH", "LD_LIBRARY_PATH"): + value = env.get(var) + if not value: + continue + entries = value.split(os.pathsep) + own = [e for e in entries if e.startswith(ws_root) or e + os.sep == ws_root] + rest = [e for e in entries if e not in own] + env[var] = os.pathsep.join(own + rest) + return env + def _popen(self, cmd: list[str], label: str) -> subprocess.Popen[str]: proc = subprocess.Popen( cmd, @@ -498,6 +592,7 @@ def _popen(self, cmd: list[str], label: str) -> subprocess.Popen[str]: text=True, bufsize=1, preexec_fn=os.setsid, + env=self._subprocess_env(), ) self._children.append(proc) threading.Thread(target=self._forward_output, args=(proc, label), daemon=True).start() @@ -505,19 +600,39 @@ def _popen(self, cmd: list[str], label: str) -> subprocess.Popen[str]: def _run_process(self, cmd: list[str], label: str) -> bool: self.get_logger().info(f"{label}: " + " ".join(cmd)) + self._child_node_failures.pop(label, None) proc = self._popen(cmd, label) code = proc.wait() + # ros2 launch는 내부 노드가 죽어도 exit code 0으로 끝나므로 + # 출력에서 감지한 노드 비정상 종료를 별도로 확인한다. + failures = self._child_node_failures.pop(label, None) + if failures: + self.get_logger().error(f"{label}: node failure detected: " + "; ".join(failures)) + return False if code == 0: self.get_logger().info(f"{label}: completed successfully") return True self.get_logger().error(f"{label}: process exited with code {code}") return False + _NODE_DIED_PATTERN = re.compile(r"\[ERROR\] \[(?P[^\]]+)\]: process has died.*exit code (?P-?\d+)") + _SHUTDOWN_PATTERN = re.compile(r"sending signal 'SIG(INT|TERM)'|user interrupted with ctrl-c") + def _forward_output(self, proc: subprocess.Popen[str], label: str) -> None: if proc.stdout is None: return + shutting_down = False for line in proc.stdout: - self.get_logger().info(f"{label}> {line.rstrip()}") + text = line.rstrip() + self.get_logger().info(f"{label}> {text}") + if not shutting_down and self._SHUTDOWN_PATTERN.search(text): + shutting_down = True + match = self._NODE_DIED_PATTERN.search(text) + # launch 종료 신호 이후의 죽음(SIGINT 받은 KeyboardInterrupt 등)과 + # 음수 exit code(시그널 종료)는 정상 정리 과정이므로 제외 + if match and int(match.group("code")) > 0 and not shutting_down: + self._child_node_failures.setdefault(label, []).append( + f"{match.group('node')} exit code {match.group('code')}") def _stop_process(self, proc: Optional[subprocess.Popen[str]], label: str) -> None: if proc is None or proc.poll() is not None: diff --git a/tools/run/run_color_scan_stage.sh b/tools/run/run_color_scan_stage.sh new file mode 100755 index 0000000..46013d2 --- /dev/null +++ b/tools/run/run_color_scan_stage.sh @@ -0,0 +1,6 @@ +#!/usr/bin/env bash +# 색상 스캔 단계: color_scan_pose(joints 0,10,32,0,100,90)로 이동한 뒤 디스펜서 색상을 스캔한다. +# dispenser_color_scan_ros.sh가 outputs/dispenser_color_map.json을 새로 만들어야 +# run_color_recipe_sequence.py가 진행되므로, 이 단계는 레시피 전에 반드시 성공해야 한다. + +cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 tools/run/direct_movej_joints.py --service-prefix dsr01 --j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90 --velocity 30 --acceleration 30 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ && tools/run/dispenser_color_scan_ros.sh diff --git a/tools/run/run_lid_close_then_shake_chain.sh b/tools/run/run_lid_close_then_shake_chain.sh new file mode 100755 index 0000000..e87027e --- /dev/null +++ b/tools/run/run_lid_close_then_shake_chain.sh @@ -0,0 +1,6 @@ +#!/usr/bin/env bash +# 디스펜서 레시피 완료 후 체인: 뚜껑 닫기(ArUco 성공 감시) -> 컵홀더 재픽업 -> 쉐이킹 -> 카메라 포즈 복귀. +# robot_pipeline_control_server.py chain_shake_after_lid_command()가 생성하는 패널 체인과 동일한 명령을 +# auto_cup_flow_router가 직접 실행할 수 있도록 스크립트로 고정한 것이다. + +( cd /home/ssu/Azas && SERVICE_PREFIX=dsr01 DISPLAY=${DISPLAY:-:0} XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} MOVE_TO_LID_VIEW_POSE=true bash /home/ssu/Azas/tools/run/run_kang_lid_grip_close_direct.sh ) & lid_pid=$!; ( cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 /home/ssu/Azas/tools/run/wait_for_lid_grip_status.py --timeout-sec 900 --success-status motion_sequence_requested ) & wait_pid=$!; while true; do if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; if ! kill -0 ${lid_pid} 2>/dev/null; then wait ${lid_pid}; lid_rc=$?; sleep 1; if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; echo '[Azas] lid_grip_close launch exited before ArUco success status; shake chain blocked.'; kill -TERM ${wait_pid} 2>/dev/null || true; wait ${wait_pid} 2>/dev/null || true; if [ ${lid_rc} -eq 0 ]; then exit 1; else exit ${lid_rc}; fi; fi; sleep 1; done; kill -TERM ${lid_pid} 2>/dev/null || true; wait ${lid_pid} 2>/dev/null || true; if [ ${wait_rc} -eq 0 ]; then echo '[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹으로 바로 넘어갑니다.'; echo '[Azas] auto_holder_pick_then_shake=true'; cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && echo '[Azas] SHAKE START: 컵홀더에 놓인 닫힌 컵을 측정 pose로 다시 side-grip 픽업한 뒤 흔듭니다.' && echo '[Azas] 순서: RG2 open -> 컵홀더 retreat 접근 -> holder final pose에서 soft grasp -> holder lift -> 관절 쉐이킹.' && echo '[Azas] 주의: 컵 좌표를 새로 만들지 않고 calibration.yaml cup_holder.side_grip_place 측정값만 사용합니다.' && python3 tools/run/pick_from_cup_holder_side_grip.py --service-prefix dsr01 --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml --approach-velocity 12.0 --approach-acceleration 16.0 --descend-velocity 6.0 --descend-acceleration 10.0 --lift-velocity 12.0 --lift-acceleration 16.0 --place-final-z-offset-m -0.020 --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 --ikin-timeout-sec 20.0 --ikin-retries 2 --gripper-grasp-width-m 0.068 --gripper-force-n 35.0 --post-grasp-settle-sec 0.8 --z-max 0.28 --execute --confirm ENABLE_CUP_HOLDER_PICK && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=remove_world -p object_id:=tumbler_in_holder -p dispenser_id:=1 -p publish_once:=true && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=attach -p object_id:=carried_tumbler -p dispenser_id:=1 -p publish_once:=true && SERVICE_PREFIX=dsr01 GRASPED_CUP_TEST_MODE=true SKIP_CUP_HOLDER_PICK=true REQUIRE_ROBOT_STANDBY=true SHAKE_CONTROL_MODE=joint SHAKE_CYCLES=3 JOINT_SHAKE_BASE_J1_DEG=0.0 JOINT_SHAKE_BASE_J2_DEG=-35.0 JOINT_SHAKE_BASE_J3_DEG=50.0 JOINT_SHAKE_BASE_J4_DEG=0.0 JOINT_SHAKE_BASE_J5_DEG=70.0 JOINT_SHAKE_BASE_J6_DEG=0.0 JOINT_SHAKE_J3_AMPLITUDE_DEG=0.0 JOINT_SHAKE_J4_AMPLITUDE_DEG=18.0 JOINT_SHAKE_J5_AMPLITUDE_DEG=20.0 JOINT_SHAKE_J6_AMPLITUDE_DEG=24.0 JOINT_SHAKE_J1_MIN_DEG=-20.0 JOINT_SHAKE_J1_MAX_DEG=5.0 JOINT_SHAKE_J2_MIN_DEG=-80.0 JOINT_SHAKE_J2_MAX_DEG=5.0 JOINT_SHAKE_J3_MIN_DEG=0.0 JOINT_SHAKE_J3_MAX_DEG=135.0 JOINT_SHAKE_MAX_SINGLE_DELTA_DEG=75.0 ENFORCE_WRIST_JOINT_LIMITS=false WRIST_MIN_DEG=-135.0 WRIST_MAX_DEG=135.0 JOINT5_MIN_DEG=40.0 JOINT5_MAX_DEG=100.0 APPROACH_JOINT_VELOCITY=18.0 APPROACH_JOINT_ACCELERATION=22.0 APPROACH_JOINT_TIME=2.6 SHAKE_JOINT_VELOCITY=90.0 SHAKE_JOINT_ACCELERATION=120.0 SHAKE_JOINT_TIME=0.0 JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=130.0 VERIFY_JOINT_TARGETS=true JOINT_TARGET_TOLERANCE_DEG=8.0 JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true tools/run/run_rule_based_shake_real.sh && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && python3 tools/run/direct_movej_joints.py --service-prefix dsr01 --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 --velocity 15 --acceleration 15 --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ; else echo '[Azas] ArUco lid_grip_close 실패/타임아웃 -> 컵홀더 재픽업/쉐이킹을 건너뜁니다.'; exit ${wait_rc}; fi From 73632110833e6fcfbe18060d8ca989d5627f0a99 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 11:18:06 +0900 Subject: [PATCH 71/88] =?UTF-8?q?=ED=8C=8C=EC=9D=B4=ED=94=84=EB=9D=BC?= =?UTF-8?q?=EC=9D=B8=20=EB=A1=9C=EC=A7=81=20=EA=B5=AC=ED=98=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 8 ++++-- .../azas_task_manager/auto_cup_flow_router.py | 25 +++++++++++++------ 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index d7f99e3..e06d1d4 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -24,8 +24,10 @@ def generate_launch_description(): DeclareLaunchArgument("side_extra_args", default_value=""), DeclareLaunchArgument("cup_uprighting_extra_args", default_value=""), DeclareLaunchArgument("side_target_x_offset_m", default_value="-0.02"), - DeclareLaunchArgument("color_scan_before_recipe", default_value="true"), + DeclareLaunchArgument("color_scan_at_start", default_value="true"), DeclareLaunchArgument("recipe_after_success", default_value="true"), + DeclareLaunchArgument("recipe_colors", default_value=""), + DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.04"), DeclareLaunchArgument("lid_shake_after_recipe", default_value="true"), Node( package="azas_task_manager", @@ -50,8 +52,10 @@ def generate_launch_description(): "side_extra_args": LaunchConfiguration("side_extra_args"), "cup_uprighting_extra_args": LaunchConfiguration("cup_uprighting_extra_args"), "side_target_x_offset_m": ParameterValue(LaunchConfiguration("side_target_x_offset_m"), value_type=float), - "color_scan_before_recipe": ParameterValue(LaunchConfiguration("color_scan_before_recipe"), value_type=bool), + "color_scan_at_start": ParameterValue(LaunchConfiguration("color_scan_at_start"), value_type=bool), "recipe_after_success": ParameterValue(LaunchConfiguration("recipe_after_success"), value_type=bool), + "recipe_colors": LaunchConfiguration("recipe_colors"), + "cup_holder_place_z_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_z_offset_m"), value_type=float), "lid_shake_after_recipe": ParameterValue(LaunchConfiguration("lid_shake_after_recipe"), value_type=bool), }], ), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index bc784a4..025dffb 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -2,6 +2,7 @@ import os import re +import shlex import signal import subprocess import sys @@ -75,7 +76,7 @@ def __init__(self) -> None: # 사이드 그립에서 base x가 +20mm 정도 어긋나는 실측 보정값 self.declare_parameter("side_target_x_offset_m", -0.02) - self.declare_parameter("color_scan_before_recipe", True) + self.declare_parameter("color_scan_at_start", True) self.declare_parameter( "color_scan_command", "bash /home/ssu/Azas/tools/run/run_color_scan_stage.sh", @@ -95,6 +96,10 @@ def __init__(self) -> None: "export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && " "python3 tools/run/run_color_recipe_sequence.py --execute --confirm", ) + # 키오스크/음성 주문(latest_recipe.json) 없이 색을 직접 내릴 때: 예) "red:2,blue:1" + self.declare_parameter("recipe_colors", "") + # 디스펜서 누르기 종료 후 컵홀더에 놓을 때 z를 더 낮추는 실측 보정값 + self.declare_parameter("cup_holder_place_z_offset_m", -0.04) self.declare_parameter("lid_shake_after_recipe", True) self.declare_parameter( "lid_shake_command", @@ -125,9 +130,11 @@ def run(self) -> int: if not self._confirmed(): return 2 - self.get_logger().info("auto cup router: observe -> open -> classify -> route") + self.get_logger().info("auto cup router: color scan -> observe -> open -> classify -> route") perception = None try: + if not self._run_color_scan_sequence(): + return 1 if not self._move_observe("initial observe"): return 1 if not self._open_gripper("initial gripper full-open"): @@ -147,8 +154,6 @@ def run(self) -> int: success = self._run_cup_uprighting(decision) if not success: return 1 - if not self._run_color_scan_sequence(): - return 1 if not self._run_recipe_sequence(): return 1 if not self._run_lid_shake_sequence(): @@ -458,14 +463,14 @@ def _run_cup_uprighting(self, decision: RouteDecision) -> bool: return self._run_process(cmd, "cup_uprighting") def _run_color_scan_sequence(self) -> bool: - if not bool(self.get_parameter("color_scan_before_recipe").value): - self.get_logger().info("color_scan_before_recipe=false; skipping dispenser color scan") + if not bool(self.get_parameter("color_scan_at_start").value): + self.get_logger().info("color_scan_at_start=false; skipping dispenser color scan") return True command = str(self.get_parameter("color_scan_command").value).strip() if not command: self.get_logger().warning("color_scan_command is empty; skipping dispenser color scan") return True - self.get_logger().info("pick flow succeeded; moving to color_scan_pose and scanning dispensers") + self.get_logger().info("moving to color_scan_pose and scanning dispensers before cup pick") return self._run_process(["bash", "-c", command], "color_scan") def _run_recipe_sequence(self) -> bool: @@ -476,6 +481,12 @@ def _run_recipe_sequence(self) -> bool: if not command: self.get_logger().warning("recipe_command is empty; skipping dispenser recipe sequence") return True + colors = str(self.get_parameter("recipe_colors").value).strip() + if colors: + command += f" --colors {shlex.quote(colors)}" + self.get_logger().info(f"recipe colors given directly: {colors}") + place_z = float(self.get_parameter("cup_holder_place_z_offset_m").value) + command += f" --cup-holder-place-final-z-offset-m {place_z}" self.get_logger().info("pick flow succeeded; starting integrated dispenser recipe sequence") return self._run_process(["bash", "-c", command], "recipe") From aaa5fc1770655beac69a351438e67a9c652adbb7 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 11:58:41 +0900 Subject: [PATCH 72/88] =?UTF-8?q?2=EC=B0=A8=20=ED=85=8C=EC=8A=A4=ED=8A=B8?= =?UTF-8?q?=20=EC=84=B1=EA=B3=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 8 +++- .../azas_cup_uprighting/_base_node.py | 5 +- .../launch/yolo_cup_uprighting.launch.py | 5 +- .../azas_task_manager/auto_cup_flow_router.py | 46 +++++++++++++------ tools/run/run_color_recipe_sequence.py | 3 ++ 5 files changed, 50 insertions(+), 17 deletions(-) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index e06d1d4..ef94bba 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -27,7 +27,10 @@ def generate_launch_description(): DeclareLaunchArgument("color_scan_at_start", default_value="true"), DeclareLaunchArgument("recipe_after_success", default_value="true"), DeclareLaunchArgument("recipe_colors", default_value=""), - DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.04"), + DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="-0.02"), + DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.03"), + DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="0.0"), + DeclareLaunchArgument("cup_holder_z_min_m", default_value="0.08"), DeclareLaunchArgument("lid_shake_after_recipe", default_value="true"), Node( package="azas_task_manager", @@ -55,7 +58,10 @@ def generate_launch_description(): "color_scan_at_start": ParameterValue(LaunchConfiguration("color_scan_at_start"), value_type=bool), "recipe_after_success": ParameterValue(LaunchConfiguration("recipe_after_success"), value_type=bool), "recipe_colors": LaunchConfiguration("recipe_colors"), + "final_regrasp_z_offset_m": ParameterValue(LaunchConfiguration("final_regrasp_z_offset_m"), value_type=float), "cup_holder_place_z_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_z_offset_m"), value_type=float), + "cup_holder_place_y_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_y_offset_m"), value_type=float), + "cup_holder_z_min_m": ParameterValue(LaunchConfiguration("cup_holder_z_min_m"), value_type=float), "lid_shake_after_recipe": ParameterValue(LaunchConfiguration("lid_shake_after_recipe"), value_type=bool), }], ), diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py index fe23838..44a8cd9 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -505,4 +505,7 @@ def run_node(node_cls): node.run() finally: node.destroy_node() - rclpy.shutdown() + # exit_after_pick 경로 등에서 컨텍스트가 이미 닫혀 있으면 재호출 시 + # RuntimeError로 exit code 1이 되어 라우터가 실패로 오인한다. + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py index 2e26847..a5d73d1 100644 --- a/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py +++ b/src/azas_cup_uprighting/launch/yolo_cup_uprighting.launch.py @@ -1,7 +1,7 @@ from copy import deepcopy from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, Shutdown from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node @@ -54,6 +54,9 @@ def _runtime_nodes(context, moveit_params, moveit_py_params): "controller_action_wait_sec": 60.0, }, ], + # exit_after_pick으로 메인 노드가 끝나면 collision/tf 보조 노드들도 함께 + # 정리해 launch가 종료되도록 한다. 안 그러면 라우터가 영원히 대기한다. + on_exit=Shutdown(), ) return [yolo_cup_uprighting_node] diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 025dffb..00ba5b2 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -98,8 +98,12 @@ def __init__(self) -> None: ) # 키오스크/음성 주문(latest_recipe.json) 없이 색을 직접 내릴 때: 예) "red:2,blue:1" self.declare_parameter("recipe_colors", "") - # 디스펜서 누르기 종료 후 컵홀더에 놓을 때 z를 더 낮추는 실측 보정값 - self.declare_parameter("cup_holder_place_z_offset_m", -0.04) + # 디스펜서 누르기 종료 후 디스펜서 앞의 컵을 마지막으로 재파지할 때 z 실측 보정값 + self.declare_parameter("final_regrasp_z_offset_m", -0.02) + # 컵홀더에 놓을 때 보정값과 place 목표 z 안전 하한 (필요 시 조정) + self.declare_parameter("cup_holder_place_z_offset_m", -0.03) + self.declare_parameter("cup_holder_place_y_offset_m", 0.0) + self.declare_parameter("cup_holder_z_min_m", 0.08) self.declare_parameter("lid_shake_after_recipe", True) self.declare_parameter( "lid_shake_command", @@ -485,8 +489,16 @@ def _run_recipe_sequence(self) -> bool: if colors: command += f" --colors {shlex.quote(colors)}" self.get_logger().info(f"recipe colors given directly: {colors}") + regrasp_z = float(self.get_parameter("final_regrasp_z_offset_m").value) place_z = float(self.get_parameter("cup_holder_place_z_offset_m").value) - command += f" --cup-holder-place-final-z-offset-m {place_z}" + place_y = float(self.get_parameter("cup_holder_place_y_offset_m").value) + z_min = float(self.get_parameter("cup_holder_z_min_m").value) + command += ( + f" --final-regrasp-extra-z-offset-m {regrasp_z}" + f" --cup-holder-place-final-z-offset-m {place_z}" + f" --cup-holder-place-final-y-offset-m {place_y}" + f" --cup-holder-z-min-m {z_min}" + ) self.get_logger().info("pick flow succeeded; starting integrated dispenser recipe sequence") return self._run_process(["bash", "-c", command], "recipe") @@ -632,18 +644,24 @@ def _run_process(self, cmd: list[str], label: str) -> bool: def _forward_output(self, proc: subprocess.Popen[str], label: str) -> None: if proc.stdout is None: return + # 단계별 출력을 파일로도 남겨 실패 시 터미널 스크롤백 없이 진단할 수 있게 한다. + log_dir = "/tmp/azas_router_logs" + os.makedirs(log_dir, exist_ok=True) + log_path = os.path.join(log_dir, f"{label}_{time.strftime('%Y%m%d_%H%M%S')}_{proc.pid}.log") shutting_down = False - for line in proc.stdout: - text = line.rstrip() - self.get_logger().info(f"{label}> {text}") - if not shutting_down and self._SHUTDOWN_PATTERN.search(text): - shutting_down = True - match = self._NODE_DIED_PATTERN.search(text) - # launch 종료 신호 이후의 죽음(SIGINT 받은 KeyboardInterrupt 등)과 - # 음수 exit code(시그널 종료)는 정상 정리 과정이므로 제외 - if match and int(match.group("code")) > 0 and not shutting_down: - self._child_node_failures.setdefault(label, []).append( - f"{match.group('node')} exit code {match.group('code')}") + with open(log_path, "w", encoding="utf-8", errors="replace") as log_file: + for line in proc.stdout: + text = line.rstrip() + self.get_logger().info(f"{label}> {text}") + log_file.write(text + "\n") + if not shutting_down and self._SHUTDOWN_PATTERN.search(text): + shutting_down = True + match = self._NODE_DIED_PATTERN.search(text) + # launch 종료 신호 이후의 죽음(SIGINT 받은 KeyboardInterrupt 등)과 + # 음수 exit code(시그널 종료)는 정상 정리 과정이므로 제외 + if match and int(match.group("code")) > 0 and not shutting_down: + self._child_node_failures.setdefault(label, []).append( + f"{match.group('node')} exit code {match.group('code')}") def _stop_process(self, proc: Optional[subprocess.Popen[str]], label: str) -> None: if proc is None or proc.poll() is not None: diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 04f0f75..060ef4d 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -369,6 +369,8 @@ def main() -> int: ) parser.add_argument("--cup-holder-place-final-z-offset-m", default="-0.030") parser.add_argument("--cup-holder-place-final-y-offset-m", default="-0.010") + parser.add_argument("--cup-holder-z-min-m", default="0.08", + help="컵홀더 place 목표 z 안전 하한. place z offset을 크게 낮출 때 함께 내려야 함") parser.add_argument("--cup-holder-approach-velocity", default="80.0") parser.add_argument("--cup-holder-approach-acceleration", default="20.0") parser.add_argument("--cup-holder-place-velocity", default="80.0") @@ -450,6 +452,7 @@ def main() -> int: "--gripper-settle-seconds", str(args.gripper_settle_seconds), "--cup-holder-place-final-z-offset-m", str(args.cup_holder_place_final_z_offset_m), "--cup-holder-place-final-y-offset-m", str(args.cup_holder_place_final_y_offset_m), + "--cup-holder-z-min-m", str(args.cup_holder_z_min_m), "--cup-holder-approach-velocity", str(args.cup_holder_approach_velocity), "--cup-holder-approach-acceleration", str(args.cup_holder_approach_acceleration), "--cup-holder-place-velocity", str(args.cup_holder_place_velocity), From c9be6bfacf9eeb85c206d8f624815e6d92d73b85 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 13:39:36 +0900 Subject: [PATCH 73/88] =?UTF-8?q?=EC=89=90=EC=9D=B4=ED=82=B9=EC=A0=84=20?= =?UTF-8?q?=EB=B0=8F=20stt=EB=A1=9C=EC=A7=81=20=EC=A0=9C=EC=99=B8=20?= =?UTF-8?q?=EC=9E=91=EB=8F=99=20=EA=B0=80=EB=8A=A5=20=ED=99=95=EC=9D=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/azas_bringup/launch/auto_cup_flow_router.launch.py | 2 ++ .../azas_task_manager/auto_cup_flow_router.py | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index ef94bba..f058c0e 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -27,6 +27,7 @@ def generate_launch_description(): DeclareLaunchArgument("color_scan_at_start", default_value="true"), DeclareLaunchArgument("recipe_after_success", default_value="true"), DeclareLaunchArgument("recipe_colors", default_value=""), + DeclareLaunchArgument("cup_pre_from_place_x_offset_m", default_value="-0.12"), DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="-0.02"), DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.03"), DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="0.0"), @@ -58,6 +59,7 @@ def generate_launch_description(): "color_scan_at_start": ParameterValue(LaunchConfiguration("color_scan_at_start"), value_type=bool), "recipe_after_success": ParameterValue(LaunchConfiguration("recipe_after_success"), value_type=bool), "recipe_colors": LaunchConfiguration("recipe_colors"), + "cup_pre_from_place_x_offset_m": ParameterValue(LaunchConfiguration("cup_pre_from_place_x_offset_m"), value_type=float), "final_regrasp_z_offset_m": ParameterValue(LaunchConfiguration("final_regrasp_z_offset_m"), value_type=float), "cup_holder_place_z_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_z_offset_m"), value_type=float), "cup_holder_place_y_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_y_offset_m"), value_type=float), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 00ba5b2..ccd3067 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -100,6 +100,8 @@ def __init__(self) -> None: self.declare_parameter("recipe_colors", "") # 디스펜서 누르기 종료 후 디스펜서 앞의 컵을 마지막으로 재파지할 때 z 실측 보정값 self.declare_parameter("final_regrasp_z_offset_m", -0.02) + # 잡기 직전 pre 위치(cup_place 기준 X offset) 보정값. 스크립트 기본 -0.09에 -30mm 추가 + self.declare_parameter("cup_pre_from_place_x_offset_m", -0.12) # 컵홀더에 놓을 때 보정값과 place 목표 z 안전 하한 (필요 시 조정) self.declare_parameter("cup_holder_place_z_offset_m", -0.03) self.declare_parameter("cup_holder_place_y_offset_m", 0.0) @@ -489,6 +491,8 @@ def _run_recipe_sequence(self) -> bool: if colors: command += f" --colors {shlex.quote(colors)}" self.get_logger().info(f"recipe colors given directly: {colors}") + cup_pre_x = float(self.get_parameter("cup_pre_from_place_x_offset_m").value) + command += f" --cup-pre-from-place-x-offset-m {cup_pre_x}" regrasp_z = float(self.get_parameter("final_regrasp_z_offset_m").value) place_z = float(self.get_parameter("cup_holder_place_z_offset_m").value) place_y = float(self.get_parameter("cup_holder_place_y_offset_m").value) From bc5bbd796402b4eeb2a8b4bc95c4eb36a990fbbf Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 15:48:48 +0900 Subject: [PATCH 74/88] =?UTF-8?q?3=EC=B0=A8=20=ED=85=8C=EC=8A=A4=ED=8A=B8?= =?UTF-8?q?=20side=20grip=EC=84=B1=EA=B3=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../azas_task_manager/auto_cup_flow_router.py | 6 +-- .../dsr_practice/yolo_cup_pick_node.py | 21 ++++++-- tools/perception/dispenser_color_scan.py | 50 +++++++++++++++---- tools/run/run_kang_lid_grip_close_direct.sh | 19 ++++--- tools/run/stop_azas_all.sh | 4 +- 5 files changed, 73 insertions(+), 27 deletions(-) diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index ccd3067..51665e3 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -661,9 +661,9 @@ def _forward_output(self, proc: subprocess.Popen[str], label: str) -> None: if not shutting_down and self._SHUTDOWN_PATTERN.search(text): shutting_down = True match = self._NODE_DIED_PATTERN.search(text) - # launch 종료 신호 이후의 죽음(SIGINT 받은 KeyboardInterrupt 등)과 - # 음수 exit code(시그널 종료)는 정상 정리 과정이므로 제외 - if match and int(match.group("code")) > 0 and not shutting_down: + # launch 종료 신호 이후의 죽음(SIGINT 받은 KeyboardInterrupt 등)만 정상 정리로 + # 간주한다. 종료 신호 전이라면 음수 exit code(SIGSEGV -11 등)도 실패다. + if match and not shutting_down: self._child_node_failures.setdefault(label, []).append( f"{match.group('node')} exit code {match.group('code')}") diff --git a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py index 819822b..38fe04f 100644 --- a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -48,6 +48,17 @@ ] ARM_JOINT_ORDER = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"] + +def set_arm_joint_positions(state, joint_positions): + """관절값을 dict 기반 joint_positions 세터로 넣는다. + + moveit_py의 set_joint_group_positions(Eigen 바인딩)는 user-site NumPy 2.x + 환경에서 list/ndarray 인자 모두 세그폴트(exit -11)하므로 사용 금지. + """ + state.joint_positions = { + name: float(value) for name, value in zip(ARM_JOINT_ORDER, joint_positions) + } + SAFE_X_MIN = 0.0 SAFE_Y_MIN = -0.35 SAFE_Y_MAX = 0.35 @@ -1071,7 +1082,7 @@ def move_to_side_prepose_if_configured(self, cup_base_xyz: np.ndarray) -> bool: ) return False joint_positions = [target_by_name[name] for name in ARM_JOINT_ORDER] - target_state.set_joint_group_positions(GROUP_NAME, joint_positions) + set_arm_joint_positions(target_state, joint_positions) target_state.update() return self.plan_and_execute( state_goal=target_state, @@ -1313,7 +1324,7 @@ def verify_joint_goal_reached(self, joint_names, joint_positions): def move_joint_home(self): home_state = RobotState(self.robot_model) - home_state.set_joint_group_positions(GROUP_NAME, HOME_JOINTS_RAD) + set_arm_joint_positions(home_state, HOME_JOINTS_RAD) home_state.update() if not self.plan_and_execute( state_goal=home_state, @@ -1335,7 +1346,7 @@ def move_home(self): def move_camera_joint_home(self): log = self.get_logger() target_state = RobotState(self.robot_model) - target_state.set_joint_group_positions(GROUP_NAME, self.camera_home_joint_positions) + set_arm_joint_positions(target_state, self.camera_home_joint_positions) target_state.update() joint_degrees = [math.degrees(value) for value in self.camera_home_joint_positions] log.info( @@ -1397,7 +1408,7 @@ def current_robot_state_from_joint_states(self, timeout_sec=1.0): return None state = RobotState(self.robot_model) joint_positions = [float(joint_map[name]) for name in ARM_JOINT_ORDER] - state.set_joint_group_positions(GROUP_NAME, joint_positions) + set_arm_joint_positions(state, joint_positions) state.update() return state @@ -1423,7 +1434,7 @@ def move_joint1_clearance_before_side_grip(self): f"joint_1 {before_deg:.1f} -> {before_deg + delta:.1f} deg" ) target_state = RobotState(self.robot_model) - target_state.set_joint_group_positions(GROUP_NAME, joint_positions) + set_arm_joint_positions(target_state, joint_positions) target_state.update() return self.plan_and_execute( state_goal=target_state, diff --git a/tools/perception/dispenser_color_scan.py b/tools/perception/dispenser_color_scan.py index c5fc921..155f1d1 100644 --- a/tools/perception/dispenser_color_scan.py +++ b/tools/perception/dispenser_color_scan.py @@ -42,6 +42,13 @@ BASE_FRAME = "base_link" EE_LINK = "link_6" CROP_HALF_PX = 60 # half-side of crop box around projected pixel +# Visible-handle detection can transiently miss a handle (operator arm in +# frame); keep retrying on fresh frames for this long before TF fallback. +VISIBLE_RETRY_SEC = 6.0 +# TF projection this far outside the frame means stale extrinsics, not a +# handle "just off-screen"; edge-crop classification there is confidently +# wrong (e.g. everything "blue" from the chair/arm strip), so report unknown. +EDGE_CLAMP_MAX_PX = 40 # HSV ranges for the physical dispenser handle colors in the current booth. @@ -383,15 +390,31 @@ def info_cb(msg: "CameraInfo") -> None: f"settle_sec={settle_sec:.2f} sample_frames={frame_count} size={frame_bgr.shape[1]}x{frame_bgr.shape[0]}" ) if visible_handle_fallback: - visible_map = detect_visible_handle_color_map( - frame_bgr, - dispenser_ids, - debug_image_path=debug_image_path, - ) - if visible_map is not None: - node.destroy_node() - rclpy.shutdown() - return visible_map + # 한 프레임만 보면 일시적 가림(작업자 팔 등)으로 핸들 하나가 빠져 + # 4/4 검출 전체가 버려지고 TF 투영으로 떨어진다. 새 프레임을 받아 + # 잠시 재시도해서 일시적 가림을 흡수한다. + retry_deadline = time.time() + VISIBLE_RETRY_SEC + seen_count = frame_count + while True: + visible_map = detect_visible_handle_color_map( + frame_bgr, + dispenser_ids, + debug_image_path=debug_image_path, + ) + if visible_map is not None: + node.destroy_node() + rclpy.shutdown() + return visible_map + while rclpy.ok() and time.time() < retry_deadline and frame_count == seen_count: + rclpy.spin_once(node, timeout_sec=0.1) + if frame_count == seen_count: + print( + f"[dispenser_color_scan] visible-handle detection failed for {VISIBLE_RETRY_SEC:.0f}s; " + "using TF projection", + file=sys.stderr, + ) + break + seen_count = frame_count T_gripper2cam = load_hand_eye() if T_gripper2cam is None: @@ -454,6 +477,15 @@ def info_cb(msg: "CameraInfo") -> None: if clamp_out_of_frame: clamped_u = min(max(u, 0), img_w - 1) clamped_v = min(max(v, 0), img_h - 1) + overshoot = max(abs(u - clamped_u), abs(v - clamped_v)) + if overshoot > EDGE_CLAMP_MAX_PX: + print( + f"[dispenser_color_scan] dispenser {did}: projected pixel ({u},{v}) is " + f"{overshoot}px outside frame {img_w}x{img_h} (stale extrinsics?); fallback unknown", + file=sys.stderr, + ) + color_map[did] = "unknown" + continue x1 = max(0, clamped_u - CROP_HALF_PX) x2 = min(img_w, clamped_u + CROP_HALF_PX) y1 = max(0, clamped_v - CROP_HALF_PX) diff --git a/tools/run/run_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh index fec0f5d..c79c55e 100755 --- a/tools/run/run_kang_lid_grip_close_direct.sh +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -46,7 +46,7 @@ echo "[Azas] OpenCV window: confirm lid ArUco, then press p. Quit with q/Esc." echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XAUTHORITY}" echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY} FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS}" echo "[Azas] aruco=${ARUCO_DICTIONARY}:${ARUCO_MARKER_ID} fallback=${ARUCO_FALLBACK_MARKERS} length_m=${ARUCO_MARKER_LENGTH_M}" -echo "[Azas] note: use_j6_yaw_for_pick/pick_j6_* are not supported by this Azas launch; using supported ArUco-axis orientation parameters." +echo "[Azas] orientation: use_j6_yaw_for_pick=true (pick_j6_yaw_axis=y sign=-1.0 offset=1.2deg), preseat=j6_step_wiggle" if [[ ! -f "${MODEL_PATH}" ]]; then echo "[Azas][WARN] model_path not found: ${MODEL_PATH}" @@ -79,7 +79,10 @@ launch_args=( aruco_dictionary:="${ARUCO_DICTIONARY}" aruco_marker_id:="${ARUCO_MARKER_ID}" \ aruco_marker_length_m:="${ARUCO_MARKER_LENGTH_M}" \ use_aruco_axis_for_orientation:=true aruco_finger_axis_quarter_turns:=0 \ - use_lid_pose_yaw_for_pick:=false lid_pose_yaw_axis:=y lid_pose_yaw_offset_deg:=0.0 lid_pose_yaw_equivalence_deg:=360.0 \ + use_lid_pose_yaw_for_pick:=false \ + use_j6_yaw_for_pick:=true pick_j6_yaw_axis:=y pick_j6_yaw_sign:=-1.0 \ + pick_j6_yaw_offset_deg:=1.2 pick_j6_yaw_equivalence_deg:=360.0 pick_j6_yaw_tolerance_deg:=1.0 \ + pick_j6_velocity:=30.0 pick_j6_acceleration:=15.0 \ visual_refine_before_grasp:=true visual_refine_sample_count:=5 visual_refine_timeout_sec:=3.0 visual_refine_max_yaw_std_deg:=5.0 \ visual_refine_max_position_std_m:=0.005 visual_refine_apply_xy:=true visual_refine_apply_yaw:=true visual_refine_fallback_to_initial_plan:=true \ enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION allow_service_control_without_moveit:=true service_prefix:="${SERVICE_PREFIX}" \ @@ -95,15 +98,15 @@ launch_args=( enable_lid_twist_after_grasp:=true \ lid_twist_target_x_m:=0.422959106 lid_twist_target_y_m:=0.223224869 lid_twist_target_z_m:=0.166827988 \ lid_twist_rx:=73.901489 lid_twist_ry:=-178.542740 lid_twist_rz:=117.385612 \ - lid_twist_transfer_clearance_m:=0.20 lid_twist_transfer_max_z_m:=0.60 \ + lid_twist_transfer_clearance_m:=0.10 lid_twist_transfer_max_z_m:=0.60 \ lid_twist_use_force_control:=false lid_twist_use_force_spiral:=true lid_twist_force_rotation_mode:=j6 \ + lid_twist_press_down_m:=0.0 \ lid_twist_down_force_n:=2.0 lid_twist_force_ref:=base lid_twist_force_service_timeout_sec:=20.0 \ lid_twist_force_settle_seconds:=0.2 lid_twist_force_release_time:=0.2 \ - lid_twist_preseat_periodic_before_turn:=true \ - lid_twist_preseat_periodic_x_amp_mm:=0.0 lid_twist_preseat_periodic_y_amp_mm:=0.0 lid_twist_preseat_periodic_z_amp_mm:=1.0 \ - lid_twist_preseat_periodic_rx_amp_deg:=0.0 lid_twist_preseat_periodic_ry_amp_deg:=0.0 lid_twist_preseat_periodic_rz_amp_deg:=10.0 \ - lid_twist_preseat_periodic_period_sec:=3.6 lid_twist_preseat_periodic_acc_time_sec:=1.0 lid_twist_preseat_periodic_repeat:=2 \ - lid_twist_preseat_periodic_ref:=tool lid_twist_rz_delta_deg:=360.0 lid_twist_turn_step_deg:=60.0 \ + lid_twist_preseat_periodic_before_turn:=true lid_twist_preseat_mode:=j6_step_wiggle \ + lid_twist_preseat_periodic_descend_m:=0.02 lid_twist_preseat_step_m:=0.005 \ + lid_twist_preseat_wiggle_deg:=10.0 lid_twist_preseat_wiggle_velocity:=50.0 lid_twist_preseat_down_velocity:=8.0 \ + lid_twist_rz_delta_deg:=360.0 lid_twist_turn_step_deg:=60.0 \ lid_twist_release_lift_m:=0.03 lid_twist_min_z_m:=0.140 lid_twist_max_z_m:=0.260 \ lid_twist_transfer_velocity:=25.0 lid_twist_press_velocity:=10.0 lid_twist_turn_velocity:=40.0 lid_twist_acceleration:=15.0 \ lid_twist_hold_seconds_before_turn:=0.2 lid_twist_hold_seconds_after_turn:=0.5 \ diff --git a/tools/run/stop_azas_all.sh b/tools/run/stop_azas_all.sh index 1be0ecd..a5ee890 100755 --- a/tools/run/stop_azas_all.sh +++ b/tools/run/stop_azas_all.sh @@ -22,9 +22,9 @@ CLEAN_FASTDDS_SHM="${CLEAN_FASTDDS_SHM:-1}" SESSIONS="${SESSIONS:-azas-logic azas-rviz-exact}" GRACE_SEC="${GRACE_SEC:-6}" -ROS_PATTERN='run_doosan_real_m0609\.sh|dsr_bringup2|run_emulator|/DRCF|ros2_control_node|robot_state_publisher|move_group|rviz2|rg2_trigger|rg2_gripper_node|rs_launch\.py|realsense2_camera_node|joint_state_relay\.py|yolo_cup_pick_node|hand_eye_static_tf_node|static_transform_publisher|link6_gripper_collision_node|measured_dispenser_collision_scene_node|collision_scene_rviz_publisher\.py|publish_color_recipe_sequence_rviz_preview\.py|publish_collision_scene_rviz\.py|lid_sticker_detector_node|lid_grip_planner_node|lid_detection_pose_bridge_node|dispenser_sequence|run_changhyun_side_grip_direct\.sh|run_kang_lid_grip_close_direct\.sh|run_somyeong_cup_uprighting_direct\.sh|run_tmux_logic_sequence\.sh|run_color_recipe_sequence\.py|run_measured_dispenser_recipe_sequence\.py|run_minimal_dispenser_cycle\.py|/opt/ros/humble/bin/ros2 |ros2cli\.daemon' +ROS_PATTERN='run_doosan_real_m0609\.sh|dsr_bringup2|run_emulator|/DRCF|ros2_control_node|robot_state_publisher|move_group|rviz2|rg2_trigger|rg2_gripper_node|rs_launch\.py|realsense2_camera_node|joint_state_relay\.py|yolo_cup_pick_node|hand_eye_static_tf_node|static_transform_publisher|link6_gripper_collision_node|measured_dispenser_collision_scene_node|workspace_collision_scene_node|yolo_cup_uprighting|collision_scene_rviz_publisher\.py|publish_color_recipe_sequence_rviz_preview\.py|publish_collision_scene_rviz\.py|lid_sticker_detector_node|lid_grip_planner_node|lid_detection_pose_bridge_node|dispenser_sequence|run_changhyun_side_grip_direct\.sh|run_kang_lid_grip_close_direct\.sh|run_somyeong_cup_uprighting_direct\.sh|run_tmux_logic_sequence\.sh|run_color_recipe_sequence\.py|run_measured_dispenser_recipe_sequence\.py|run_minimal_dispenser_cycle\.py|/opt/ros/humble/bin/ros2 |ros2cli\.daemon' -PROTECT_PATTERN='codex|oh-my-codex|omx|claude|bwrap|stop_azas_all\.sh|(^|[ /])tmux( |$|:)' +PROTECT_PATTERN='codex|oh-my-codex|omx|claude|bwrap|stop_azas_all\.sh|grep -E|(^|[ /])tmux( |$|:)' if [[ "${KILL_PANEL}" != "1" && "${KILL_PANEL}" != "true" ]]; then PROTECT_PATTERN="${PROTECT_PATTERN}|robot_pipeline_control_server\.py|run_robot_pipeline_control_panel\.sh" fi From e8a3ad3e46e5be6e9499e50639c045a165eb2e9b Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 19:12:45 +0900 Subject: [PATCH 75/88] =?UTF-8?q?4=EC=B0=A8=20=ED=85=8C=EC=8A=A4=ED=8A=B8?= =?UTF-8?q?=20=EC=99=84=EB=A3=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 2 + .../azas_task_manager/auto_cup_flow_router.py | 3 + tools/checks/check_dispenser_color_scan.py | 50 +++++++++++++ tools/perception/dispenser_color_scan.py | 20 ++++-- tools/run/place_side_grip_cup_in_holder.py | 2 +- tools/run/run_color_recipe_sequence.py | 2 + tools/run/run_kang_lid_grip_close_direct.sh | 2 +- .../run_measured_dispenser_recipe_sequence.py | 25 ++++++- tools/run/run_stt_order_then_router.sh | 70 +++++++++++++++++++ 9 files changed, 167 insertions(+), 9 deletions(-) create mode 100755 tools/run/run_stt_order_then_router.sh diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index f058c0e..8e36d7f 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -28,6 +28,7 @@ def generate_launch_description(): DeclareLaunchArgument("recipe_after_success", default_value="true"), DeclareLaunchArgument("recipe_colors", default_value=""), DeclareLaunchArgument("cup_pre_from_place_x_offset_m", default_value="-0.12"), + DeclareLaunchArgument("dispenser_3_cup_pre_extra_x_offset_m", default_value="-0.01"), DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="-0.02"), DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.03"), DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="0.0"), @@ -60,6 +61,7 @@ def generate_launch_description(): "recipe_after_success": ParameterValue(LaunchConfiguration("recipe_after_success"), value_type=bool), "recipe_colors": LaunchConfiguration("recipe_colors"), "cup_pre_from_place_x_offset_m": ParameterValue(LaunchConfiguration("cup_pre_from_place_x_offset_m"), value_type=float), + "dispenser_3_cup_pre_extra_x_offset_m": ParameterValue(LaunchConfiguration("dispenser_3_cup_pre_extra_x_offset_m"), value_type=float), "final_regrasp_z_offset_m": ParameterValue(LaunchConfiguration("final_regrasp_z_offset_m"), value_type=float), "cup_holder_place_z_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_z_offset_m"), value_type=float), "cup_holder_place_y_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_y_offset_m"), value_type=float), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 51665e3..1c54cc2 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -102,6 +102,7 @@ def __init__(self) -> None: self.declare_parameter("final_regrasp_z_offset_m", -0.02) # 잡기 직전 pre 위치(cup_place 기준 X offset) 보정값. 스크립트 기본 -0.09에 -30mm 추가 self.declare_parameter("cup_pre_from_place_x_offset_m", -0.12) + self.declare_parameter("dispenser_3_cup_pre_extra_x_offset_m", -0.01) # 컵홀더에 놓을 때 보정값과 place 목표 z 안전 하한 (필요 시 조정) self.declare_parameter("cup_holder_place_z_offset_m", -0.03) self.declare_parameter("cup_holder_place_y_offset_m", 0.0) @@ -493,6 +494,8 @@ def _run_recipe_sequence(self) -> bool: self.get_logger().info(f"recipe colors given directly: {colors}") cup_pre_x = float(self.get_parameter("cup_pre_from_place_x_offset_m").value) command += f" --cup-pre-from-place-x-offset-m {cup_pre_x}" + dispenser_3_pre_x = float(self.get_parameter("dispenser_3_cup_pre_extra_x_offset_m").value) + command += f" --dispenser-3-cup-pre-extra-x-offset-m {dispenser_3_pre_x}" regrasp_z = float(self.get_parameter("final_regrasp_z_offset_m").value) place_z = float(self.get_parameter("cup_holder_place_z_offset_m").value) place_y = float(self.get_parameter("cup_holder_place_y_offset_m").value) diff --git a/tools/checks/check_dispenser_color_scan.py b/tools/checks/check_dispenser_color_scan.py index f82397e..35fd520 100644 --- a/tools/checks/check_dispenser_color_scan.py +++ b/tools/checks/check_dispenser_color_scan.py @@ -18,6 +18,8 @@ EXPECTED_IDS = {"1", "2", "3", "4"} # One synthetic color per dispenser slot for the offline test DISPENSER_COLORS = {"1": "red", "2": "green", "3": "yellow", "4": "blue"} +EXPECTED_FALSE_POSITIVE_MAP = {"1": "red", "2": "yellow", "3": "blue", "4": "green"} +EXPECTED_ARBITRARY_ORDER_MAP = {"1": "red", "2": "yellow", "3": "green", "4": "blue"} def fail(msg: str) -> int: @@ -40,6 +42,50 @@ def create_synthetic_images(image_dir: Path) -> None: cv2.imwrite(str(out_path), patch) +def check_visible_handle_false_positive_filter() -> tuple[bool, str]: + sys.path.insert(0, str(ROOT)) + try: + import cv2 # type: ignore + import numpy as np # type: ignore + except ImportError as exc: + return False, f"opencv/numpy required for visible-handle test: {exc}" + + from tools.perception.color_discrimination import bgr_patch_for_color # noqa: E402 + from tools.perception.dispenser_color_scan import detect_visible_handle_color_map # noqa: E402 + + def fill_box(frame: "np.ndarray", x: int, y: int, w: int, h: int, color: str) -> None: + patch = bgr_patch_for_color(color, size=max(w, h)) + frame[y : y + h, x : x + w] = cv2.resize(patch, (w, h)) + + frame = np.zeros((480, 640, 3), dtype=np.uint8) + frame[:, :] = (45, 45, 45) + # This upper, horizontal yellow blob reproduces the 2026-06-12 false + # positive shape. It must not become dispenser 1. + fill_box(frame, 214, 38, 86, 31, "yellow") + fill_box(frame, 254, 87, 36, 62, "red") + fill_box(frame, 320, 89, 28, 63, "yellow") + fill_box(frame, 380, 91, 32, 66, "blue") + fill_box(frame, 445, 83, 33, 68, "green") + + color_map = detect_visible_handle_color_map(frame, ["1", "2", "3", "4"]) + if color_map != EXPECTED_FALSE_POSITIVE_MAP: + return False, f"visible-handle map mismatch: got {color_map}, expected {EXPECTED_FALSE_POSITIVE_MAP}" + + frame_swapped = np.zeros((480, 640, 3), dtype=np.uint8) + frame_swapped[:, :] = (45, 45, 45) + fill_box(frame_swapped, 254, 87, 36, 62, "red") + fill_box(frame_swapped, 320, 89, 28, 63, "yellow") + fill_box(frame_swapped, 380, 91, 32, 66, "green") + fill_box(frame_swapped, 445, 83, 33, 68, "blue") + color_map = detect_visible_handle_color_map(frame_swapped, ["1", "2", "3", "4"]) + if color_map != EXPECTED_ARBITRARY_ORDER_MAP: + return False, ( + "visible-handle arbitrary order mismatch: " + f"got {color_map}, expected {EXPECTED_ARBITRARY_ORDER_MAP}" + ) + return True, f"visible-handle false-positive filter map: {color_map}" + + def main() -> int: with tempfile.TemporaryDirectory() as tmp_dir: image_dir = Path(tmp_dir) / "images" @@ -85,6 +131,10 @@ def main() -> int: if invalid_colors: return fail(f"invalid color values in output: {invalid_colors}") + ok, detail = check_visible_handle_false_positive_filter() + if not ok: + return fail(detail) + print(f"[PASS] {detail}") print(f"[PASS] dispenser_color_scan produced valid map: {color_map}") return 0 diff --git a/tools/perception/dispenser_color_scan.py b/tools/perception/dispenser_color_scan.py index 155f1d1..4c1c0df 100644 --- a/tools/perception/dispenser_color_scan.py +++ b/tools/perception/dispenser_color_scan.py @@ -61,6 +61,10 @@ "green": ((40, 60, 50, 85, 255, 255),), "blue": ((85, 80, 60, 130, 255, 255),), } +HANDLE_CENTER_Y_MIN_FRACTION = 0.16 +HANDLE_CENTER_Y_MAX_FRACTION = 0.38 +MIN_HANDLE_HEIGHT_OVER_WIDTH = 0.45 +MAX_HANDLE_ROW_STD_FRACTION = 0.045 def write_json_immediately(path: Path, payload: dict[str, str]) -> None: @@ -206,17 +210,23 @@ def detect_visible_handle_color_map( for contour in contours: area = float(cv2.contourArea(contour)) x, y, w, h = cv2.boundingRect(contour) + center_x = float(x) + float(w) * 0.5 + center_y = float(y) + float(h) * 0.5 # Booth-specific visual gate: handles are vertical colored blobs in # the upper/middle image, not the operator clothes, chairs, or cup. if area < min_area or w < min_w or h < min_h or w > max_w or h > max_h: continue - if not (0.06 * img_h <= y <= 0.42 * img_h): + if float(h) / float(max(w, 1)) < MIN_HANDLE_HEIGHT_OVER_WIDTH: + continue + if not ( + HANDLE_CENTER_Y_MIN_FRACTION * img_h + <= center_y + <= HANDLE_CENTER_Y_MAX_FRACTION * img_h + ): continue - if not (0.25 * img_w <= x <= 0.90 * img_w): + if not (0.25 * img_w <= center_x <= 0.90 * img_w): continue score = area + float(h) * 10.0 - center_x = float(x) + float(w) * 0.5 - center_y = float(y) + float(h) * 0.5 color_candidates.append((score, color, x, y, w, h, area, center_x, center_y)) color_candidates.sort(key=lambda item: item[0], reverse=True) if color_candidates: @@ -242,6 +252,8 @@ def detect_visible_handle_color_map( centers_y = [item[8] for item in combo] mean_y = sum(centers_y) / float(len(centers_y)) row_std = math.sqrt(sum((y - mean_y) ** 2 for y in centers_y) / float(len(centers_y))) + if row_std > max(12.0, MAX_HANDLE_ROW_STD_FRACTION * img_h): + continue area_score = sum(item[6] for item in combo) score = area_score - 200.0 * row_std if best_combo is None or score > best_combo[0]: diff --git a/tools/run/place_side_grip_cup_in_holder.py b/tools/run/place_side_grip_cup_in_holder.py index 75a6aa2..4f72330 100755 --- a/tools/run/place_side_grip_cup_in_holder.py +++ b/tools/run/place_side_grip_cup_in_holder.py @@ -290,7 +290,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--place-final-z-offset-m", type=float, - default=-0.020, + default=-0.030, help=( "Measured adjustment added only to place_final Z. Use a negative value " "to lower the cup into the holder without rewriting calibration.yaml." diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 060ef4d..b3fc4a1 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -288,6 +288,7 @@ def main() -> int: parser.add_argument("--move-release-offset-z-m", default="0.010") parser.add_argument("--cup-pre-from-place-x-offset-m", default="-0.090") parser.add_argument("--cup-pre-from-place-z-offset-m", default="0.030") + parser.add_argument("--dispenser-3-cup-pre-extra-x-offset-m", default="-0.010") parser.add_argument("--generated-cup-pre-max-joint-delta-deg", default="190.0") parser.add_argument( "--press-contact-use-joint-move", @@ -418,6 +419,7 @@ def main() -> int: "--move-release-offset-z-m", str(args.move_release_offset_z_m), "--cup-pre-from-place-x-offset-m", str(args.cup_pre_from_place_x_offset_m), "--cup-pre-from-place-z-offset-m", str(args.cup_pre_from_place_z_offset_m), + "--dispenser-3-cup-pre-extra-x-offset-m", str(args.dispenser_3_cup_pre_extra_x_offset_m), "--generated-cup-pre-max-joint-delta-deg", str(args.generated_cup_pre_max_joint_delta_deg), "--regrasp-retreat-x-m", str(args.regrasp_retreat_x_m), "--regrasp-retreat-y-m", str(args.regrasp_retreat_y_m), diff --git a/tools/run/run_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh index c79c55e..1db5430 100755 --- a/tools/run/run_kang_lid_grip_close_direct.sh +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -105,7 +105,7 @@ launch_args=( lid_twist_force_settle_seconds:=0.2 lid_twist_force_release_time:=0.2 \ lid_twist_preseat_periodic_before_turn:=true lid_twist_preseat_mode:=j6_step_wiggle \ lid_twist_preseat_periodic_descend_m:=0.02 lid_twist_preseat_step_m:=0.005 \ - lid_twist_preseat_wiggle_deg:=10.0 lid_twist_preseat_wiggle_velocity:=50.0 lid_twist_preseat_down_velocity:=8.0 \ + lid_twist_preseat_wiggle_deg:=60.0 lid_twist_preseat_wiggle_velocity:=50.0 lid_twist_preseat_down_velocity:=8.0 \ lid_twist_rz_delta_deg:=360.0 lid_twist_turn_step_deg:=60.0 \ lid_twist_release_lift_m:=0.03 lid_twist_min_z_m:=0.140 lid_twist_max_z_m:=0.260 \ lid_twist_transfer_velocity:=25.0 lid_twist_press_velocity:=10.0 lid_twist_turn_velocity:=40.0 lid_twist_acceleration:=15.0 \ diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index 94d7f73..c4ad2d9 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -488,9 +488,11 @@ def print_dry_run_group_detail(args: argparse.Namespace, dispenser_id: str, pres if cup_common_pre is not None: print(f"[PLAN] dispenser {dispenser_id}: cup CUP_COMMON_PRE -> DISP_PLACE -> RELEASE") else: + extra_x_m = args.dispenser_3_cup_pre_extra_x_offset_m if str(dispenser_id) == "3" else 0.0 + total_x_offset_m = args.cup_pre_from_place_x_offset_m + extra_x_m print( f"[PLAN] dispenser {dispenser_id}: cup generated DISP_PRE " - f"(DISP_PLACE X{args.cup_pre_from_place_x_offset_m * 1000.0:+.0f}mm " + f"(DISP_PLACE X{total_x_offset_m * 1000.0:+.0f}mm " f"Z{args.cup_pre_from_place_z_offset_m * 1000.0:+.0f}mm) " "-> DISP_PLACE -> RELEASE" ) @@ -1788,13 +1790,21 @@ def move_and_release(self, dispenser_id: str) -> None: f"[Azas] cup placement: dispenser={dispenser_id} using measured " "DISP_PLACE with generated DISP_PRE from X offset; saved cup_pre_place_joints_deg ignored" ) + extra_x_m = ( + self.args.dispenser_3_cup_pre_extra_x_offset_m + if str(dispenser_id) == "3" + else 0.0 + ) + total_x_offset_m = self.args.cup_pre_from_place_x_offset_m + extra_x_m pre_target = list(final_target) - pre_target[0] += self.args.cup_pre_from_place_x_offset_m * 1000.0 + pre_target[0] += total_x_offset_m * 1000.0 pre_target[2] += self.args.cup_pre_from_place_z_offset_m * 1000.0 print( "[Azas] generated cup pre: " f"dispenser={dispenser_id} " - f"pre_x_offset={self.args.cup_pre_from_place_x_offset_m * 1000.0:.1f}mm " + f"pre_x_offset={total_x_offset_m * 1000.0:.1f}mm " + f"base_pre_x_offset={self.args.cup_pre_from_place_x_offset_m * 1000.0:.1f}mm " + f"dispenser_3_extra_x_offset={extra_x_m * 1000.0:.1f}mm " f"pre_z_offset={self.args.cup_pre_from_place_z_offset_m * 1000.0:.1f}mm " f"target_posx=[{pre_target[0]:.1f}, {pre_target[1]:.1f}, {pre_target[2]:.1f}, " f"{pre_target[3]:.1f}, {pre_target[4]:.1f}, {pre_target[5]:.1f}]" @@ -2898,6 +2908,15 @@ def parse_args() -> argparse.Namespace: "Default +0.030m." ), ) + parser.add_argument( + "--dispenser-3-cup-pre-extra-x-offset-m", + type=float, + default=-0.010, + help=( + "Extra X offset applied only to generated DISP3_PRE. Default -0.010m " + "adds 10mm robot-side clearance without changing measured DISP3_PLACE." + ), + ) parser.add_argument( "--use-cup-common-pre", action=argparse.BooleanOptionalAction, diff --git a/tools/run/run_stt_order_then_router.sh b/tools/run/run_stt_order_then_router.sh new file mode 100755 index 0000000..def6687 --- /dev/null +++ b/tools/run/run_stt_order_then_router.sh @@ -0,0 +1,70 @@ +#!/usr/bin/env bash +set -euo pipefail + +# STT/키오스크 주문 -> 자동 칵테일 파이프라인. +# /azas/voice/confirmed_recipe_decision 주문을 기다렸다가(listen_stt_recipe.py가 +# outputs/latest_recipe.json 저장) auto_cup_flow_router를 실행한다. +# recipe_colors를 일부러 비워 두므로 레시피 단계는 방금 저장된 주문 내용을 사용한다. +# +# Usage: +# bash tools/run/run_stt_order_then_router.sh # 주문 1건 처리 후 종료 +# LOOP=true bash tools/run/run_stt_order_then_router.sh # 주문 올 때마다 반복 처리 +# ORDER_TIMEOUT_SEC=3600 ... # 주문 대기 한도(기본 86400초) +# +# 사전 조건(이 스크립트가 띄우지 않음): +# - tmux 로봇 스택: bash tools/run/start_azas_tmux_stack.sh +# - kiosk/voice 데모: bash tools/run/run_kiosk_voice_demo.sh + +ROOT="${ROOT:-/home/ssu/Azas}" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +ORDER_TIMEOUT_SEC="${ORDER_TIMEOUT_SEC:-86400}" +LOOP="${LOOP:-false}" +CLASSIFIER_PATH="${CLASSIFIER_PATH:-${ROOT}/cup_classifier_best.pth}" + +cd "${ROOT}" + +set +u +source /opt/ros/humble/setup.bash +[[ -f /home/ssu/ws_moveit/install/setup.bash ]] && source /home/ssu/ws_moveit/install/setup.bash +[[ -f /home/ssu/ros2_ws/install/setup.bash ]] && source /home/ssu/ros2_ws/install/setup.bash +source "${ROOT}/install/setup.bash" +set -u + +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" +export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +mkdir -p "${ROS_LOG_DIR}" + +run_one_order() { + echo "[Azas] STT 주문 대기 중... (/azas/voice/confirmed_recipe_decision, timeout=${ORDER_TIMEOUT_SEC}s)" + if ! python3 "${ROOT}/tools/run/listen_stt_recipe.py" --timeout "${ORDER_TIMEOUT_SEC}"; then + echo "[Azas] 주문 수신 실패/타임아웃" >&2 + return 1 + fi + echo "[Azas] 주문 수신 -> auto_cup_flow_router 시작 (recipe=outputs/latest_recipe.json)" + # recipe_colors는 비워 둔다: 채우면 latest_recipe.json(방금 받은 주문)이 무시된다. + ros2 launch azas_bringup auto_cup_flow_router.launch.py \ + enable_real_motion:=true \ + router_confirm:=ENABLE_AUTO_CUP_ROUTER \ + service_prefix:="${SERVICE_PREFIX}" \ + moveit_controller_name:="/${SERVICE_PREFIX}/dsr_moveit_controller" \ + controller_action_name:="/${SERVICE_PREFIX}/dsr_moveit_controller/follow_joint_trajectory" \ + classifier_path:="${CLASSIFIER_PATH}" \ + classifier_arch:=resnet18 \ + route_hold_sec:=2.0 \ + route_stable_required_samples:=5 \ + route_stable_min_sec:=0.8 +} + +if [[ "${LOOP}" == "true" ]]; then + echo "[Azas] LOOP 모드: 주문이 올 때마다 파이프라인을 반복 실행합니다. 중지: Ctrl-C" + while true; do + if ! run_one_order; then + echo "[Azas] 이번 주문 처리 실패; 5초 후 다음 주문 대기" >&2 + sleep 5 + fi + done +else + run_one_order +fi From 854b442b8d908c0812940bab6e59596175001081 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Fri, 12 Jun 2026 19:35:27 +0900 Subject: [PATCH 76/88] =?UTF-8?q?5=EC=B0=A8=ED=85=8C=EC=8A=A4=ED=8A=B8=20s?= =?UTF-8?q?tt,=20mediapipe=EC=A0=9C=EC=99=B8=20=EC=84=B1=EA=B3=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- tools/run/robot_pipeline_control_server.py | 1 + tools/run/run_lid_close_then_shake_chain.sh | 2 +- tools/run/run_rule_based_shake_real.sh | 10 +++++++++- 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 81c3239..093e9a9 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -3761,6 +3761,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "VERIFY_JOINT_TARGETS=true JOINT_TARGET_TOLERANCE_DEG=8.0 " "JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 " "REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true " + "REAL_ROBOT_MOTION_CONFIRM=ENABLE_REAL_ROBOT_MOTION " "tools/run/run_rule_based_shake_real.sh" " && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && " "python3 tools/run/direct_movej_joints.py " diff --git a/tools/run/run_lid_close_then_shake_chain.sh b/tools/run/run_lid_close_then_shake_chain.sh index e87027e..cffcb9d 100755 --- a/tools/run/run_lid_close_then_shake_chain.sh +++ b/tools/run/run_lid_close_then_shake_chain.sh @@ -3,4 +3,4 @@ # robot_pipeline_control_server.py chain_shake_after_lid_command()가 생성하는 패널 체인과 동일한 명령을 # auto_cup_flow_router가 직접 실행할 수 있도록 스크립트로 고정한 것이다. -( cd /home/ssu/Azas && SERVICE_PREFIX=dsr01 DISPLAY=${DISPLAY:-:0} XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} MOVE_TO_LID_VIEW_POSE=true bash /home/ssu/Azas/tools/run/run_kang_lid_grip_close_direct.sh ) & lid_pid=$!; ( cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 /home/ssu/Azas/tools/run/wait_for_lid_grip_status.py --timeout-sec 900 --success-status motion_sequence_requested ) & wait_pid=$!; while true; do if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; if ! kill -0 ${lid_pid} 2>/dev/null; then wait ${lid_pid}; lid_rc=$?; sleep 1; if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; echo '[Azas] lid_grip_close launch exited before ArUco success status; shake chain blocked.'; kill -TERM ${wait_pid} 2>/dev/null || true; wait ${wait_pid} 2>/dev/null || true; if [ ${lid_rc} -eq 0 ]; then exit 1; else exit ${lid_rc}; fi; fi; sleep 1; done; kill -TERM ${lid_pid} 2>/dev/null || true; wait ${lid_pid} 2>/dev/null || true; if [ ${wait_rc} -eq 0 ]; then echo '[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹으로 바로 넘어갑니다.'; echo '[Azas] auto_holder_pick_then_shake=true'; cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && echo '[Azas] SHAKE START: 컵홀더에 놓인 닫힌 컵을 측정 pose로 다시 side-grip 픽업한 뒤 흔듭니다.' && echo '[Azas] 순서: RG2 open -> 컵홀더 retreat 접근 -> holder final pose에서 soft grasp -> holder lift -> 관절 쉐이킹.' && echo '[Azas] 주의: 컵 좌표를 새로 만들지 않고 calibration.yaml cup_holder.side_grip_place 측정값만 사용합니다.' && python3 tools/run/pick_from_cup_holder_side_grip.py --service-prefix dsr01 --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml --approach-velocity 12.0 --approach-acceleration 16.0 --descend-velocity 6.0 --descend-acceleration 10.0 --lift-velocity 12.0 --lift-acceleration 16.0 --place-final-z-offset-m -0.020 --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 --ikin-timeout-sec 20.0 --ikin-retries 2 --gripper-grasp-width-m 0.068 --gripper-force-n 35.0 --post-grasp-settle-sec 0.8 --z-max 0.28 --execute --confirm ENABLE_CUP_HOLDER_PICK && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=remove_world -p object_id:=tumbler_in_holder -p dispenser_id:=1 -p publish_once:=true && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=attach -p object_id:=carried_tumbler -p dispenser_id:=1 -p publish_once:=true && SERVICE_PREFIX=dsr01 GRASPED_CUP_TEST_MODE=true SKIP_CUP_HOLDER_PICK=true REQUIRE_ROBOT_STANDBY=true SHAKE_CONTROL_MODE=joint SHAKE_CYCLES=3 JOINT_SHAKE_BASE_J1_DEG=0.0 JOINT_SHAKE_BASE_J2_DEG=-35.0 JOINT_SHAKE_BASE_J3_DEG=50.0 JOINT_SHAKE_BASE_J4_DEG=0.0 JOINT_SHAKE_BASE_J5_DEG=70.0 JOINT_SHAKE_BASE_J6_DEG=0.0 JOINT_SHAKE_J3_AMPLITUDE_DEG=0.0 JOINT_SHAKE_J4_AMPLITUDE_DEG=18.0 JOINT_SHAKE_J5_AMPLITUDE_DEG=20.0 JOINT_SHAKE_J6_AMPLITUDE_DEG=24.0 JOINT_SHAKE_J1_MIN_DEG=-20.0 JOINT_SHAKE_J1_MAX_DEG=5.0 JOINT_SHAKE_J2_MIN_DEG=-80.0 JOINT_SHAKE_J2_MAX_DEG=5.0 JOINT_SHAKE_J3_MIN_DEG=0.0 JOINT_SHAKE_J3_MAX_DEG=135.0 JOINT_SHAKE_MAX_SINGLE_DELTA_DEG=75.0 ENFORCE_WRIST_JOINT_LIMITS=false WRIST_MIN_DEG=-135.0 WRIST_MAX_DEG=135.0 JOINT5_MIN_DEG=40.0 JOINT5_MAX_DEG=100.0 APPROACH_JOINT_VELOCITY=18.0 APPROACH_JOINT_ACCELERATION=22.0 APPROACH_JOINT_TIME=2.6 SHAKE_JOINT_VELOCITY=90.0 SHAKE_JOINT_ACCELERATION=120.0 SHAKE_JOINT_TIME=0.0 JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=130.0 VERIFY_JOINT_TARGETS=true JOINT_TARGET_TOLERANCE_DEG=8.0 JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true tools/run/run_rule_based_shake_real.sh && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && python3 tools/run/direct_movej_joints.py --service-prefix dsr01 --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 --velocity 15 --acceleration 15 --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ; else echo '[Azas] ArUco lid_grip_close 실패/타임아웃 -> 컵홀더 재픽업/쉐이킹을 건너뜁니다.'; exit ${wait_rc}; fi +( cd /home/ssu/Azas && SERVICE_PREFIX=dsr01 DISPLAY=${DISPLAY:-:0} XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} MOVE_TO_LID_VIEW_POSE=true bash /home/ssu/Azas/tools/run/run_kang_lid_grip_close_direct.sh ) & lid_pid=$!; ( cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 /home/ssu/Azas/tools/run/wait_for_lid_grip_status.py --timeout-sec 900 --success-status motion_sequence_requested ) & wait_pid=$!; while true; do if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; if ! kill -0 ${lid_pid} 2>/dev/null; then wait ${lid_pid}; lid_rc=$?; sleep 1; if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; echo '[Azas] lid_grip_close launch exited before ArUco success status; shake chain blocked.'; kill -TERM ${wait_pid} 2>/dev/null || true; wait ${wait_pid} 2>/dev/null || true; if [ ${lid_rc} -eq 0 ]; then exit 1; else exit ${lid_rc}; fi; fi; sleep 1; done; kill -TERM ${lid_pid} 2>/dev/null || true; wait ${lid_pid} 2>/dev/null || true; if [ ${wait_rc} -eq 0 ]; then echo '[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹으로 바로 넘어갑니다.'; echo '[Azas] auto_holder_pick_then_shake=true'; cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && echo '[Azas] SHAKE START: 컵홀더에 놓인 닫힌 컵을 측정 pose로 다시 side-grip 픽업한 뒤 흔듭니다.' && echo '[Azas] 순서: RG2 open -> 컵홀더 retreat 접근 -> holder final pose에서 soft grasp -> holder lift -> 관절 쉐이킹.' && echo '[Azas] 주의: 컵 좌표를 새로 만들지 않고 calibration.yaml cup_holder.side_grip_place 측정값만 사용합니다.' && python3 tools/run/pick_from_cup_holder_side_grip.py --service-prefix dsr01 --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml --approach-velocity 12.0 --approach-acceleration 16.0 --descend-velocity 6.0 --descend-acceleration 10.0 --lift-velocity 12.0 --lift-acceleration 16.0 --place-final-z-offset-m -0.020 --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 --ikin-timeout-sec 20.0 --ikin-retries 2 --gripper-grasp-width-m 0.068 --gripper-force-n 35.0 --post-grasp-settle-sec 0.8 --z-max 0.28 --execute --confirm ENABLE_CUP_HOLDER_PICK && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=remove_world -p object_id:=tumbler_in_holder -p dispenser_id:=1 -p publish_once:=true && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=attach -p object_id:=carried_tumbler -p dispenser_id:=1 -p publish_once:=true && SERVICE_PREFIX=dsr01 GRASPED_CUP_TEST_MODE=true SKIP_CUP_HOLDER_PICK=true REQUIRE_ROBOT_STANDBY=true SHAKE_CONTROL_MODE=joint SHAKE_CYCLES=3 JOINT_SHAKE_BASE_J1_DEG=0.0 JOINT_SHAKE_BASE_J2_DEG=-35.0 JOINT_SHAKE_BASE_J3_DEG=50.0 JOINT_SHAKE_BASE_J4_DEG=0.0 JOINT_SHAKE_BASE_J5_DEG=70.0 JOINT_SHAKE_BASE_J6_DEG=0.0 JOINT_SHAKE_J3_AMPLITUDE_DEG=0.0 JOINT_SHAKE_J4_AMPLITUDE_DEG=18.0 JOINT_SHAKE_J5_AMPLITUDE_DEG=20.0 JOINT_SHAKE_J6_AMPLITUDE_DEG=24.0 JOINT_SHAKE_J1_MIN_DEG=-20.0 JOINT_SHAKE_J1_MAX_DEG=5.0 JOINT_SHAKE_J2_MIN_DEG=-80.0 JOINT_SHAKE_J2_MAX_DEG=5.0 JOINT_SHAKE_J3_MIN_DEG=0.0 JOINT_SHAKE_J3_MAX_DEG=135.0 JOINT_SHAKE_MAX_SINGLE_DELTA_DEG=75.0 ENFORCE_WRIST_JOINT_LIMITS=false WRIST_MIN_DEG=-135.0 WRIST_MAX_DEG=135.0 JOINT5_MIN_DEG=40.0 JOINT5_MAX_DEG=100.0 APPROACH_JOINT_VELOCITY=18.0 APPROACH_JOINT_ACCELERATION=22.0 APPROACH_JOINT_TIME=2.6 SHAKE_JOINT_VELOCITY=90.0 SHAKE_JOINT_ACCELERATION=120.0 SHAKE_JOINT_TIME=0.0 JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=130.0 VERIFY_JOINT_TARGETS=true JOINT_TARGET_TOLERANCE_DEG=8.0 JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true REAL_ROBOT_MOTION_CONFIRM=ENABLE_REAL_ROBOT_MOTION tools/run/run_rule_based_shake_real.sh && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && python3 tools/run/direct_movej_joints.py --service-prefix dsr01 --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 --velocity 15 --acceleration 15 --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ; else echo '[Azas] ArUco lid_grip_close 실패/타임아웃 -> 컵홀더 재픽업/쉐이킹을 건너뜁니다.'; exit ${wait_rc}; fi diff --git a/tools/run/run_rule_based_shake_real.sh b/tools/run/run_rule_based_shake_real.sh index b47781d..b540c09 100755 --- a/tools/run/run_rule_based_shake_real.sh +++ b/tools/run/run_rule_based_shake_real.sh @@ -82,6 +82,7 @@ JOINT_TARGET_POLL_SEC="${JOINT_TARGET_POLL_SEC:-0.05}" REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE="${REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE:-true}" STATE_VALIDITY_SERVICE="${STATE_VALIDITY_SERVICE:-/check_state_validity}" PLANNING_GROUP="${PLANNING_GROUP:-manipulator}" +REAL_ROBOT_MOTION_CONFIRM="${REAL_ROBOT_MOTION_CONFIRM:-${SHAKE_REAL_MOTION_CONFIRM:-}}" echo "[Azas] SHAKE START 설명: 컵홀더에 놓인 닫힌 컵을 side grip으로 다시 잡은 뒤 흔드는 단계입니다." echo "[Azas] 순서: 컵홀더 place 완료 확인 -> 컵홀더 측정 pose로 RG2 side-grip 픽업 -> 들어 올림 -> 관절 쉐이킹 실행." @@ -259,7 +260,14 @@ else echo " - lifted shake volume is clear around x=${SHAKE_CENTER_X}, y=${SHAKE_CENTER_Y}, z=${SHAKE_CENTER_Z}" fi echo -read -r -p "Type ENABLE_REAL_ROBOT_MOTION to continue: " CONFIRM +CONFIRM="${REAL_ROBOT_MOTION_CONFIRM}" +if [[ -n "${CONFIRM}" ]]; then + echo "[Azas] Using non-interactive real-motion confirmation from environment." +else + if ! read -r -p "Type ENABLE_REAL_ROBOT_MOTION to continue: " CONFIRM; then + CONFIRM="" + fi +fi if [[ "${CONFIRM}" != "ENABLE_REAL_ROBOT_MOTION" ]]; then echo "[Azas] Confirmation did not match. Refusing real robot shake." exit 1 From 31d2ccd27da7bd5855bb8e18cd9d26f991bb57eb Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sat, 13 Jun 2026 04:00:32 +0900 Subject: [PATCH 77/88] feat: Implement cocktail recipe catalog and voice command automation - Added new UI elements and logic for displaying cocktail recipes in voice.js. - Introduced a new SVG icon for the cocktail representation. - Enhanced the Python scripts for managing the cocktail preparation sequence with resume capabilities. - Created shell scripts to automate the voice command flow and manage the Azas voice stack in a tmux session. - Updated the run scripts to support new features and ensure proper execution of the cocktail preparation pipeline. --- DESIGN.md | 82 +++ .../azas_cup_uprighting/_base_node.py | 15 +- src/azas_cup_uprighting/config/moveit_py.yaml | 24 +- src/azas_voice/azas_voice/command_parser.py | 39 +- .../azas_voice/llm_recipe_mapper_node.py | 13 +- src/azas_voice/azas_voice/recipe_catalog.py | 224 +++++++ .../voice_pipeline_executor_node.py | 245 ++++++++ .../azas_voice/voice_screen_node.py | 16 + src/azas_voice/config/recipes.yaml | 158 ++++- src/azas_voice/launch/azas_voice.launch.py | 19 + src/azas_voice/package.xml | 2 + src/azas_voice/setup.py | 1 + src/azas_voice/test/test_command_parser.py | 20 + src/azas_voice/test/test_llm_recipe_mapper.py | 25 +- src/azas_voice/web/voice.css | 565 +++++++++++++++++- src/azas_voice/web/voice.html | 201 +++++-- src/azas_voice/web/voice.js | 327 ++++++++++ tools/run/azas_cocktail_icon.svg | 16 + tools/run/run_color_recipe_sequence.py | 6 +- .../run_measured_dispenser_recipe_sequence.py | 314 +++++++++- tools/run/run_voice_auto_cup_flow.sh | 51 ++ tools/run/start_azas_voice_stack.sh | 71 +++ tools/run/stop_azas_voice_stack.sh | 22 + 23 files changed, 2361 insertions(+), 95 deletions(-) create mode 100644 DESIGN.md create mode 100644 src/azas_voice/azas_voice/voice_pipeline_executor_node.py create mode 100644 tools/run/azas_cocktail_icon.svg create mode 100755 tools/run/run_voice_auto_cup_flow.sh create mode 100755 tools/run/start_azas_voice_stack.sh create mode 100755 tools/run/stop_azas_voice_stack.sh diff --git a/DESIGN.md b/DESIGN.md new file mode 100644 index 0000000..1639901 --- /dev/null +++ b/DESIGN.md @@ -0,0 +1,82 @@ +# Design + +## Source of truth +- Status: Draft +- Last refreshed: 2026-06-13 +- Primary product surfaces: Azas voice order screen, menu preview panel, robot pipeline status UI, kiosk/menu surfaces. +- Evidence reviewed: `src/azas_voice/web/voice.html`, `src/azas_voice/web/voice.css`, `src/azas_voice/web/voice.js`, `src/azas_voice/azas_voice/voice_screen_node.py`, `src/azas_voice/azas_voice/voice_pipeline_executor_node.py`, `src/azas_voice/config/recipes.yaml`, `src/azas_kiosk/`. + +## Brand +- Personality: calm, precise, service-oriented cocktail robot. +- Trust signals: visible order confirmation, clear recipe ingredients, robot process status, failure/resume state visibility. +- Avoid: marketing hero pages, decorative-only UI, hidden robot motion state, fake coordinates or unsupported safety claims. + +## Product goals +- Goals: let users order many named drinks by voice or touch, preview the finished drink, and understand the robot's current manufacturing stage. +- Non-goals: manual robot coordinate entry, free-form motion generation, unsupported recipe execution outside measured dispenser/color mappings. +- Success signals: users can pick from a larger menu, see ingredient amounts, see the current robot step, and recover from interrupted dispenser sequences. + +## Personas and jobs +- Primary personas: demo operator, guest ordering a drink, developer validating the robot flow. +- User jobs: choose or request a drink, confirm execution, monitor robot progress, understand a stopped/resumed run. +- Key contexts of use: local robot station, ROS launch-driven demo, touchscreen or browser view near the robot. + +## Information architecture +- Primary navigation: single voice order screen with adjacent menu/status panel. +- Core routes/screens: voice conversation, selected drink preview, robot process stage, catalog list. +- Content hierarchy: current order and confirmation first, finished drink preview second, recipe catalog and process detail nearby. + +## Design principles +- Principle 1: show operational state directly instead of explaining the system. +- Principle 2: make the drink choice visual and scannable without hiding execution readiness. +- Tradeoffs: favor compact, reliable status over decorative immersion; prefer symbolic recipe/color data over robot-coordinate exposure. + +## Visual language +- Color: ingredient colors map consistently to red/juice, yellow/syrup, green/liqueur, blue/rum. +- Typography: readable dashboard sizing; compact headings inside panels. +- Spacing/layout rhythm: two-column desktop layout with stacked mobile flow. +- Shape/radius/elevation: restrained panels and item cards; avoid nested card-on-card layouts. +- Motion: small process animations for robot stage changes; keep motion nonessential and readable. +- Imagery/iconography: HTML/SVG drink preview and CSS robot scene are acceptable when real finished-drink photos are unavailable. + +## Components +- Existing components to reuse: voice orb, dialogue bubbles, status grid, recipe glass SVG, ingredient chips, pipeline step list. +- New/changed components: catalog item buttons, drink stat block, robot process scene, resume-aware pipeline stage. +- Variants and states: idle, recommended, confirmed, making, completed, failed, dry-run, resume recovery. +- Token/component ownership: `src/azas_voice/web/voice.css` owns current web styling; recipe data comes from `src/azas_voice/config/recipes.yaml`. + +## Accessibility +- Target standard: practical WCAG AA for text contrast and keyboard/touch operation where possible. +- Keyboard/focus behavior: catalog entries and test form controls must remain button/input elements with visible focus. +- Contrast/readability: status text and badges must remain readable over panel backgrounds. +- Screen-reader semantics: use section labels and meaningful button labels for menu order actions. +- Reduced motion and sensory considerations: animations should be decorative and not required for understanding status. + +## Responsive behavior +- Supported breakpoints/devices: desktop browser near robot, tablet/touch display, narrow mobile fallback. +- Layout adaptations: voice and menu panels stack on narrow screens; catalog remains scrollable. +- Touch/hover differences: catalog buttons must be usable without hover-only affordances. + +## Interaction states +- Loading: retain previous state until fresh `/api/state` arrives. +- Empty: show no selected recipe and invite a voice/test utterance. +- Error: show pipeline/status failure and last known stage when available. +- Success: show completed badge and final drink preview. +- Disabled: hardware execution may remain dry-run from launch parameters. +- Offline/slow network, if applicable: periodic refresh should keep the last known UI state visible. + +## Content voice +- Tone: concise Korean service copy. +- Terminology: use menu, 레시피, 제조, 디스펜서, 컵 픽업, 재개 consistently. +- Microcopy rules: do not expose internal implementation detail unless it helps operator recovery. + +## Implementation constraints +- Framework/styling system: static HTML/CSS/JavaScript served by `voice_screen_node.py`. +- Design-token constraints: no central token system yet; keep colors local and ingredient-specific. +- Performance constraints: catalog rendering should avoid repeated full DOM rebuilds unless catalog data changes. +- Compatibility constraints: ROS nodes publish JSON status; browser UI polls `/api/state`. +- Test/screenshot expectations: run parser/mapper tests for recipe changes and smoke browser/server behavior when launch environment is available. + +## Open questions +- [ ] Whether production demos should include real generated drink images per recipe or keep the current deterministic SVG/HTML preview. +- [ ] Whether interrupted pipeline recovery should also surface the checkpoint JSON contents in the operator panel. diff --git a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py index 44a8cd9..4e93d86 100644 --- a/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py +++ b/src/azas_cup_uprighting/azas_cup_uprighting/_base_node.py @@ -145,13 +145,22 @@ def _ensure_moveit(self) -> bool: log = self.get_logger() log.info("MoveItPy 지연 초기화 중...") - self.robot = MoveItPy(node_name=self.MOVEIT_NODE_NAME) + try: + self.robot = MoveItPy(node_name=self.MOVEIT_NODE_NAME) + except RuntimeError as exc: + self.robot = None + self.arm = None + self.robot_model = None + self.ompl_params = None + self.pilz_params = None + log.error(f"MoveItPy 초기화 실패: {exc}") + return False self.arm = self.robot.get_planning_component(cfg.GROUP_NAME) self.robot_model = self.robot.get_robot_model() self.ompl_params = self._make_plan_params( - "ompl", "RRTConnect", vel=0.2, acc=0.1, time=2.0) + "ompl", "RRTConnect", vel=0.08, acc=0.05, time=3.0) self.pilz_params = self._make_plan_params( - "pilz_industrial_motion_planner", "PTP", vel=0.15, acc=0.1, time=2.0) + "pilz_industrial_motion_planner", "PTP", vel=0.06, acc=0.04, time=3.0) self.on_moveit_ready() log.info("MoveItPy 지연 초기화 완료") return True diff --git a/src/azas_cup_uprighting/config/moveit_py.yaml b/src/azas_cup_uprighting/config/moveit_py.yaml index 0e89b41..0b9f488 100644 --- a/src/azas_cup_uprighting/config/moveit_py.yaml +++ b/src/azas_cup_uprighting/config/moveit_py.yaml @@ -3,7 +3,7 @@ planning_scene_monitor_options: name: "planning_scene_monitor" robot_description: "robot_description" - joint_state_topic: "/joint_states" + joint_state_topic: "/dsr01/joint_states" attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" @@ -15,16 +15,16 @@ plan_request_params: planning_attempts: 1 planning_pipeline: ompl - max_velocity_scaling_factor: 0.1 - max_acceleration_scaling_factor: 0.1 + max_velocity_scaling_factor: 0.08 + max_acceleration_scaling_factor: 0.05 ompl_rrtc: plan_request_params: planning_attempts: 1 planning_pipeline: ompl planner_id: "RRTConnectkConfigDefault" - max_velocity_scaling_factor: 1.0 - max_acceleration_scaling_factor: 1.0 + max_velocity_scaling_factor: 0.08 + max_acceleration_scaling_factor: 0.05 planning_time: 1.0 ompl_rrt_star: @@ -32,8 +32,8 @@ planning_attempts: 1 planning_pipeline: ompl_rrt_star planner_id: "RRTstarkConfigDefault" - max_velocity_scaling_factor: 1.0 - max_acceleration_scaling_factor: 1.0 + max_velocity_scaling_factor: 0.08 + max_acceleration_scaling_factor: 0.05 planning_time: 1.5 pilz_lin: @@ -41,14 +41,14 @@ planning_attempts: 1 planning_pipeline: pilz_industrial_motion_planner planner_id: "PTP" - max_velocity_scaling_factor: 0.1 - max_acceleration_scaling_factor: 0.1 + max_velocity_scaling_factor: 0.06 + max_acceleration_scaling_factor: 0.04 planning_time: 0.8 chomp: plan_request_params: planning_attempts: 1 planning_pipeline: chomp - max_velocity_scaling_factor: 1.0 - max_acceleration_scaling_factor: 1.0 - planning_time: 1.5 \ No newline at end of file + max_velocity_scaling_factor: 0.08 + max_acceleration_scaling_factor: 0.05 + planning_time: 1.5 diff --git a/src/azas_voice/azas_voice/command_parser.py b/src/azas_voice/azas_voice/command_parser.py index 98fe270..c8a2340 100644 --- a/src/azas_voice/azas_voice/command_parser.py +++ b/src/azas_voice/azas_voice/command_parser.py @@ -18,6 +18,7 @@ RECIPE_DISPENSERS, RECIPE_DISPLAY_NAMES, TRAIT_KEYWORDS, + recipe_amounts, ) @@ -63,10 +64,15 @@ def _contains_any(normalized: str, words: tuple[str, ...]) -> bool: def _match_recipe(normalized: str) -> str | None: if "디스펜서" in normalized: return None + matches: list[tuple[int, str]] = [] for recipe_id, aliases in RECIPE_ALIASES.items(): - if _contains_any(normalized, aliases): - return recipe_id - return None + for alias in aliases: + normalized_alias = normalize_text(alias) + if normalized_alias and normalized_alias in normalized: + matches.append((len(normalized_alias), recipe_id)) + if not matches: + return None + return max(matches)[1] def _match_colors(normalized: str) -> tuple[str, ...]: @@ -94,12 +100,22 @@ def _recipe_description(recipe_id: str) -> str: def _random_recipe_decision(utterance: str, normalized: str) -> RecipeDecision: recipe_id = random.choice(tuple(RECIPE_DISPENSERS)) dispenser_ids = RECIPE_DISPENSERS[recipe_id] + amounts = recipe_amounts(recipe_id) description = _recipe_description(recipe_id) confirmation = ( f"{_recipe_name(recipe_id)}를 추천드릴게요. " f"{description} 진행할까요?" ) - return RecipeDecision(True, utterance, normalized, "make_cocktail", recipe_id, dispenser_ids, confirmation) + return RecipeDecision( + True, + utterance, + normalized, + "make_cocktail", + recipe_id, + dispenser_ids, + confirmation, + dispenser_amounts=amounts, + ) def _level_text(amount: int, zero: str, low: str, normal: str, high: str) -> str: @@ -236,11 +252,20 @@ def parse_recipe_command(text: str) -> RecipeDecision: "no recipe or dispenser color matched", ) + amounts = recipe_amounts(recipe_id) if recipe_id is None: recipe_id = "custom_color_selection" - elif not dispenser_ids: + else: dispenser_ids = RECIPE_DISPENSERS.get(recipe_id, ()) - dispenser_text = ", ".join(dispenser_ids) if dispenser_ids else "configured recipe dispensers" confirmation = f"{_recipe_name(recipe_id)} 요청을 인식했습니다. 진행할까요?" - return RecipeDecision(True, utterance, normalized, "make_cocktail", recipe_id, dispenser_ids, confirmation) + return RecipeDecision( + True, + utterance, + normalized, + "make_cocktail", + recipe_id, + dispenser_ids, + confirmation, + dispenser_amounts=amounts, + ) diff --git a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py index b4905ca..0cd437c 100644 --- a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py +++ b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py @@ -18,6 +18,7 @@ RECIPE_DESCRIPTIONS, RECIPE_DISPENSERS, RECIPE_DISPLAY_NAMES, + recipe_amounts, ) @@ -128,10 +129,13 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: recipe_id = str(recipe_id).strip() if recipe_id else None if intent == "make_cocktail" and (wanted_traits or avoided_traits): recipe_id = "custom_preference_mix" - if recipe_id and not recipe_id.startswith("recipe_") and recipe_id not in ALLOWED_CUSTOM_RECIPE_IDS: + if recipe_id and recipe_id not in RECIPE_DISPENSERS and recipe_id not in ALLOWED_CUSTOM_RECIPE_IDS: recipe_id = None - if recipe_id and recipe_id not in ALLOWED_CUSTOM_RECIPE_IDS: + if recipe_id and recipe_id in RECIPE_DISPENSERS: dispenser_ids = RECIPE_DISPENSERS.get(recipe_id, ()) + catalog_amounts = recipe_amounts(recipe_id) + if catalog_amounts: + dispenser_amounts = catalog_amounts if intent == "make_cocktail" and recipe_id is None and not dispenser_ids: return _fallback_decision(text, "missing_recipe_or_dispenser") @@ -286,8 +290,9 @@ def _call_chat_api(self, text: str, api_key: str) -> dict: "For descriptive preference or recommendation requests, extract wanted_traits and avoided_traits instead of calculating amounts. " "Allowed traits: sweetness, fruitiness, freshness, aroma, alcohol, bitterness, softness, light, depth, herbal. " "Examples of preferences: not too strong, light, easy to drink, rich aroma, sweet, not sweet, fruity. " - "For a plain recommendation with no preferences, choose one recipe_01..recipe_04. " - "Only choose recipe_01..recipe_04 when the user explicitly asks for a numbered/color menu or gives no preferences. " + "For a plain recommendation with no preferences, choose one of these recipe_id values: " + f"{', '.join(RECIPE_DISPENSERS)}. " + "Only choose a catalog recipe when the user explicitly asks for a numbered/named menu or gives no preferences. " "Allowed dispenser_ids values: red, yellow, green, blue only. " "Do not output dispenser_amounts; the application calculates amounts from traits. " "Never output robot coordinates, calibration values, trajectories, or safety approvals." diff --git a/src/azas_voice/azas_voice/recipe_catalog.py b/src/azas_voice/azas_voice/recipe_catalog.py index d5ff31f..ba10f45 100644 --- a/src/azas_voice/azas_voice/recipe_catalog.py +++ b/src/azas_voice/azas_voice/recipe_catalog.py @@ -1,3 +1,19 @@ +from __future__ import annotations + +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError: # pragma: no cover - deterministic fallback below keeps tests importable. + yaml = None + +try: + from ament_index_python.packages import get_package_share_directory +except ImportError: # pragma: no cover - source-tree tests do not need ROS sourced. + get_package_share_directory = None + + DISPENSER_ALIASES = { "red": ("1번", "일번", "디스펜서1", "디스펜서일", "빨강", "빨간색", "레드", "red", "빨간"), "yellow": ("2번", "이번", "디스펜서2", "디스펜서이", "노랑", "노란색", "옐로우", "yellow", "노란"), @@ -165,6 +181,8 @@ "recipe_03": ("green",), "recipe_04": ("blue",), } +RECIPE_AMOUNTS: dict[str, dict[str, int]] = {} +RECIPE_METADATA: dict[str, dict[str, object]] = {} MOOD_WORDS = ( "기분", @@ -322,3 +340,209 @@ "계속", ) CANCEL_WORDS = ("취소", "아니", "아니요", "멈춰", "중지", "그만", "정지") + + +def recipe_amounts(recipe_id: str | None) -> dict[str, int] | None: + if not recipe_id: + return None + amounts = RECIPE_AMOUNTS.get(recipe_id) + return dict(amounts) if amounts else None + + +def build_public_catalog() -> dict[str, object]: + ingredients = { + color: { + "role": role.get("role", color), + "label": role.get("label", color), + "traits": list(DISPENSER_TRAITS.get(color, ())), + "aliases": list(COLOR_ALIASES.get(color, ())), + } + for color, role in DISPENSER_ROLES.items() + } + recipes = [] + for recipe_id, dispenser_ids in RECIPE_DISPENSERS.items(): + metadata = RECIPE_METADATA.get(recipe_id, {}) + amounts = recipe_amounts(recipe_id) or {color: 1 for color in dispenser_ids} + recipes.append( + { + "recipe_id": recipe_id, + "name": RECIPE_DISPLAY_NAMES.get(recipe_id, recipe_id), + "description": RECIPE_DESCRIPTIONS.get(recipe_id, ""), + "aliases": list(RECIPE_ALIASES.get(recipe_id, ())), + "dispenser_ids": list(dispenser_ids), + "dispenser_amounts": amounts, + "tags": list(metadata.get("tags", [])), + "mood_tags": list(metadata.get("mood_tags", [])), + "color": metadata.get("color", dispenser_ids[0] if dispenser_ids else ""), + "sweetness": metadata.get("sweetness"), + "acidity": metadata.get("acidity"), + "strength": metadata.get("strength"), + } + ) + return {"ingredients": ingredients, "recipes": recipes} + + +def _candidate_config_paths() -> tuple[Path, ...]: + paths = [Path(__file__).resolve().parents[1] / "config" / "recipes.yaml"] + if get_package_share_directory is not None: + try: + paths.insert(0, Path(get_package_share_directory("azas_voice")) / "config" / "recipes.yaml") + except Exception: + pass + return tuple(paths) + + +def _read_catalog_config() -> dict[str, Any] | None: + if yaml is None: + return None + for path in _candidate_config_paths(): + if not path.is_file(): + continue + payload = yaml.safe_load(path.read_text(encoding="utf-8")) or {} + if isinstance(payload, dict): + return payload + return None + + +def _dedupe(values: list[object] | tuple[object, ...]) -> tuple[str, ...]: + result: list[str] = [] + for value in values: + text = str(value).strip() + if text and text not in result: + result.append(text) + return tuple(result) + + +def _recipe_number_aliases(recipe_id: str) -> tuple[str, ...]: + digits = "".join(ch for ch in recipe_id if ch.isdigit()) + if not digits: + return () + number = int(digits) + korean_numbers = { + 1: "일", + 2: "이", + 3: "삼", + 4: "사", + 5: "오", + 6: "육", + 7: "칠", + 8: "팔", + 9: "구", + 10: "십", + 11: "십일", + 12: "십이", + 13: "십삼", + 14: "십사", + 15: "십오", + 16: "십육", + } + aliases = [f"{number}번", f"레시피{number}", f"recipe{number}", recipe_id] + korean = korean_numbers.get(number) + if korean: + aliases.extend([f"{korean}번", f"{korean}번메뉴"]) + return tuple(aliases) + + +def _normalize_amounts(raw: object) -> dict[str, int]: + if not isinstance(raw, dict): + return {} + amounts: dict[str, int] = {} + for color in ("red", "yellow", "green", "blue"): + try: + amount = int(raw.get(color, 0)) + except (TypeError, ValueError): + amount = 0 + amounts[color] = max(0, min(amount, 3)) + return amounts + + +def _apply_catalog_config() -> None: + config = _read_catalog_config() + if not config: + return + + colors = config.get("colors") + if isinstance(colors, dict): + for color, block in colors.items(): + if color not in DISPENSER_ALIASES or not isinstance(block, dict): + continue + aliases = list(DISPENSER_ALIASES[color]) + aliases.extend(block.get("aliases", []) or []) + aliases.extend([color, block.get("role", ""), block.get("ingredient_role", "")]) + DISPENSER_ALIASES[color] = _dedupe(tuple(aliases)) + DISPENSER_TRAITS[color] = _dedupe(tuple(block.get("traits", []) or DISPENSER_TRAITS.get(color, ()))) + role_name = str(block.get("role") or DISPENSER_ROLES[color].get("role") or color) + label = str(block.get("ingredient_role") or DISPENSER_ROLES[color].get("label") or role_name) + DISPENSER_ROLES[color] = { + "role": role_name, + "label": label, + "levels": DISPENSER_ROLES[color].get("levels", ()), + } + + recipes = config.get("recipes") + if not isinstance(recipes, dict): + return + + loaded_aliases: dict[str, tuple[str, ...]] = {} + loaded_names: dict[str, str] = {} + loaded_descriptions: dict[str, str] = {} + loaded_dispensers: dict[str, tuple[str, ...]] = {} + loaded_amounts: dict[str, dict[str, int]] = {} + loaded_metadata: dict[str, dict[str, object]] = {} + + for recipe_id, block in recipes.items(): + if not isinstance(block, dict): + continue + recipe_key = str(recipe_id).strip() + if not recipe_key: + continue + amounts = _normalize_amounts(block.get("dispenser_amounts")) + raw_ids = block.get("dispenser_ids", []) + dispenser_ids = [ + str(item).strip() + for item in raw_ids + if str(item).strip() in DISPENSER_ALIASES + ] + if amounts: + dispenser_ids = [color for color in ("red", "yellow", "green", "blue") if amounts.get(color, 0) > 0] + dispenser_ids = list(_dedupe(tuple(dispenser_ids))) + if not dispenser_ids: + continue + + name = str(block.get("name") or recipe_key) + description = str(block.get("description") or "") + aliases = list(block.get("aliases", []) or []) + aliases.extend([recipe_key, name]) + aliases.extend(_recipe_number_aliases(recipe_key)) + + loaded_aliases[recipe_key] = _dedupe(tuple(aliases)) + loaded_names[recipe_key] = name + loaded_descriptions[recipe_key] = description + loaded_dispensers[recipe_key] = tuple(dispenser_ids) + if amounts: + loaded_amounts[recipe_key] = amounts + loaded_metadata[recipe_key] = { + "tags": list(block.get("tags", []) or []), + "mood_tags": list(block.get("mood_tags", []) or []), + "color": block.get("color") or dispenser_ids[0], + "sweetness": block.get("sweetness"), + "acidity": block.get("acidity"), + "strength": block.get("strength"), + } + + if loaded_dispensers: + RECIPE_ALIASES.clear() + RECIPE_ALIASES.update(loaded_aliases) + RECIPE_DISPLAY_NAMES.clear() + RECIPE_DISPLAY_NAMES.update(loaded_names) + RECIPE_DESCRIPTIONS.clear() + RECIPE_DESCRIPTIONS.update(loaded_descriptions) + RECIPE_DISPENSERS.clear() + RECIPE_DISPENSERS.update(loaded_dispensers) + RECIPE_AMOUNTS.clear() + RECIPE_AMOUNTS.update(loaded_amounts) + RECIPE_METADATA.clear() + RECIPE_METADATA.update(loaded_metadata) + + +_apply_catalog_config() diff --git a/src/azas_voice/azas_voice/voice_pipeline_executor_node.py b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py new file mode 100644 index 0000000..957cbbf --- /dev/null +++ b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py @@ -0,0 +1,245 @@ +from __future__ import annotations + +import json +import os +import signal +import subprocess +import threading + +try: + import rclpy + from rclpy.node import Node + from std_msgs.msg import String +except ImportError: # pragma: no cover - keeps pure helper tests ROS-free. + rclpy = None + Node = object + String = None + + +ALLOWED_DISPENSERS = ("red", "yellow", "green", "blue") + +# 라우터 stdout에서 단계 전환을 감지해 UI에 보여줄 한국어 단계명으로 변환한다. +# (auto_cup_flow_router의 로그 문구가 바뀌면 여기도 같이 갱신할 것) +STAGE_MARKERS: tuple[tuple[str, str], ...] = ( + ("auto cup router: color scan", "디스펜서 색 스캔"), + ("route decided: side_grasp", "컵 픽업 (세워진 컵)"), + ("route decided: cup_uprighting", "컵 픽업 (쓰러진 컵)"), + ("starting integrated dispenser recipe sequence", "디스펜서 레시피 진행"), + ("resume_state loaded", "중단 지점 복구"), + ("resume_state step_start", "디스펜서 레시피 진행"), + ("starting lid close", "뚜껑 체결 / 쉐이킹"), + ("selected flow completed", "완료"), +) + + +def recipe_colors_from_decision( + decision: dict[str, object], + *, + max_repeats_per_dispenser: int = 3, + default_amount: int = 1, +) -> str: + """confirmed decision JSON을 라우터 recipe_colors 문자열로 변환한다. + + dispenser_amounts가 있으면 그 양을, 없으면 dispenser_ids마다 default_amount를 쓴다. + 예: {"dispenser_amounts": {"yellow": 2, "blue": 1}} -> "yellow:2,blue:1" + """ + if decision.get("intent") != "make_cocktail": + return "" + + amounts_payload = decision.get("dispenser_amounts", {}) + amounts: dict[str, int] = {} + if isinstance(amounts_payload, dict): + for color in ALLOWED_DISPENSERS: + try: + amount = int(amounts_payload.get(color, 0)) + except (TypeError, ValueError): + amount = 0 + amounts[color] = max(0, min(amount, max_repeats_per_dispenser)) + + if not any(amounts.values()): + raw_ids = decision.get("dispenser_ids", []) + if not isinstance(raw_ids, list): + return "" + for raw_id in raw_ids: + color = str(raw_id).strip() + if color in ALLOWED_DISPENSERS: + amounts[color] = max( + amounts.get(color, 0), + min(default_amount, max_repeats_per_dispenser), + ) + + parts = [f"{color}:{amounts[color]}" for color in ALLOWED_DISPENSERS if amounts.get(color, 0) > 0] + return ",".join(parts) + + +def stage_from_line(line: str) -> str | None: + for marker, stage in STAGE_MARKERS: + if marker in line: + return stage + return None + + +class VoicePipelineExecutorNode(Node): + """Confirmed voice recipe -> full auto cup flow (pick -> recipe -> lid -> shake). + + voice_dispenser_executor_node가 디스펜서 프레스 단발만 실행하는 것과 달리, + 이 노드는 검증된 auto_cup_flow_router 전체 파이프라인을 래퍼 스크립트로 실행한다. + """ + + def __init__(self): + super().__init__("voice_pipeline_executor_node") + self.declare_parameter("confirmed_decision_topic", "/azas/voice/confirmed_recipe_decision") + self.declare_parameter("status_topic", "/azas/voice/pipeline_status") + self.declare_parameter("enable_hardware_execution", False) + self.declare_parameter("require_confirmed", True) + self.declare_parameter("flow_script", "/home/ssu/Azas/tools/run/run_voice_auto_cup_flow.sh") + self.declare_parameter("service_prefix", "dsr01") + self.declare_parameter("max_repeats_per_dispenser", 3) + self.declare_parameter("default_amount", 1) + + self._status_pub = self.create_publisher( + String, + str(self.get_parameter("status_topic").value), + 10, + ) + self.create_subscription( + String, + str(self.get_parameter("confirmed_decision_topic").value), + self._on_confirmed_decision, + 10, + ) + + self._lock = threading.Lock() + self._active_proc: subprocess.Popen[str] | None = None + self._active_recipe_id: str | None = None + + self.get_logger().info( + "Voice pipeline executor ready: " + f"enable_hardware_execution={bool(self.get_parameter('enable_hardware_execution').value)}" + ) + + def _on_confirmed_decision(self, msg: String) -> None: + try: + decision = json.loads(msg.data) + except json.JSONDecodeError as exc: + self._publish_status("blocked", reason="invalid_confirmed_decision_json", error=str(exc)) + return + + if bool(self.get_parameter("require_confirmed").value) and not decision.get("confirmed"): + self._publish_status("blocked", reason="decision_not_confirmed", decision=decision) + return + + recipe_colors = recipe_colors_from_decision( + decision, + max_repeats_per_dispenser=int(self.get_parameter("max_repeats_per_dispenser").value), + default_amount=int(self.get_parameter("default_amount").value), + ) + if not recipe_colors: + self._publish_status("blocked", reason="no_executable_recipe_colors", decision=decision) + return + + with self._lock: + if self._active_proc is not None and self._active_proc.poll() is None: + self._publish_status( + "busy", + reason="pipeline_already_running", + active_recipe_id=self._active_recipe_id, + rejected_recipe_id=decision.get("recipe_id"), + ) + return + self._active_recipe_id = str(decision.get("recipe_id") or "") + + command = [ + "bash", + str(self.get_parameter("flow_script").value), + recipe_colors, + ] + self._publish_status( + "starting", + recipe_id=decision.get("recipe_id"), + recipe_colors=recipe_colors, + command=command, + hardware_enabled=bool(self.get_parameter("enable_hardware_execution").value), + ) + + if not bool(self.get_parameter("enable_hardware_execution").value): + self._publish_status("dry_run", recipe_colors=recipe_colors, command=command) + return + + env = os.environ.copy() + env["ROUTER_CONFIRM"] = "ENABLE_AUTO_CUP_ROUTER" + env["SERVICE_PREFIX"] = str(self.get_parameter("service_prefix").value) + try: + proc = subprocess.Popen( + command, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + bufsize=1, + preexec_fn=os.setsid, + env=env, + ) + except OSError as exc: + self._publish_status("failed", reason="flow_failed_to_start", error=str(exc)) + return + + with self._lock: + self._active_proc = proc + threading.Thread( + target=self._monitor_pipeline, + args=(proc, recipe_colors), + daemon=True, + ).start() + + def _monitor_pipeline(self, proc: subprocess.Popen[str], recipe_colors: str) -> None: + last_stage = "" + if proc.stdout is not None: + for line in proc.stdout: + stage = stage_from_line(line) + if stage and stage != last_stage: + last_stage = stage + self._publish_status("running", stage=stage, recipe_colors=recipe_colors) + code = proc.wait() + with self._lock: + self._active_proc = None + self._active_recipe_id = None + if code == 0: + self._publish_status("completed", recipe_colors=recipe_colors) + else: + self._publish_status( + "failed", + recipe_colors=recipe_colors, + returncode=code, + last_stage=last_stage, + ) + + def _publish_status(self, status: str, **fields: object) -> None: + msg = String() + msg.data = json.dumps({"status": status, **fields}, ensure_ascii=False) + self._status_pub.publish(msg) + if status in {"blocked", "failed", "busy"}: + self.get_logger().warn(msg.data) + else: + self.get_logger().info(msg.data) + + def destroy_node(self): + with self._lock: + proc = self._active_proc + if proc is not None and proc.poll() is None: + try: + os.killpg(os.getpgid(proc.pid), signal.SIGINT) + except OSError: + pass + super().destroy_node() + + +def main(args=None): + if rclpy is None: + raise RuntimeError("rclpy is required to run voice_pipeline_executor_node") + rclpy.init(args=args) + node = VoicePipelineExecutorNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() diff --git a/src/azas_voice/azas_voice/voice_screen_node.py b/src/azas_voice/azas_voice/voice_screen_node.py index fafd9f3..c72f240 100644 --- a/src/azas_voice/azas_voice/voice_screen_node.py +++ b/src/azas_voice/azas_voice/voice_screen_node.py @@ -24,15 +24,19 @@ Node = object String = None +from azas_voice.recipe_catalog import build_public_catalog + def build_initial_state() -> dict[str, Any]: return { "started_at": time.time(), + "catalog": build_public_catalog(), "last_stt": "", "last_confirmation": "", "ui_state": {"state": "idle", "emotion": "neutral", "text": ""}, "decision": {}, "confirmed_decision": {}, + "pipeline_status": {}, "events": [], } @@ -52,6 +56,7 @@ def __init__(self): self.declare_parameter("confirmation_topic", "/azas/voice/confirmation") self.declare_parameter("ui_state_topic", "/azas/voice/ui_state") self.declare_parameter("confirmed_decision_topic", "/azas/voice/confirmed_recipe_decision") + self.declare_parameter("pipeline_status_topic", "/azas/voice/pipeline_status") self._lock = threading.Lock() self._events: deque[dict[str, Any]] = deque(maxlen=12) @@ -92,6 +97,12 @@ def __init__(self): self._on_confirmed_decision, 10, ) + self.create_subscription( + String, + str(self.get_parameter("pipeline_status_topic").value), + self._on_pipeline_status, + 10, + ) self._web_root = Path(get_package_share_directory("azas_voice")) / "web" host = str(self.get_parameter("host").value) @@ -153,6 +164,11 @@ def _on_confirmed_decision(self, msg: String) -> None: self._state["confirmed_decision"] = confirmed self._state["confirmed_decision_at"] = time.time() + def _on_pipeline_status(self, msg: String) -> None: + with self._lock: + self._state["pipeline_status"] = _json_or_text(msg.data) + self._state["pipeline_status_at"] = time.time() + def _remember(self, speaker: str, text: str) -> None: with self._lock: self._events.appendleft( diff --git a/src/azas_voice/config/recipes.yaml b/src/azas_voice/config/recipes.yaml index fb66d9c..56f4d78 100644 --- a/src/azas_voice/config/recipes.yaml +++ b/src/azas_voice/config/recipes.yaml @@ -74,6 +74,162 @@ recipes: mood_tags: [relaxed, playful] sweetness: 4 acidity: 1 - strength: 0 + strength: 3 color: blue description: 시원하고 부드러운 블루 계열 메뉴 + + recipe_05: + name: 선셋 하이볼 + aliases: [5번, 오번, 레시피5, 선셋, 선셋하이볼, 노을, 과일하이볼] + dispenser_ids: [red, yellow, blue] + dispenser_amounts: {red: 2, yellow: 1, green: 0, blue: 1} + tags: [fruity, sweet, highball] + mood_tags: [cheerful, bright] + sweetness: 4 + acidity: 3 + strength: 2 + color: red + description: 주스의 과일감에 시럽과 럼을 살짝 더한 밝은 하이볼 + + recipe_06: + name: 허브 토닉 + aliases: [6번, 육번, 레시피6, 허브토닉, 허브, 깔끔한허브, 향좋은거] + dispenser_ids: [red, yellow, green] + dispenser_amounts: {red: 1, yellow: 1, green: 2, blue: 0} + tags: [herbal, clean, aromatic] + mood_tags: [calm, clean] + sweetness: 2 + acidity: 3 + strength: 0 + color: green + description: 리큐르 향을 중심으로 산뜻하게 마무리되는 논알콜 허브 톤 + + recipe_07: + name: 베리 스위트 + aliases: [7번, 칠번, 레시피7, 베리스위트, 베리, 달달한베리, 달콤한거] + dispenser_ids: [red, yellow] + dispenser_amounts: {red: 2, yellow: 2, green: 0, blue: 0} + tags: [sweet, fruity, soft] + mood_tags: [comfort, cheerful] + sweetness: 5 + acidity: 2 + strength: 0 + color: red + description: 주스와 시럽을 균형 있게 섞은 달콤한 베리 계열 + + recipe_08: + name: 블루 라군 + aliases: [8번, 팔번, 레시피8, 블루라군, 라군, 시원한거, 파란칵테일] + dispenser_ids: [red, yellow, blue] + dispenser_amounts: {red: 1, yellow: 1, green: 0, blue: 2} + tags: [cool, blue, balanced] + mood_tags: [relaxed, playful] + sweetness: 3 + acidity: 2 + strength: 3 + color: blue + description: 럼의 존재감에 과일감과 달콤함을 얹은 시원한 블루 믹스 + + recipe_09: + name: 프루트 펀치 + aliases: [9번, 구번, 레시피9, 프루트펀치, 과일펀치, 무알콜펀치, 논알콜펀치] + dispenser_ids: [red, yellow, green] + dispenser_amounts: {red: 3, yellow: 1, green: 1, blue: 0} + tags: [non_alcoholic, fruity, fresh] + mood_tags: [refreshing, bright] + sweetness: 3 + acidity: 4 + strength: 0 + color: red + description: 럼 없이 주스 중심으로 상큼하게 만든 과일 펀치 + + recipe_10: + name: 드라이 허브 쿨러 + aliases: [10번, 십번, 레시피10, 드라이허브, 허브쿨러, 덜단거, 드라이한거] + dispenser_ids: [red, green, blue] + dispenser_amounts: {red: 1, yellow: 0, green: 2, blue: 1} + tags: [dry, herbal, light_alcohol] + mood_tags: [calm, focused] + sweetness: 1 + acidity: 3 + strength: 2 + color: green + description: 단맛을 낮추고 허브 향과 약한 럼 감을 살린 드라이 계열 + + recipe_11: + name: 시트러스 스플래시 + aliases: [11번, 십일번, 레시피11, 시트러스, 스플래시, 상큼한거, 새콤한거] + dispenser_ids: [red, yellow, green] + dispenser_amounts: {red: 1, yellow: 2, green: 1, blue: 0} + tags: [fresh, citrus, bright] + mood_tags: [refreshing, energetic] + sweetness: 3 + acidity: 5 + strength: 0 + color: yellow + description: 시럽의 밝은 단맛과 리큐르 향을 곁들인 상큼한 논알콜 믹스 + + recipe_12: + name: 딥 럼 펀치 + aliases: [12번, 십이번, 레시피12, 딥럼, 럼펀치, 강한거, 도수있는거] + dispenser_ids: [red, yellow, blue] + dispenser_amounts: {red: 1, yellow: 1, green: 0, blue: 3} + tags: [strong, rum, deep] + mood_tags: [bold, relaxed] + sweetness: 3 + acidity: 1 + strength: 5 + color: blue + description: 럼을 강하게 잡고 주스와 시럽으로 마무리한 진한 펀치 + + recipe_13: + name: 릴랙스 가든 + aliases: [13번, 십삼번, 레시피13, 릴랙스, 가든, 편한거, 부드러운허브] + dispenser_ids: [yellow, green, blue] + dispenser_amounts: {red: 0, yellow: 2, green: 2, blue: 1} + tags: [soft, herbal, relaxed] + mood_tags: [relaxed, calm] + sweetness: 4 + acidity: 1 + strength: 2 + color: green + description: 부드러운 시럽과 허브 향에 럼을 약하게 더한 차분한 메뉴 + + recipe_14: + name: 라이트 과일 소다 + aliases: [14번, 십사번, 레시피14, 라이트소다, 과일소다, 가벼운거, 부담없는거] + dispenser_ids: [red, green] + dispenser_amounts: {red: 2, yellow: 0, green: 1, blue: 0} + tags: [light, fruity, non_alcoholic] + mood_tags: [easy, refreshing] + sweetness: 2 + acidity: 3 + strength: 0 + color: red + description: 주스 중심에 향만 가볍게 얹은 부담 없는 논알콜 과일 소다 + + recipe_15: + name: 스위트 아로마 + aliases: [15번, 십오번, 레시피15, 스위트아로마, 향달달, 향좋고달달한거] + dispenser_ids: [red, yellow, green] + dispenser_amounts: {red: 1, yellow: 2, green: 2, blue: 0} + tags: [sweet, aromatic, soft] + mood_tags: [comfort, cheerful] + sweetness: 5 + acidity: 2 + strength: 0 + color: yellow + description: 달콤함과 리큐르 향을 함께 살린 부드러운 아로마 메뉴 + + recipe_16: + name: 클린 그린 + aliases: [16번, 십육번, 레시피16, 클린그린, 그린논알콜, 깔끔한논알콜, 쓴맛덜한허브] + dispenser_ids: [red, yellow, green] + dispenser_amounts: {red: 1, yellow: 1, green: 2, blue: 0} + tags: [clean, herbal, non_alcoholic] + mood_tags: [clean, calm] + sweetness: 2 + acidity: 3 + strength: 0 + color: green + description: 럼 없이 허브 향을 깔끔하게 살린 그린 계열 논알콜 메뉴 diff --git a/src/azas_voice/launch/azas_voice.launch.py b/src/azas_voice/launch/azas_voice.launch.py index 3203b8a..4f0152e 100644 --- a/src/azas_voice/launch/azas_voice.launch.py +++ b/src/azas_voice/launch/azas_voice.launch.py @@ -25,6 +25,9 @@ def generate_launch_description(): DeclareLaunchArgument("use_conversation_manager", default_value="true"), DeclareLaunchArgument("use_dispenser_executor", default_value="false"), DeclareLaunchArgument("enable_dispenser_hardware_execution", default_value="false"), + DeclareLaunchArgument("use_pipeline_executor", default_value="false"), + DeclareLaunchArgument("enable_pipeline_hardware_execution", default_value="false"), + DeclareLaunchArgument("pipeline_service_prefix", default_value="dsr01"), DeclareLaunchArgument("dispenser_service_prefix", default_value="/"), DeclareLaunchArgument("dispenser_tcp_name", default_value=""), DeclareLaunchArgument( @@ -123,6 +126,22 @@ def generate_launch_description(): ], condition=IfCondition(use_dispenser_executor), ), + Node( + package="azas_voice", + executable="voice_pipeline_executor_node", + name="voice_pipeline_executor_node", + output="screen", + parameters=[ + { + "enable_hardware_execution": ParameterValue( + LaunchConfiguration("enable_pipeline_hardware_execution"), + value_type=bool, + ), + "service_prefix": LaunchConfiguration("pipeline_service_prefix"), + } + ], + condition=IfCondition(LaunchConfiguration("use_pipeline_executor")), + ), Node( package="azas_voice", executable="stt_node", diff --git a/src/azas_voice/package.xml b/src/azas_voice/package.xml index 4a13158..9bfc312 100644 --- a/src/azas_voice/package.xml +++ b/src/azas_voice/package.xml @@ -7,12 +7,14 @@ MIT rclpy + ament_index_python azas_gripper geometry_msgs moveit_msgs std_msgs moveit_py python3-numpy + python3-yaml python3-pytest diff --git a/src/azas_voice/setup.py b/src/azas_voice/setup.py index 93bf1d5..89a3a81 100644 --- a/src/azas_voice/setup.py +++ b/src/azas_voice/setup.py @@ -29,6 +29,7 @@ "stt_node = azas_voice.stt_node:main", "tts_node = azas_voice.tts_node:main", "voice_dispenser_executor_node = azas_voice.voice_dispenser_executor_node:main", + "voice_pipeline_executor_node = azas_voice.voice_pipeline_executor_node:main", "voice_screen_node = azas_voice.voice_screen_node:main", "stt_pick_and_place_legacy = azas_voice.stt_pick_and_place_legacy:main", "stt_robot_control_legacy = azas_voice.stt_robot_control_legacy:main", diff --git a/src/azas_voice/test/test_command_parser.py b/src/azas_voice/test/test_command_parser.py index c734d0e..7948c18 100644 --- a/src/azas_voice/test/test_command_parser.py +++ b/src/azas_voice/test/test_command_parser.py @@ -1,4 +1,5 @@ from azas_voice.command_parser import normalize_text, parse_recipe_command +from azas_voice.recipe_catalog import RECIPE_DISPENSERS, build_public_catalog def assert_dispenser_colors(dispenser_ids): @@ -49,6 +50,25 @@ def test_four_menu_recipes_map_to_one_dispenser_each(): assert decision.dispenser_ids == dispenser_ids +def test_yaml_catalog_exposes_many_named_menus(): + catalog = build_public_catalog() + assert len(catalog["recipes"]) >= 12 + assert "recipe_12" in RECIPE_DISPENSERS + + +def test_named_yaml_recipe_maps_to_catalog_amounts(): + decision = parse_recipe_command("선셋 하이볼 만들어줘") + assert decision.valid + assert decision.recipe_id == "recipe_05" + assert decision.dispenser_ids == ("red", "yellow", "blue") + assert decision.dispenser_amounts == { + "red": 2, + "yellow": 1, + "green": 0, + "blue": 1, + } + + def test_mood_request_maps_to_custom_recommendation(): decision = parse_recipe_command("오늘 기분이 우울한데 칵테일 추천해줘") assert decision.valid diff --git a/src/azas_voice/test/test_llm_recipe_mapper.py b/src/azas_voice/test/test_llm_recipe_mapper.py index 9063b7a..618c0b1 100644 --- a/src/azas_voice/test/test_llm_recipe_mapper.py +++ b/src/azas_voice/test/test_llm_recipe_mapper.py @@ -66,6 +66,28 @@ def test_sanitize_llm_decision_fills_recipe_dispenser_ids(): assert "진행할까요" in decision.confirmation +def test_sanitize_llm_decision_accepts_expanded_catalog_recipe_amounts(): + decision = _sanitize_llm_decision( + "딥 럼 펀치 만들어줘", + { + "intent": "make_cocktail", + "recipe_id": "recipe_12", + "dispenser_ids": [], + "confirmation": "", + }, + ) + + assert decision.valid + assert decision.recipe_id == "recipe_12" + assert decision.dispenser_ids == ("red", "yellow", "blue") + assert decision.dispenser_amounts == { + "red": 1, + "yellow": 1, + "green": 0, + "blue": 3, + } + + def test_sanitize_llm_decision_preserves_recommendation_wording(): decision = _sanitize_llm_decision( "추천해줘", @@ -84,7 +106,8 @@ def test_sanitize_llm_decision_preserves_recommendation_wording(): assert "추천" in decision.confirmation assert "진행할까요" in decision.confirmation assert decision.profile is None - assert decision.dispenser_amounts is None + if decision.dispenser_amounts is not None: + assert all(color in {"red", "yellow", "green", "blue"} for color in decision.dispenser_amounts) def test_sanitize_llm_decision_prefers_local_preference_recommendation(): diff --git a/src/azas_voice/web/voice.css b/src/azas_voice/web/voice.css index aecb3fe..e781c2d 100644 --- a/src/azas_voice/web/voice.css +++ b/src/azas_voice/web/voice.css @@ -44,15 +44,537 @@ input { border: 0; } -.voice-shell { +.app-shell { display: grid; - gap: 14px; - width: min(100%, 540px); + grid-template-columns: minmax(0, 480px) minmax(0, 1fr); + gap: 18px; + width: min(100%, 1180px); min-height: 100vh; margin: 0 auto; padding: 18px; } +.voice-shell { + display: grid; + gap: 14px; + align-content: start; + min-width: 0; +} + +.menu-stage { + display: grid; + gap: 14px; + align-content: start; + min-width: 0; +} + +.menu-header { + display: flex; + align-items: flex-start; + justify-content: space-between; + gap: 14px; +} + +.menu-header-status { + display: grid; + justify-items: end; + gap: 8px; +} + +.catalog-count { + color: var(--muted); + font-size: 13px; + font-weight: 900; +} + +.menu-badge { + flex: 0 0 auto; + padding: 10px 13px; + border-radius: 999px; + font-size: 14px; + font-weight: 900; + background: rgba(255, 255, 255, 0.8); + color: var(--muted); + box-shadow: 0 8px 22px rgba(42, 129, 101, 0.13); +} + +.menu-badge.recommended { + color: #a05f00; + background: linear-gradient(135deg, rgba(255, 226, 143, 0.95), rgba(255, 255, 255, 0.8)); +} + +.menu-badge.confirmed { + color: #10745b; + background: linear-gradient(135deg, rgba(178, 245, 216, 0.95), rgba(255, 255, 255, 0.8)); +} + +.menu-badge.making { + color: #0f5fa8; + background: linear-gradient(135deg, rgba(176, 219, 255, 0.95), rgba(255, 255, 255, 0.8)); + animation: breathe 1.6s ease-in-out infinite; +} + +.menu-badge.done { + color: #ffffff; + background: linear-gradient(135deg, #34c08f, #2aa4d8); +} + +.menu-badge.failed { + color: #a8253c; + background: linear-gradient(135deg, rgba(255, 196, 206, 0.95), rgba(255, 255, 255, 0.8)); +} + +.menu-empty { + display: grid; + justify-items: center; + gap: 12px; + padding: 40px 18px; + border: 1px dashed rgba(38, 54, 49, 0.22); + border-radius: 8px; + background: rgba(255, 255, 255, 0.55); + color: var(--muted); + text-align: center; + font-weight: 700; + line-height: 1.5; +} + +.empty-glass svg { + width: 130px; + height: 162px; + opacity: 0.55; +} + +.empty-mark { + font-size: 56px; + font-weight: 900; + fill: rgba(110, 129, 121, 0.5); +} + +.menu-card { + display: grid; + grid-template-columns: minmax(0, 220px) minmax(0, 1fr); + gap: 18px; + align-items: center; + padding: 22px; + border: 1px solid var(--line); + border-radius: 8px; + background: + linear-gradient(150deg, rgba(255, 255, 255, 0.9), rgba(255, 248, 228, 0.7)), + radial-gradient(circle at 80% 12%, rgba(109, 180, 255, 0.12), transparent 36%); + box-shadow: 0 22px 50px rgba(67, 119, 96, 0.16); +} + +.glass-wrap svg { + width: 100%; + max-width: 220px; + height: auto; + display: block; + margin: 0 auto; +} + +.preview-label { + display: block; + margin-bottom: 8px; + color: var(--muted); + font-size: 13px; + font-weight: 900; + text-align: center; +} + +.glass-line { + fill: none; + stroke: rgba(38, 54, 49, 0.6); + stroke-width: 3; + stroke-linejoin: round; +} + +.glass-layer { + transition: y 600ms ease, height 600ms ease; +} + +.bubble-dot { + fill: rgba(255, 255, 255, 0.65); +} + +.straw { + stroke: #ff9a76; + stroke-width: 6; + stroke-linecap: round; +} + +.garnish { + fill: #ffd464; + stroke: #f0a32d; + stroke-width: 2.4; +} + +.garnish-cut { + stroke: #f0a32d; + stroke-width: 2; +} + +.menu-info { + display: grid; + gap: 10px; + min-width: 0; +} + +.menu-info h2 { + margin: 0; + font-size: 28px; + line-height: 1.15; +} + +.menu-info p { + margin: 0; + color: var(--muted); + font-weight: 700; + line-height: 1.45; +} + +.ingredient-chips { + display: flex; + flex-wrap: wrap; + gap: 8px; + margin: 0; + padding: 0; + list-style: none; +} + +.ingredient-chips li { + display: inline-flex; + align-items: center; + gap: 7px; + padding: 8px 12px; + border-radius: 999px; + background: rgba(255, 255, 255, 0.85); + border: 1px solid var(--line); + font-size: 14px; + font-weight: 900; +} + +.ingredient-chips .swatch { + width: 14px; + height: 14px; + border-radius: 50%; + box-shadow: inset 0 -2px 4px rgba(0, 0, 0, 0.12); +} + +.drink-stats { + display: grid; + grid-template-columns: repeat(3, minmax(0, 1fr)); + gap: 8px; + margin: 0; +} + +.drink-stats div { + min-width: 0; + padding: 9px 10px; + border: 1px solid var(--line); + border-radius: 8px; + background: rgba(255, 255, 255, 0.72); +} + +.drink-stats dt { + color: var(--muted); + font-size: 12px; + font-weight: 900; +} + +.drink-stats dd { + margin: 4px 0 0; + font-size: 16px; + font-weight: 900; +} + +.pipeline-steps { + display: flex; + flex-wrap: wrap; + gap: 6px; + margin: 4px 0 0; + padding: 0; + list-style: none; + counter-reset: step; +} + +.pipeline-steps li { + padding: 7px 11px; + border-radius: 999px; + font-size: 13px; + font-weight: 900; + color: var(--muted); + background: rgba(255, 255, 255, 0.6); + border: 1px solid var(--line); +} + +.pipeline-steps li.active { + color: #0f5fa8; + background: linear-gradient(135deg, rgba(176, 219, 255, 0.95), rgba(255, 255, 255, 0.85)); + animation: breathe 1.6s ease-in-out infinite; +} + +.pipeline-steps li.done { + color: #10745b; + background: rgba(178, 245, 216, 0.7); +} + +.robot-process { + display: grid; + gap: 8px; + margin-top: 2px; +} + +.robot-scene { + position: relative; + height: 132px; + overflow: hidden; + border-radius: 8px; + background: + linear-gradient(180deg, rgba(245, 250, 246, 0.92), rgba(255, 255, 255, 0.74)), + linear-gradient(90deg, rgba(95, 216, 173, 0.16), rgba(255, 212, 100, 0.18)); + border: 1px solid var(--line); +} + +.robot-scene::after { + content: ""; + position: absolute; + left: 12px; + right: 12px; + bottom: 22px; + height: 3px; + background: rgba(38, 54, 49, 0.16); +} + +.robot-base { + position: absolute; + left: 28px; + bottom: 25px; + width: 54px; + height: 22px; + border-radius: 8px 8px 4px 4px; + background: #2f4b43; +} + +.robot-arm { + position: absolute; + left: 64px; + bottom: 45px; + height: 13px; + border-radius: 8px; + background: #39cfa2; + transform-origin: 0 50%; + transition: transform 420ms ease, width 420ms ease; +} + +.robot-arm.lower { + width: 78px; + transform: rotate(-22deg); +} + +.robot-arm.upper { + width: 64px; + left: 128px; + bottom: 73px; + transform: rotate(18deg); + background: #ffd464; +} + +.robot-gripper { + position: absolute; + left: 184px; + bottom: 82px; + width: 28px; + height: 18px; + border: 4px solid #2f4b43; + border-left: 0; + border-radius: 0 8px 8px 0; + transition: left 420ms ease, bottom 420ms ease; +} + +.robot-cup { + position: absolute; + left: 200px; + bottom: 31px; + width: 32px; + height: 42px; + border: 3px solid rgba(38, 54, 49, 0.64); + border-top-width: 4px; + border-radius: 4px 4px 10px 10px; + background: rgba(255, 255, 255, 0.55); + transition: left 420ms ease, bottom 420ms ease, transform 420ms ease; +} + +.robot-cup span { + position: absolute; + left: 4px; + right: 4px; + bottom: 4px; + height: 18px; + border-radius: 2px 2px 7px 7px; + background: linear-gradient(180deg, #ff7e96, #ffd464); +} + +.robot-dispenser { + position: absolute; + right: 24px; + bottom: 25px; + width: 54px; + height: 78px; + border-radius: 8px 8px 4px 4px; + background: #e8efe9; + border: 1px solid rgba(38, 54, 49, 0.2); +} + +.robot-dispenser span { + position: absolute; + left: 17px; + bottom: 14px; + width: 20px; + height: 42px; + border-radius: 8px; + background: linear-gradient(180deg, #6db4ff, #69d98a); +} + +.robot-shaker { + position: absolute; + right: 98px; + bottom: 30px; + width: 28px; + height: 58px; + border-radius: 6px 6px 10px 10px; + background: linear-gradient(180deg, #dfe8e2, #a8b8ae); + opacity: 0.44; +} + +.robot-scene[data-step="scan"] .robot-cup { + transform: translateY(-4px); +} + +.robot-scene[data-step="pick"] .robot-cup { + left: 176px; + bottom: 65px; +} + +.robot-scene[data-step="pick"] .robot-gripper { + left: 172px; + bottom: 84px; +} + +.robot-scene[data-step="dispense"] .robot-cup { + left: calc(100% - 84px); +} + +.robot-scene[data-step="dispense"] .robot-gripper { + left: calc(100% - 112px); +} + +.robot-scene[data-step="shake"] .robot-cup { + left: calc(100% - 142px); + transform: rotate(-8deg); + animation: cupShake 520ms ease-in-out infinite; +} + +.robot-scene[data-step="done"] .robot-cup { + left: calc(100% - 92px); + bottom: 31px; +} + +.robot-status-text { + margin: 0; + color: var(--muted); + font-size: 13px; + font-weight: 900; +} + +.catalog-panel { + display: grid; + gap: 10px; + padding: 16px; + border: 1px solid var(--line); + border-radius: 8px; + background: rgba(255, 255, 255, 0.62); +} + +.catalog-title-row { + display: flex; + align-items: baseline; + justify-content: space-between; + gap: 12px; +} + +.catalog-title-row h2 { + margin: 0; + font-size: 20px; +} + +.catalog-title-row span { + color: var(--muted); + font-size: 13px; + font-weight: 900; +} + +.catalog-list { + display: grid; + grid-template-columns: repeat(auto-fit, minmax(185px, 1fr)); + gap: 8px; + max-height: 300px; + overflow: auto; + padding-right: 2px; +} + +.catalog-item { + display: grid; + grid-template-columns: 32px minmax(0, 1fr); + gap: 9px; + align-items: start; + min-width: 0; + padding: 10px; + border: 1px solid var(--line); + border-radius: 8px; + background: rgba(255, 255, 255, 0.78); +} + +.catalog-item.selected { + outline: 3px solid rgba(57, 207, 162, 0.32); + background: rgba(231, 255, 244, 0.86); +} + +.catalog-art { + display: flex; + align-items: end; + width: 28px; + height: 42px; + overflow: hidden; + border: 2px solid rgba(38, 54, 49, 0.55); + border-radius: 4px 4px 8px 8px; + background: rgba(255, 255, 255, 0.48); +} + +.catalog-layer { + flex: 1; + align-self: stretch; +} + +.catalog-copy { + min-width: 0; +} + +.catalog-copy strong { + display: block; + overflow-wrap: anywhere; + font-size: 15px; +} + +.catalog-copy span { + display: block; + margin-top: 3px; + color: var(--muted); + overflow-wrap: anywhere; + font-size: 12px; + font-weight: 800; + line-height: 1.35; +} + .voice-header { display: flex; align-items: flex-start; @@ -271,8 +793,39 @@ h1 { } } +@keyframes cupShake { + 0%, + 100% { + transform: rotate(-8deg) translateX(0); + } + 50% { + transform: rotate(8deg) translateX(5px); + } +} + +@media (max-width: 960px) { + .app-shell { + grid-template-columns: 1fr; + } + + .menu-card { + grid-template-columns: 1fr; + justify-items: center; + text-align: center; + } + + .ingredient-chips, + .pipeline-steps { + justify-content: center; + } + + .drink-stats { + width: 100%; + } +} + @media (max-width: 420px) { - .voice-shell { + .app-shell { padding: 12px; } @@ -297,4 +850,8 @@ h1 { .status-grid { grid-template-columns: 1fr; } + + .drink-stats { + grid-template-columns: 1fr; + } } diff --git a/src/azas_voice/web/voice.html b/src/azas_voice/web/voice.html index 94b8526..f2d85e8 100644 --- a/src/azas_voice/web/voice.html +++ b/src/azas_voice/web/voice.html @@ -7,56 +7,157 @@ -
-
-
-

Azas Voice

-

대화 상태

-
- 대기 중 -
- -
-
- - - -
- - -
- -
-
- 사용자 -

아직 인식된 발화가 없습니다.

-
-
- Azas -

말씀해주시면 주문을 도와드릴게요.

-
-
- -
-
- 선택 메뉴 - - -
-
- 의도 - 대기 -
-
- 확정 - 대기 -
-
- -
- - -
-
+
+
+
+
+

Azas Voice

+

대화 상태

+
+ 대기 중 +
+ +
+
+ + + +
+ + +
+ +
+
+ 사용자 +

아직 인식된 발화가 없습니다.

+
+
+ Azas +

말씀해주시면 주문을 도와드릴게요.

+
+
+ +
+
+ 선택 메뉴 + - +
+
+ 의도 + 대기 +
+
+ 확정 + 대기 +
+
+ +
+ + +
+
+ + +
diff --git a/src/azas_voice/web/voice.js b/src/azas_voice/web/voice.js index 9aca0ab..394191d 100644 --- a/src/azas_voice/web/voice.js +++ b/src/azas_voice/web/voice.js @@ -10,6 +10,332 @@ const intent = document.querySelector("#intent"); const confirmed = document.querySelector("#confirmed"); const testForm = document.querySelector("#test-form"); const testUtterance = document.querySelector("#test-utterance"); +const menuBadge = document.querySelector("#menu-badge"); +const menuEmpty = document.querySelector("#menu-empty"); +const menuCard = document.querySelector("#menu-card"); +const menuName = document.querySelector("#menu-name"); +const menuDesc = document.querySelector("#menu-desc"); +const glassLayers = document.querySelector("#glass-layers"); +const ingredientChips = document.querySelector("#ingredient-chips"); +const pipelineSteps = [...document.querySelectorAll("#pipeline-steps li")]; +const catalogCount = document.querySelector("#catalog-count"); +const catalogSummary = document.querySelector("#catalog-summary"); +const catalogList = document.querySelector("#catalog-list"); +const statSweetness = document.querySelector("#stat-sweetness"); +const statAcidity = document.querySelector("#stat-acidity"); +const statStrength = document.querySelector("#stat-strength"); +const robotScene = document.querySelector("#robot-scene"); +const robotStatusText = document.querySelector("#robot-status-text"); + +const INGREDIENTS = { + red: { label: "주스", color: "#ff7e96" }, + yellow: { label: "시럽", color: "#ffd464" }, + green: { label: "리큐르", color: "#69d98a" }, + blue: { label: "럼", color: "#6db4ff" }, +}; + +const RECIPE_NAMES = { + recipe_01: "레드 메뉴", + recipe_02: "옐로우 메뉴", + recipe_03: "그린 메뉴", + recipe_04: "블루 메뉴", + custom_preference_mix: "나만의 추천 믹스", + custom_color_selection: "커스텀 선택", +}; + +const RECIPE_DESCRIPTIONS = { + recipe_01: "주스 중심이라 과일감이 선명하고 가볍게 마시기 좋아요.", + recipe_02: "시럽 중심이라 달콤하고 부드러운 느낌이 강해요.", + recipe_03: "리큐르 중심이라 향이 선명하고 깔끔한 여운이 있어요.", + recipe_04: "럼 중심이라 칵테일다운 존재감과 깊이가 있어요.", + custom_preference_mix: "말씀하신 취향에 맞춰 재료 비율을 조합했어요.", + custom_color_selection: "고르신 색 재료 그대로 만들어드려요.", +}; + +let catalogSignature = ""; + +// 라우터 단계명(/azas/voice/pipeline_status의 stage) -> 진행 스텝 인덱스 +const STAGE_TO_STEP = { + "디스펜서 색 스캔": 0, + "컵 픽업 (세워진 컵)": 1, + "컵 픽업 (쓰러진 컵)": 1, + "디스펜서 레시피 진행": 2, + "중단 지점 복구": 2, + "뚜껑 체결 / 쉐이킹": 3, + "완료": 4, +}; + +// 잔 내부(clip-path 기준): y 30~167, x 33~127 +const GLASS_TOP = 30; +const GLASS_BOTTOM = 167; +const FILL_RATIO = 0.86; + +function amountsFromDecision(decision) { + const amounts = {}; + const payload = decision.dispenser_amounts || {}; + for (const color of Object.keys(INGREDIENTS)) { + const value = Number(payload[color] || 0); + if (value > 0) amounts[color] = Math.min(value, 3); + } + if (Object.keys(amounts).length === 0 && Array.isArray(decision.dispenser_ids)) { + for (const color of decision.dispenser_ids) { + if (INGREDIENTS[color]) amounts[color] = 1; + } + } + return amounts; +} + +function recipeCatalog(state) { + const recipes = state.catalog && Array.isArray(state.catalog.recipes) ? state.catalog.recipes : []; + return recipes; +} + +function recipeInfo(state, recipeKey) { + return recipeCatalog(state).find((recipe) => recipe.recipe_id === recipeKey) || null; +} + +function amountsFromRecipeInfo(info) { + const amounts = {}; + const payload = (info && info.dispenser_amounts) || {}; + for (const color of Object.keys(INGREDIENTS)) { + const value = Number(payload[color] || 0); + if (value > 0) amounts[color] = Math.min(value, 3); + } + if (Object.keys(amounts).length === 0 && info && Array.isArray(info.dispenser_ids)) { + for (const color of info.dispenser_ids) { + if (INGREDIENTS[color]) amounts[color] = 1; + } + } + return amounts; +} + +function selectedDecision(state) { + const decision = state.decision || {}; + const confirmedDecision = state.confirmed_decision || {}; + if (confirmedDecision.intent === "make_cocktail") return confirmedDecision; + if (decision.intent === "make_cocktail") return decision; + return null; +} + +function selectedRecipeKey(state) { + const shown = selectedDecision(state); + return shown ? String(shown.recipe_id || "") : ""; +} + +function renderGlass(amounts) { + const total = Object.values(amounts).reduce((sum, value) => sum + value, 0); + glassLayers.replaceChildren(); + if (total <= 0) return; + const innerHeight = (GLASS_BOTTOM - GLASS_TOP) * FILL_RATIO; + let bottom = GLASS_BOTTOM; + for (const color of ["blue", "green", "yellow", "red"]) { + const value = amounts[color]; + if (!value) continue; + const height = (value / total) * innerHeight; + const rect = document.createElementNS("http://www.w3.org/2000/svg", "rect"); + rect.setAttribute("class", "glass-layer"); + rect.setAttribute("x", "20"); + rect.setAttribute("width", "120"); + rect.setAttribute("y", String(bottom - height)); + rect.setAttribute("height", String(height + 1)); + rect.setAttribute("fill", INGREDIENTS[color].color); + glassLayers.appendChild(rect); + bottom -= height; + } +} + +function renderStats(info) { + const fields = [ + [statSweetness, info && info.sweetness], + [statAcidity, info && info.acidity], + [statStrength, info && info.strength], + ]; + for (const [node, value] of fields) { + node.textContent = value === null || value === undefined || value === "" ? "-" : `${value}/5`; + } +} + +function renderChips(amounts) { + ingredientChips.replaceChildren(); + for (const color of ["red", "yellow", "green", "blue"]) { + const value = amounts[color]; + if (!value) continue; + const item = document.createElement("li"); + const swatch = document.createElement("span"); + swatch.className = "swatch"; + swatch.style.background = INGREDIENTS[color].color; + item.append(swatch, `${INGREDIENTS[color].label} ×${value}`); + ingredientChips.appendChild(item); + } +} + +function setBadge(kind, text) { + menuBadge.className = `menu-badge ${kind}`; + menuBadge.textContent = text; +} + +function renderSteps(pipeline) { + const status = pipeline.status || ""; + let activeIndex = -1; + if (status === "running" && pipeline.stage in STAGE_TO_STEP) { + activeIndex = STAGE_TO_STEP[pipeline.stage]; + } else if (status === "starting") { + activeIndex = 0; + } else if (status === "completed") { + activeIndex = pipelineSteps.length; + } + pipelineSteps.forEach((step, index) => { + step.classList.toggle("done", activeIndex > index); + step.classList.toggle("active", activeIndex === index); + }); + return activeIndex; +} + +function renderRobot(activeIndex, pipeline, hasMenu) { + if (!hasMenu) { + robotScene.dataset.step = "idle"; + robotStatusText.textContent = "주문 대기"; + return; + } + const status = pipeline.status || ""; + if (status === "failed") { + robotScene.dataset.step = "idle"; + robotStatusText.textContent = "제조 중단"; + return; + } + if (status === "completed" || activeIndex >= pipelineSteps.length) { + robotScene.dataset.step = "done"; + robotStatusText.textContent = "완료"; + return; + } + const stepNames = ["scan", "pick", "dispense", "shake", "done"]; + const statusText = ["디스펜서 색 스캔", "컵 픽업", "디스펜서 토출", "뚜껑 체결 / 쉐이킹", "완료"]; + const index = activeIndex >= 0 ? activeIndex : 0; + robotScene.dataset.step = stepNames[Math.min(index, stepNames.length - 1)]; + robotStatusText.textContent = pipeline.stage || statusText[Math.min(index, statusText.length - 1)]; +} + +function catalogArt(amounts) { + const wrapper = document.createElement("span"); + wrapper.className = "catalog-art"; + const colors = Object.entries(amounts).filter(([, value]) => value > 0); + if (!colors.length) { + const empty = document.createElement("span"); + empty.className = "catalog-layer"; + empty.style.background = "rgba(110, 129, 121, 0.25)"; + wrapper.appendChild(empty); + return wrapper; + } + for (const [color, value] of colors) { + const layer = document.createElement("span"); + layer.className = "catalog-layer"; + layer.style.background = INGREDIENTS[color].color; + layer.style.flexGrow = String(value); + wrapper.appendChild(layer); + } + return wrapper; +} + +function renderCatalog(state) { + const recipes = recipeCatalog(state); + const selectedKey = selectedRecipeKey(state); + catalogCount.textContent = `메뉴 ${recipes.length}개`; + catalogSummary.textContent = recipes.length ? "클릭해서 주문 입력" : "YAML 카탈로그 대기"; + const signature = JSON.stringify(recipes.map((recipe) => [ + recipe.recipe_id, + recipe.name, + recipe.description, + recipe.dispenser_amounts, + ])); + if (signature !== catalogSignature) { + catalogSignature = signature; + catalogList.replaceChildren(); + for (const recipe of recipes) { + const amounts = amountsFromRecipeInfo(recipe); + const button = document.createElement("button"); + button.type = "button"; + button.className = "catalog-item"; + button.dataset.recipeId = recipe.recipe_id; + button.setAttribute("aria-label", `${recipe.name} 주문`); + button.appendChild(catalogArt(amounts)); + + const copy = document.createElement("span"); + copy.className = "catalog-copy"; + const name = document.createElement("strong"); + name.textContent = recipe.name || recipe.recipe_id; + const desc = document.createElement("span"); + desc.textContent = recipe.description || "카탈로그 메뉴"; + copy.append(name, desc); + button.appendChild(copy); + button.addEventListener("click", async () => { + try { + await postUtterance(`${recipe.name || recipe.recipe_id} 만들어줘`); + await refreshState(); + } catch (error) { + azasText.textContent = error.message || String(error); + } + }); + catalogList.appendChild(button); + } + } + for (const item of catalogList.querySelectorAll(".catalog-item")) { + item.classList.toggle("selected", item.dataset.recipeId === selectedKey); + } +} + +function renderMenu(state) { + renderCatalog(state); + const confirmedDecision = state.confirmed_decision || {}; + const pipeline = state.pipeline_status || {}; + const shown = selectedDecision(state); + + if (!shown) { + menuCard.hidden = true; + menuEmpty.hidden = false; + renderRobot(-1, pipeline, false); + setBadge("idle", "대기 중"); + return; + } + + const recipeKey = String(shown.recipe_id || ""); + const info = recipeInfo(state, recipeKey); + const amounts = Object.keys(amountsFromDecision(shown)).length + ? amountsFromDecision(shown) + : amountsFromRecipeInfo(info); + if (Object.keys(amounts).length === 0) { + menuCard.hidden = true; + menuEmpty.hidden = false; + renderRobot(-1, pipeline, false); + setBadge("idle", "대기 중"); + return; + } + + menuEmpty.hidden = true; + menuCard.hidden = false; + menuName.textContent = (info && info.name) || RECIPE_NAMES[recipeKey] || "커스텀 칵테일"; + menuDesc.textContent = + (info && info.description) || RECIPE_DESCRIPTIONS[recipeKey] || "주문하신 조합으로 준비할게요."; + renderStats(info); + renderGlass(amounts); + renderChips(amounts); + const activeIndex = renderSteps(pipeline); + renderRobot(activeIndex, pipeline, true); + + const pipelineStatus = pipeline.status || ""; + if (pipelineStatus === "failed") { + setBadge("failed", "제조 실패"); + } else if (pipelineStatus === "completed") { + setBadge("done", "완성! 맛있게 드세요"); + } else if (pipelineStatus === "running" || pipelineStatus === "starting") { + setBadge("making", pipeline.stage ? `제조 중 · ${pipeline.stage}` : "제조 중"); + } else if (pipelineStatus === "dry_run") { + setBadge("making", "리허설 (dry run)"); + } else if (confirmedDecision.confirmed) { + setBadge("confirmed", "주문 확정"); + } else { + setBadge("recommended", "추천 메뉴 · \"응\" 하시면 시작해요"); + } +} let analyser = null; let timeData = null; @@ -179,6 +505,7 @@ async function refreshState() { recipeId.textContent = decision.recipe_id || confirmedDecision.recipe_id || "-"; intent.textContent = decision.intent || "대기"; confirmed.textContent = confirmedDecision.confirmed ? "확정됨" : "대기"; + renderMenu(state); } async function postUtterance(text) { diff --git a/tools/run/azas_cocktail_icon.svg b/tools/run/azas_cocktail_icon.svg new file mode 100644 index 0000000..cbc6024 --- /dev/null +++ b/tools/run/azas_cocktail_icon.svg @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index b3fc4a1..e4f5c93 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -349,7 +349,9 @@ def main() -> int: ) parser.add_argument("--regrasp-rear-entry-offset-x-m", default="-0.090") parser.add_argument("--regrasp-rear-entry-offset-y-m", default="0.0") - parser.add_argument("--final-regrasp-extra-x-offset-m", default="0.020") + parser.add_argument("--final-regrasp-extra-x-offset-m", default="0.000") + parser.add_argument("--skip-initial-move-release", action="store_true", + help="복구 모드: 컵이 이미 첫 디스펜서 front-hold에 놓여 있다고 가정하고 press부터 시작") parser.add_argument("--final-regrasp-extra-y-offset-m", default="0.0") parser.add_argument("--final-regrasp-extra-z-offset-m", default="0.0") parser.add_argument("--final-regrasp-grasp-width-m", default="0.068") @@ -477,6 +479,8 @@ def main() -> int: sequence_extra_args.append( "--skip-release-pre" if args.skip_release_pre else "--no-skip-release-pre" ) + if args.skip_initial_move_release: + sequence_extra_args.append("--skip-initial-move-release") sequence_extra_args.append( "--use-cup-common-pre" if args.use_cup_common_pre else "--no-use-cup-common-pre" ) diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index c4ad2d9..9e19379 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -13,6 +13,7 @@ from __future__ import annotations import argparse +import json import math import shlex import subprocess @@ -41,6 +42,7 @@ ROOT = Path("/home/ssu/Azas") DEFAULT_CONFIG = ROOT / "src" / "azas_bringup" / "config" / "measured_dispenser_collision.yaml" CALIBRATION_CONFIG = ROOT / "src" / "azas_bringup" / "config" / "calibration.yaml" +DEFAULT_RESUME_STATE = ROOT / "outputs" / "measured_dispenser_recipe_resume.json" MOVE_FRONT_HOLD = ROOT / "tools" / "run" / "move_to_measured_dispenser_front_hold.py" PICK_FRONT_HOLD = ROOT / "tools" / "run" / "pick_from_measured_dispenser_front_hold.py" RG2_OPEN = ROOT / "tools" / "run" / "rg2_full_open_verify.sh" @@ -549,6 +551,229 @@ def group_consecutive_dispenser_ids(dispenser_ids: list[str]) -> list[tuple[str, return groups +RESUME_STAGES = ("move_release", "press", "regrasp") +RESUME_STAGE_LABELS = { + "move_release": "move/release", + "press": "press", + "regrasp": "re-grasp/lift", + "cup_holder": "cup-holder place", +} + + +def grouped_resume_payload(groups: list[tuple[str, int]]) -> list[dict[str, object]]: + return [ + {"dispenser_id": dispenser_id, "press_count": int(press_count)} + for dispenser_id, press_count in groups + ] + + +def resume_recipe_token(dispenser_ids: list[str]) -> str: + return ",".join(dispenser_ids) + + +def resume_stage_index(stage: str) -> int: + try: + return RESUME_STAGES.index(stage) + except ValueError as exc: + raise ValueError(f"unknown resume stage: {stage}") from exc + + +class RecipeResumeTracker: + """Durably records the next robot step after each successful stage. + + The checkpoint stores symbolic dispenser IDs and stage names only. It does + not persist or synthesize robot coordinates; all poses still come from the + measured calibration/vision-derived path used by the runner. + """ + + def __init__( + self, + args: argparse.Namespace, + dispenser_ids: list[str], + grouped_dispenser_ids: list[tuple[str, int]], + ) -> None: + self.enabled = bool(args.execute) + self.resume_enabled = bool(args.resume) + self.path = Path(args.resume_state_file) + self.recipe_token = resume_recipe_token(dispenser_ids) + self.groups = grouped_resume_payload(grouped_dispenser_ids) + self.total_groups = len(grouped_dispenser_ids) + self.next_group_index = 1 + self.next_stage = "move_release" + self.loaded = False + + if not self.enabled: + return + if args.clear_resume_state: + self.clear() + if self.resume_enabled: + self._load_if_present() + self._write(status="running") + + def clear(self) -> None: + try: + self.path.unlink() + print(f"[Azas] resume_state cleared: {self.path}") + except FileNotFoundError: + pass + + def _base_payload(self) -> dict[str, object]: + return { + "version": 1, + "runner": Path(__file__).name, + "recipe_token": self.recipe_token, + "groups": self.groups, + "total_groups": self.total_groups, + "next_group_index": self.next_group_index, + "next_stage": self.next_stage, + "updated_at": time.strftime("%Y-%m-%dT%H:%M:%S%z"), + } + + def _write( + self, + *, + status: str, + current_group_index: int | None = None, + current_stage: str | None = None, + dispenser_id: str | None = None, + press_count: int | None = None, + ) -> None: + if not self.enabled: + return + payload = self._base_payload() + payload["status"] = status + if current_group_index is not None: + payload["current_group_index"] = current_group_index + if current_stage is not None: + payload["current_stage"] = current_stage + if dispenser_id is not None: + payload["current_dispenser_id"] = dispenser_id + if press_count is not None: + payload["current_press_count"] = press_count + self.path.parent.mkdir(parents=True, exist_ok=True) + self.path.write_text(json.dumps(payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8") + + def _load_if_present(self) -> None: + if not self.path.is_file(): + return + try: + payload = json.loads(self.path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError) as exc: + raise ValueError( + f"resume state is unreadable: {self.path} ({exc}); " + "pass --clear-resume-state only after confirming the robot/cup state is safe" + ) from exc + if not isinstance(payload, dict): + raise ValueError(f"resume state is invalid: {self.path}") + + status = str(payload.get("status") or "") + stored_recipe = str(payload.get("recipe_token") or "") + stored_groups = payload.get("groups") + if stored_recipe != self.recipe_token or stored_groups != self.groups: + if status == "completed": + print(f"[Azas] resume_state completed for a different recipe; starting fresh: {self.path}") + return + raise ValueError( + "resume state belongs to a different unfinished recipe. " + f"state={self.path} stored_recipe={stored_recipe!r} requested_recipe={self.recipe_token!r}; " + "use the same --dispenser-ids to resume or pass --clear-resume-state after manual safety review" + ) + if status == "completed": + print(f"[Azas] resume_state already completed for this recipe; starting fresh: {self.path}") + return + + try: + next_group_index = int(payload.get("next_group_index", 1)) + except (TypeError, ValueError) as exc: + raise ValueError(f"resume state has invalid next_group_index: {self.path}") from exc + next_stage = str(payload.get("next_stage") or "move_release") + if not 1 <= next_group_index <= self.total_groups + 1: + raise ValueError(f"resume state next_group_index is out of range: {next_group_index}") + if next_stage not in (*RESUME_STAGES, "cup_holder"): + raise ValueError(f"resume state next_stage is invalid: {next_stage!r}") + if next_group_index <= self.total_groups and next_stage == "cup_holder": + raise ValueError("resume state cannot enter cup_holder before all dispenser groups complete") + + self.next_group_index = next_group_index + self.next_stage = next_stage + self.loaded = True + print( + f"[Azas] resume_state loaded: {self.path} " + f"next_group={self.next_group_index}/{self.total_groups} " + f"next_stage={RESUME_STAGE_LABELS.get(self.next_stage, self.next_stage)}" + ) + + def should_run_stage(self, group_index: int, stage: str) -> bool: + if not self.enabled: + return True + if group_index < self.next_group_index: + return False + if group_index > self.next_group_index: + return True + if self.next_stage == "cup_holder": + return False + return resume_stage_index(stage) >= resume_stage_index(self.next_stage) + + def start_stage(self, group_index: int, stage: str, dispenser_id: str, press_count: int) -> None: + if not self.enabled: + return + self.next_group_index = group_index + self.next_stage = stage + self._write( + status="running", + current_group_index=group_index, + current_stage=stage, + dispenser_id=dispenser_id, + press_count=press_count, + ) + print( + f"[Azas] resume_state step_start: group={group_index}/{self.total_groups} " + f"stage={RESUME_STAGE_LABELS[stage]} state={self.path}" + ) + + def complete_stage(self, group_index: int, stage: str) -> None: + if not self.enabled: + return + if stage == "move_release": + self.next_group_index = group_index + self.next_stage = "press" + elif stage == "press": + self.next_group_index = group_index + self.next_stage = "regrasp" + elif stage == "regrasp": + self.next_group_index = group_index + 1 + self.next_stage = "move_release" + else: + raise ValueError(f"cannot complete unknown stage: {stage}") + self._write(status="running") + print( + f"[Azas] resume_state step_done: group={group_index}/{self.total_groups} " + f"stage={RESUME_STAGE_LABELS[stage]} next_group={self.next_group_index} " + f"next_stage={RESUME_STAGE_LABELS.get(self.next_stage, self.next_stage)}" + ) + + def should_run_cup_holder(self) -> bool: + if not self.enabled: + return True + return self.next_group_index >= self.total_groups + 1 + + def start_cup_holder(self) -> None: + if not self.enabled: + return + self.next_group_index = self.total_groups + 1 + self.next_stage = "cup_holder" + self._write(status="running", current_group_index=self.next_group_index, current_stage="cup_holder") + print(f"[Azas] resume_state step_start: final stage=cup-holder place state={self.path}") + + def complete_all(self) -> None: + if not self.enabled: + return + self.next_group_index = self.total_groups + 1 + self.next_stage = "cup_holder" + self._write(status="completed") + print(f"[Azas] resume_state completed: {self.path}") + + class IntegratedRecipeMotion: """Keep ROS service clients alive across release/re-grasp loops. @@ -3432,7 +3657,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--final-regrasp-extra-x-offset-m", type=float, - default=0.020, + default=0.000, help=( "Only for the final re-grasp before cup-holder placement: add this X offset " "to the cup re-grasp target. Positive X moves closer toward the dispenser/cup." @@ -3544,6 +3769,26 @@ def parse_args() -> argparse.Namespace: "and start from press -> re-grasp/lift without repeating the move/release placement." ), ) + parser.add_argument( + "--resume", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "Automatically resume from --resume-state-file when it contains the same unfinished " + "recipe. Use --no-resume to ignore a completed/stale state file." + ), + ) + parser.add_argument( + "--resume-state-file", + type=Path, + default=DEFAULT_RESUME_STATE, + help="Durable JSON checkpoint used to remember the next recipe stage after unexpected stops.", + ) + parser.add_argument( + "--clear-resume-state", + action="store_true", + help="Delete the existing resume checkpoint before starting this run.", + ) parser.add_argument("--execute", action="store_true") parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} when --execute is used") args = parser.parse_args() @@ -3644,6 +3889,16 @@ def main() -> int: f"place_final_y_offset_m={args.cup_holder_place_final_y_offset_m:.3f} " f"place_final_z_offset_m={args.cup_holder_place_final_z_offset_m:.3f}" ) + try: + resume_tracker = RecipeResumeTracker(args, dispenser_ids, grouped_dispenser_ids) + except ValueError as exc: + print(f"[BLOCKED] resume_state: {exc}") + return 2 + if args.execute: + print( + f"[Azas] resume_state_file={resume_tracker.path} " + f"auto_resume={str(args.resume).lower()} loaded={str(resume_tracker.loaded).lower()}" + ) motion: IntegratedRecipeMotion | None = None if args.execute and not args.legacy_subprocess_primitives: @@ -3678,6 +3933,12 @@ def main() -> int: label_prefix = f"recipe group {index}/{total_groups} dispenser {dispenser_id} x{press_count}" final_regrasp = index == total_groups print(f"[Azas] START {label_prefix}: physical_dispenser={dispenser_id}") + move_release_needed = resume_tracker.should_run_stage(index, "move_release") + press_needed = resume_tracker.should_run_stage(index, "press") + regrasp_needed = resume_tracker.should_run_stage(index, "regrasp") + if args.execute and not (move_release_needed or press_needed or regrasp_needed): + print(f"[Azas] SKIP {label_prefix}: completed in resume_state") + continue if args.execute: try: require_dispenser_press_contact_enabled(dispenser_id) @@ -3693,12 +3954,15 @@ def main() -> int: print_dry_run_group_detail(args, dispenser_id, press_count) continue - if args.skip_initial_move_release: + if move_release_needed: + resume_tracker.start_stage(index, "move_release", dispenser_id, press_count) + if move_release_needed and args.skip_initial_move_release: print( f"[Azas] {label_prefix}: skipping initial move/release; " "cup is assumed already released at dispenser front-hold" ) - elif motion is None: + resume_tracker.complete_stage(index, "move_release") + elif move_release_needed and motion is None: print(f"[Azas] {label_prefix}: MOVE/RELEASE physical_dispenser={dispenser_id}") rc = run_command(f"{label_prefix}: move cup to front-hold and release", move_and_release_cmd(args, dispenser_id)) if rc != 0: @@ -3706,15 +3970,19 @@ def main() -> int: rc = run_command(f"{label_prefix}: RG2 full-open release verify", [str(RG2_OPEN)]) if rc != 0: return rc - else: + resume_tracker.complete_stage(index, "move_release") + elif move_release_needed: try: print(f"[Azas] {label_prefix}: MOVE/RELEASE physical_dispenser={dispenser_id}") motion.move_and_release(dispenser_id) except RuntimeError as exc: print(f"[FAIL] {label_prefix}: integrated move/release failed: {exc}") return 1 + resume_tracker.complete_stage(index, "move_release") + else: + print(f"[Azas] SKIP {label_prefix}: move/release completed in resume_state") - if motion is None: + if motion is None and (press_needed or regrasp_needed): rc = run_command( f"{label_prefix}: mark tumbler world object at dispenser", tumbler_scene_cmd( @@ -3725,7 +3993,9 @@ def main() -> int: ) if rc != 0: return rc - if motion is None: + if press_needed: + resume_tracker.start_stage(index, "press", dispenser_id, press_count) + if press_needed and motion is None: print(f"[Azas] {label_prefix}: PRESS physical_dispenser={dispenser_id} count={press_count}") rc = run_command( f"{label_prefix}: press dispenser {press_count} time(s)", @@ -3733,20 +4003,27 @@ def main() -> int: ) if rc != 0: return rc - else: + resume_tracker.complete_stage(index, "press") + elif press_needed: try: print(f"[Azas] {label_prefix}: PRESS physical_dispenser={dispenser_id} count={press_count}") motion.press_dispenser(dispenser_id, press_count) except RuntimeError as exc: print(f"[FAIL] {label_prefix}: integrated press failed: {exc}") return 1 + resume_tracker.complete_stage(index, "press") + else: + print(f"[Azas] SKIP {label_prefix}: press completed in resume_state") - if motion is None: + if regrasp_needed: + resume_tracker.start_stage(index, "regrasp", dispenser_id, press_count) + if regrasp_needed and motion is None: print(f"[Azas] {label_prefix}: RE-GRASP physical_dispenser={dispenser_id}") rc = run_command(f"{label_prefix}: re-grasp cup from front-hold", pick_cmd(args, dispenser_id)) if rc != 0: return rc - else: + resume_tracker.complete_stage(index, "regrasp") + elif regrasp_needed: try: regrasp_label = "FINAL RE-GRASP" if final_regrasp else "RE-GRASP" print(f"[Azas] {label_prefix}: {regrasp_label} physical_dispenser={dispenser_id}") @@ -3763,8 +4040,11 @@ def main() -> int: if rc != 0: print(f"[FAIL] {label_prefix}: fallback re-grasp/lift failed after integrated timeout") return rc + resume_tracker.complete_stage(index, "regrasp") + else: + print(f"[Azas] SKIP {label_prefix}: re-grasp/lift completed in resume_state") - if motion is None: + if motion is None and regrasp_needed: rc = run_command( f"{label_prefix}: remove dispenser world object", tumbler_scene_cmd( @@ -3788,7 +4068,13 @@ def main() -> int: "CUP_HOLDER_PLACE_FINAL -> RG2_OPEN -> CUP_HOLDER_RETREAT" ) elif args.place_cup_holder_after_sequence: - if motion is None: + cup_holder_needed = resume_tracker.should_run_cup_holder() + if not cup_holder_needed: + print("[Azas] SKIP final cup-holder place: completed in resume_state") + resume_tracker.complete_all() + else: + resume_tracker.start_cup_holder() + if cup_holder_needed and motion is None: rc = run_command( "place final cup in holder", [ @@ -3827,13 +4113,17 @@ def main() -> int: ) if rc != 0: return rc - else: + resume_tracker.complete_all() + elif cup_holder_needed: try: print("[Azas] final: PLACE CUP IN HOLDER") motion.place_cup_in_holder() except RuntimeError as exc: print(f"[FAIL] final cup-holder place failed: {exc}") return 1 + resume_tracker.complete_all() + elif args.execute: + resume_tracker.complete_all() finally: if motion is not None: motion.close() diff --git a/tools/run/run_voice_auto_cup_flow.sh b/tools/run/run_voice_auto_cup_flow.sh new file mode 100755 index 0000000..d163002 --- /dev/null +++ b/tools/run/run_voice_auto_cup_flow.sh @@ -0,0 +1,51 @@ +#!/usr/bin/env bash +# 음성 주문(confirmed recipe)을 받아 전체 자동 칵테일 파이프라인을 실행한다: +# 컵 분류/픽 -> 디스펜서 레시피 -> 컵홀더 -> 뚜껑 체결 -> 쉐이킹. +# 2026-06-13 수동 4-터미널 구성으로 검증된 라우터 명령을 그대로 고정한 래퍼. +# 사용: RECIPE_COLORS="yellow:2,blue:1" bash run_voice_auto_cup_flow.sh +# 또는 bash run_voice_auto_cup_flow.sh "yellow:2,blue:1" +set -euo pipefail + +RECIPE_COLORS="${1:-${RECIPE_COLORS:-}}" +if [[ -z "${RECIPE_COLORS}" ]]; then + echo "[voice_flow] RECIPE_COLORS is required (e.g. \"yellow:2,blue:1\")" >&2 + exit 2 +fi +if ! [[ "${RECIPE_COLORS}" =~ ^(red|yellow|green|blue):[0-9]+(,(red|yellow|green|blue):[0-9]+)*$ ]]; then + echo "[voice_flow] invalid RECIPE_COLORS: ${RECIPE_COLORS}" >&2 + exit 2 +fi + +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +ROUTER_CONFIRM="${ROUTER_CONFIRM:-}" +if [[ "${ROUTER_CONFIRM}" != "ENABLE_AUTO_CUP_ROUTER" ]]; then + echo "[voice_flow] BLOCKED: set ROUTER_CONFIRM=ENABLE_AUTO_CUP_ROUTER to run real motion." >&2 + exit 3 +fi + +cd /home/ssu/Azas +set +u +source /opt/ros/humble/setup.bash +[[ -f /home/ssu/ws_moveit/install/setup.bash ]] && source /home/ssu/ws_moveit/install/setup.bash +[[ -f /home/ssu/ros2_ws/install/setup.bash ]] && source /home/ssu/ros2_ws/install/setup.bash +source /home/ssu/Azas/install/setup.bash +set -u + +# 검증된 단일 DDS 구성: 모든 스택 터미널과 동일해야 service discovery가 안정적이다. +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" +export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" + +echo "[voice_flow] starting full auto cup flow: recipe_colors=${RECIPE_COLORS}" +exec ros2 launch azas_bringup auto_cup_flow_router.launch.py \ + enable_real_motion:=true \ + router_confirm:=ENABLE_AUTO_CUP_ROUTER \ + service_prefix:="${SERVICE_PREFIX}" \ + moveit_controller_name:=/${SERVICE_PREFIX}/dsr_moveit_controller \ + controller_action_name:=/${SERVICE_PREFIX}/dsr_moveit_controller/follow_joint_trajectory \ + classifier_path:=/home/ssu/Azas/cup_classifier_best.pth \ + classifier_arch:=resnet18 \ + route_hold_sec:=2.0 \ + route_stable_required_samples:=5 \ + route_stable_min_sec:=0.8 \ + recipe_colors:="${RECIPE_COLORS}" diff --git a/tools/run/start_azas_voice_stack.sh b/tools/run/start_azas_voice_stack.sh new file mode 100755 index 0000000..de7bfc9 --- /dev/null +++ b/tools/run/start_azas_voice_stack.sh @@ -0,0 +1,71 @@ +#!/usr/bin/env bash +# Azas 음성 칵테일 데모 원커맨드 기동: +# bash tools/run/start_azas_voice_stack.sh +# tmux 세션 하나에 로봇/그리퍼/카메라/음성스택을 순서대로 띄우고 브라우저를 연다. +# 이후 사용자는 화면에서 말만 하면 된다 ("달달한 거 한잔 줘" -> "응"). +# +# 2026-06-13 검증 구성 고정: +# - joint_state_relay는 띄우지 않는다 (bringup의 broadcaster가 이미 /joint_states를 +# 퍼블리시하므로, relay까지 켜면 이중 퍼블리시로 MoveIt 실행 검증이 깨져 pick이 실패한다). +# - 모든 창에 동일한 DDS env (ROS_DOMAIN_ID=9, ROS_LOCALHOST_ONLY=1, UDPv4)를 강제한다. +set -euo pipefail + +ROOT="${ROOT:-/home/ssu/Azas}" +SESSION="${SESSION:-azas-voice}" +ROBOT_HOST="${ROBOT_HOST:-192.168.1.100}" +ROBOT_NAME="${ROBOT_NAME:-dsr01}" +RT_HOST="${RT_HOST:-0.0.0.0}" +RG2_IP="${RG2_IP:-192.168.1.1}" +RG2_PORT="${RG2_PORT:-502}" +VOICE_PORT="${VOICE_PORT:-8090}" +# 기본은 실제 로봇 제조까지 켠다. 리허설만 하려면 HW_EXEC=false 로 실행. +HW_EXEC="${HW_EXEC:-true}" +USE_LLM="${USE_LLM:-false}" +OPEN_BROWSER="${OPEN_BROWSER:-true}" + +ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" +FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" + +cd "${ROOT}" +mkdir -p "${ROOT}/log/tmux_logic" /tmp/azas_ros_logs + +if tmux has-session -t "${SESSION}" >/dev/null 2>&1; then + echo "[Azas] existing ${SESSION} session found; stopping it first." + tmux list-panes -s -t "${SESSION}" -F '#{pane_id}' | while read -r pane; do + [[ -n "${pane}" ]] && tmux send-keys -t "${pane}" C-c >/dev/null 2>&1 || true + done + sleep 3 + tmux kill-session -t "${SESSION}" >/dev/null 2>&1 || true +fi + +common_env="export ROS_DOMAIN_ID=${ROS_DOMAIN_ID}; export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}; export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS}; export ROS_LOG_DIR=/tmp/azas_ros_logs" +stamp='$(date +%Y%m%d-%H%M%S)' + +robot_cmd="cd ${ROOT}; ${common_env}; export ROBOT_HOST=${ROBOT_HOST}; export ROBOT_NAME=${ROBOT_NAME}; export RT_HOST=${RT_HOST}; export DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP; bash tools/run/run_doosan_real_m0609.sh 2>&1 | tee ${ROOT}/log/tmux_logic/robot-${stamp}.log" +gripper_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch ${ROOT}/install/azas_gripper/share/azas_gripper/launch/rg2_trigger.launch.py ip:=${RG2_IP} port:=${RG2_PORT} connect:=true open_width:=1100 close_width:=0 force:=300 settle_seconds:=0.6 2>&1 | tee ${ROOT}/log/tmux_logic/gripper-${stamp}.log" +camera_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch realsense2_camera rs_launch.py camera_name:=camera initial_reset:=true reconnect_timeout:=5.0 enable_color:=true enable_depth:=true align_depth.enable:=true rgb_camera.color_profile:=640x480x30 depth_module.depth_profile:=640x480x30 2>&1 | tee ${ROOT}/log/tmux_logic/camera-${stamp}.log" +voice_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch azas_voice azas_voice.launch.py use_pipeline_executor:=true enable_pipeline_hardware_execution:=${HW_EXEC} use_llm:=${USE_LLM} enable_llm:=${USE_LLM} voice_screen_port:=${VOICE_PORT} 2>&1 | tee ${ROOT}/log/tmux_logic/voice-${stamp}.log" + +echo "[Azas] starting robot bringup..." +tmux new-session -d -s "${SESSION}" -n robot "${robot_cmd}" +sleep 10 +echo "[Azas] starting gripper..." +tmux new-window -t "${SESSION}" -n gripper "${gripper_cmd}" +sleep 3 +echo "[Azas] starting camera..." +tmux new-window -t "${SESSION}" -n camera "${camera_cmd}" +sleep 8 +echo "[Azas] starting voice stack (port ${VOICE_PORT}, hardware=${HW_EXEC})..." +tmux new-window -t "${SESSION}" -n voice "${voice_cmd}" +sleep 4 + +echo "" +echo "[Azas] voice cocktail stack is up: tmux session '${SESSION}' (robot/gripper/camera/voice)" +echo "[Azas] panel: http://localhost:${VOICE_PORT} — 말로 주문하고 '응'으로 확정하면 제조가 시작됩니다." +echo "[Azas] logs: tmux attach -t ${SESSION} / stop: bash tools/run/stop_azas_voice_stack.sh" +tmux list-windows -t "${SESSION}" + +if [[ "${OPEN_BROWSER}" == "true" && -n "${DISPLAY:-}" ]] && command -v xdg-open >/dev/null 2>&1; then + xdg-open "http://localhost:${VOICE_PORT}" >/dev/null 2>&1 || true +fi diff --git a/tools/run/stop_azas_voice_stack.sh b/tools/run/stop_azas_voice_stack.sh new file mode 100755 index 0000000..7d6879e --- /dev/null +++ b/tools/run/stop_azas_voice_stack.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +# azas-voice tmux 세션(로봇/그리퍼/카메라/음성스택)을 정리한다. +set -euo pipefail + +SESSION="${SESSION:-azas-voice}" + +if ! tmux has-session -t "${SESSION}" >/dev/null 2>&1; then + echo "[Azas] no '${SESSION}' session running." + exit 0 +fi + +tmux list-panes -s -t "${SESSION}" -F '#{pane_id}' | while read -r pane; do + [[ -n "${pane}" ]] && tmux send-keys -t "${pane}" C-c >/dev/null 2>&1 || true +done + +for _ in {1..25}; do + pgrep -f 'run_doosan_real_m0609|rg2_trigger.launch.py|rs_launch.py camera_name:=camera|azas_voice.launch.py|auto_cup_flow_router' >/dev/null 2>&1 || break + sleep 0.2 +done + +tmux kill-session -t "${SESSION}" >/dev/null 2>&1 || true +echo "[Azas] '${SESSION}' session stopped." From 5f7a7c992b513d3a4272f2fea376648a04452aec Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sat, 13 Jun 2026 19:25:51 +0900 Subject: [PATCH 78/88] =?UTF-8?q?6=EC=B0=A8=20=ED=85=8C=EC=8A=A4=ED=8A=B8?= =?UTF-8?q?=20=EC=99=84=EB=A3=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 35 +- .../azas_task_manager/auto_cup_flow_router.py | 436 +++++++++++++++++- .../auto_flow_resume_state.py | 317 +++++++++++++ .../test/test_auto_flow_resume_state.py | 89 ++++ src/azas_voice/azas_voice/command_parser.py | 112 ++++- .../azas_voice/conversation_manager_node.py | 21 + .../azas_voice/llm_recipe_mapper_node.py | 32 +- src/azas_voice/azas_voice/recipe_catalog.py | 8 + .../voice_pipeline_executor_node.py | 154 ++++++- src/azas_voice/test/test_command_parser.py | 32 ++ .../test_voice_pipeline_recovery_helpers.py | 30 ++ src/azas_voice/web/voice.js | 24 +- .../launch/yolo_cup_pick_node.launch.py | 28 ++ tools/run/pick_from_cup_holder_side_grip.py | 2 +- ...pick_from_measured_dispenser_front_hold.py | 2 +- tools/run/place_side_grip_cup_in_holder.py | 17 + ...lish_color_recipe_sequence_rviz_preview.py | 2 +- tools/run/robot_pipeline_control_server.py | 32 +- tools/run/run_color_recipe_sequence.py | 120 +++-- tools/run/run_color_scan_stage.sh | 56 ++- tools/run/run_holder_pick_then_shake_chain.sh | 89 ++++ tools/run/run_kang_lid_grip_close_direct.sh | 11 +- tools/run/run_lid_close_then_shake_chain.sh | 9 +- .../run_measured_dispenser_recipe_sequence.py | 114 ++++- tools/run/run_rule_based_shake_real.sh | 8 +- tools/run/run_tmux_logic_sequence.sh | 9 +- tools/run/run_voice_auto_cup_flow.sh | 29 +- tools/run/stop_azas_all.sh | 8 +- tools/run/stop_azas_voice_stack.sh | 59 ++- tools/run/wait_for_lid_grip_status.py | 58 ++- 30 files changed, 1809 insertions(+), 134 deletions(-) create mode 100644 src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py create mode 100644 src/azas_task_manager/test/test_auto_flow_resume_state.py create mode 100644 src/azas_voice/test/test_voice_pipeline_recovery_helpers.py create mode 100644 tools/run/run_holder_pick_then_shake_chain.sh diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index 8e36d7f..2faffd3 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -10,6 +10,7 @@ def generate_launch_description(): DeclareLaunchArgument("enable_real_motion", default_value="false"), DeclareLaunchArgument("router_confirm", default_value=""), DeclareLaunchArgument("service_prefix", default_value=""), + DeclareLaunchArgument("motion_service_prefix", default_value="auto"), DeclareLaunchArgument("moveit_controller_name", default_value="/dsr_moveit_controller"), DeclareLaunchArgument("controller_action_name", default_value="/dsr_moveit_controller/follow_joint_trajectory"), DeclareLaunchArgument("yolo_model_path", default_value="/home/ssu/Azas/local_models/best.pt"), @@ -24,16 +25,28 @@ def generate_launch_description(): DeclareLaunchArgument("side_extra_args", default_value=""), DeclareLaunchArgument("cup_uprighting_extra_args", default_value=""), DeclareLaunchArgument("side_target_x_offset_m", default_value="-0.02"), + DeclareLaunchArgument("side_trajectory_execution_duration_scaling", default_value="3.0"), + DeclareLaunchArgument("side_trajectory_execution_goal_margin_sec", default_value="3.0"), DeclareLaunchArgument("color_scan_at_start", default_value="true"), DeclareLaunchArgument("recipe_after_success", default_value="true"), DeclareLaunchArgument("recipe_colors", default_value=""), DeclareLaunchArgument("cup_pre_from_place_x_offset_m", default_value="-0.12"), DeclareLaunchArgument("dispenser_3_cup_pre_extra_x_offset_m", default_value="-0.01"), - DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="-0.02"), - DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.03"), + DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="0.0"), + DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.04"), DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="0.0"), - DeclareLaunchArgument("cup_holder_z_min_m", default_value="0.08"), + DeclareLaunchArgument("cup_holder_rz_offset_deg", default_value="-1.0"), + DeclareLaunchArgument("cup_holder_z_min_m", default_value="0.06"), DeclareLaunchArgument("lid_shake_after_recipe", default_value="true"), + DeclareLaunchArgument("holder_pick_shake_command", default_value=""), + DeclareLaunchArgument("shake_only_command", default_value=""), + DeclareLaunchArgument("resume_mode", default_value="normal"), + DeclareLaunchArgument("resume_state_file", default_value="/home/ssu/Azas/outputs/auto_cup_flow_resume.json"), + DeclareLaunchArgument("resume_events_file", default_value="/home/ssu/Azas/outputs/auto_cup_flow_events.jsonl"), + DeclareLaunchArgument( + "dispenser_resume_state_file", + default_value="/home/ssu/Azas/outputs/measured_dispenser_recipe_resume.json", + ), Node( package="azas_task_manager", executable="auto_cup_flow_router", @@ -43,6 +56,7 @@ def generate_launch_description(): "enable_real_motion": ParameterValue(LaunchConfiguration("enable_real_motion"), value_type=bool), "router_confirm": LaunchConfiguration("router_confirm"), "service_prefix": LaunchConfiguration("service_prefix"), + "motion_service_prefix": LaunchConfiguration("motion_service_prefix"), "moveit_controller_name": LaunchConfiguration("moveit_controller_name"), "controller_action_name": LaunchConfiguration("controller_action_name"), "yolo_model_path": LaunchConfiguration("yolo_model_path"), @@ -57,6 +71,14 @@ def generate_launch_description(): "side_extra_args": LaunchConfiguration("side_extra_args"), "cup_uprighting_extra_args": LaunchConfiguration("cup_uprighting_extra_args"), "side_target_x_offset_m": ParameterValue(LaunchConfiguration("side_target_x_offset_m"), value_type=float), + "side_trajectory_execution_duration_scaling": ParameterValue( + LaunchConfiguration("side_trajectory_execution_duration_scaling"), + value_type=float, + ), + "side_trajectory_execution_goal_margin_sec": ParameterValue( + LaunchConfiguration("side_trajectory_execution_goal_margin_sec"), + value_type=float, + ), "color_scan_at_start": ParameterValue(LaunchConfiguration("color_scan_at_start"), value_type=bool), "recipe_after_success": ParameterValue(LaunchConfiguration("recipe_after_success"), value_type=bool), "recipe_colors": LaunchConfiguration("recipe_colors"), @@ -65,8 +87,15 @@ def generate_launch_description(): "final_regrasp_z_offset_m": ParameterValue(LaunchConfiguration("final_regrasp_z_offset_m"), value_type=float), "cup_holder_place_z_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_z_offset_m"), value_type=float), "cup_holder_place_y_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_y_offset_m"), value_type=float), + "cup_holder_rz_offset_deg": ParameterValue(LaunchConfiguration("cup_holder_rz_offset_deg"), value_type=float), "cup_holder_z_min_m": ParameterValue(LaunchConfiguration("cup_holder_z_min_m"), value_type=float), "lid_shake_after_recipe": ParameterValue(LaunchConfiguration("lid_shake_after_recipe"), value_type=bool), + "holder_pick_shake_command": LaunchConfiguration("holder_pick_shake_command"), + "shake_only_command": LaunchConfiguration("shake_only_command"), + "resume_mode": LaunchConfiguration("resume_mode"), + "resume_state_file": LaunchConfiguration("resume_state_file"), + "resume_events_file": LaunchConfiguration("resume_events_file"), + "dispenser_resume_state_file": LaunchConfiguration("dispenser_resume_state_file"), }], ), ]) diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 1c54cc2..b942dff 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -1,5 +1,6 @@ from __future__ import annotations +import ast import os import re import shlex @@ -20,6 +21,15 @@ from sensor_msgs.msg import Image from std_srvs.srv import Trigger +from azas_task_manager.auto_flow_resume_state import ( + DEFAULT_EVENTS_LOG, + DEFAULT_RESUME_STATE, + AutoFlowResumeStore, +) + + +DEFAULT_DISPENSER_RESUME_STATE = "/home/ssu/Azas/outputs/measured_dispenser_recipe_resume.json" + @dataclass(frozen=True) class RouteDecision: @@ -51,6 +61,7 @@ def __init__(self) -> None: self.declare_parameter("observe_time", 0.0) self.declare_parameter("motion_timeout_sec", 25.0) self.declare_parameter("service_prefix", "") + self.declare_parameter("motion_service_prefix", "auto") self.declare_parameter("gripper_open_service", "/jarvis/rg2/open") self.declare_parameter("detection_topic", "/azas/cup_detection") @@ -75,6 +86,8 @@ def __init__(self) -> None: self.declare_parameter("cup_uprighting_extra_args", "") # 사이드 그립에서 base x가 +20mm 정도 어긋나는 실측 보정값 self.declare_parameter("side_target_x_offset_m", -0.02) + self.declare_parameter("side_trajectory_execution_duration_scaling", 3.0) + self.declare_parameter("side_trajectory_execution_goal_margin_sec", 3.0) self.declare_parameter("color_scan_at_start", True) self.declare_parameter( @@ -99,19 +112,26 @@ def __init__(self) -> None: # 키오스크/음성 주문(latest_recipe.json) 없이 색을 직접 내릴 때: 예) "red:2,blue:1" self.declare_parameter("recipe_colors", "") # 디스펜서 누르기 종료 후 디스펜서 앞의 컵을 마지막으로 재파지할 때 z 실측 보정값 - self.declare_parameter("final_regrasp_z_offset_m", -0.02) + self.declare_parameter("final_regrasp_z_offset_m", 0.0) # 잡기 직전 pre 위치(cup_place 기준 X offset) 보정값. 스크립트 기본 -0.09에 -30mm 추가 self.declare_parameter("cup_pre_from_place_x_offset_m", -0.12) self.declare_parameter("dispenser_3_cup_pre_extra_x_offset_m", -0.01) # 컵홀더에 놓을 때 보정값과 place 목표 z 안전 하한 (필요 시 조정) - self.declare_parameter("cup_holder_place_z_offset_m", -0.03) + self.declare_parameter("cup_holder_place_z_offset_m", -0.04) self.declare_parameter("cup_holder_place_y_offset_m", 0.0) - self.declare_parameter("cup_holder_z_min_m", 0.08) + self.declare_parameter("cup_holder_rz_offset_deg", -1.0) + self.declare_parameter("cup_holder_z_min_m", 0.06) self.declare_parameter("lid_shake_after_recipe", True) self.declare_parameter( "lid_shake_command", "bash /home/ssu/Azas/tools/run/run_lid_close_then_shake_chain.sh", ) + self.declare_parameter("holder_pick_shake_command", "") + self.declare_parameter("shake_only_command", "") + self.declare_parameter("resume_mode", "normal") + self.declare_parameter("resume_state_file", str(DEFAULT_RESUME_STATE)) + self.declare_parameter("resume_events_file", str(DEFAULT_EVENTS_LOG)) + self.declare_parameter("dispenser_resume_state_file", DEFAULT_DISPENSER_RESUME_STATE) self._latest_detection: Optional[CupDetection] = None self._latest_image: Optional[np.ndarray] = None @@ -119,6 +139,8 @@ def __init__(self) -> None: self._window_enabled = bool(self.get_parameter("show_classification_window").value) self._children: list[subprocess.Popen[str]] = [] self._child_node_failures: dict[str, list[str]] = {} + self._stage_failure_reasons: dict[str, str] = {} + self._resume_store: AutoFlowResumeStore | None = None self.create_subscription( CupDetection, @@ -140,31 +162,41 @@ def run(self) -> int: self.get_logger().info("auto cup router: color scan -> observe -> open -> classify -> route") perception = None try: - if not self._run_color_scan_sequence(): + if not self._prepare_resume_store(): + return 2 + if not self._run_resumable_stage( + "color_scan", + self._run_color_scan_sequence, + verified={"color_map": True}, + ): return 1 - if not self._move_observe("initial observe"): + if not self._run_resumable_stage("observe", lambda: self._move_observe("initial observe")): return 1 - if not self._open_gripper("initial gripper full-open"): - return 1 - - perception = self._start_perception() - decision = self._wait_for_route_decision() - if decision is None: - self.get_logger().error("route decision failed: no stable upright/lying classification") + if not self._run_resumable_stage("open_gripper", lambda: self._open_gripper("initial gripper full-open")): return 1 - self._stop_process(perception, "perception") - perception = None - - if decision.route == "side_grasp": - success = self._run_side_grasp(decision) - else: - success = self._run_cup_uprighting(decision) - if not success: + if not self._run_resumable_stage( + "cup_pick", + self._run_cup_pick_stage, + verified={"cup_picked": True}, + held_objects={"cup": "gripper", "lid": "unknown"}, + ): return 1 - if not self._run_recipe_sequence(): + if not self._run_resumable_stage( + "recipe", + self._run_recipe_sequence, + verified={"dispenser_sequence_done": True, "cup_in_holder": True}, + held_objects={"cup": "in_holder", "lid": "unknown"}, + ): return 1 - if not self._run_lid_shake_sequence(): + if not self._run_resumable_stage( + "lid_shake", + self._run_lid_shake_sequence, + verified={"lid_closed": True, "shake_done": True}, + held_objects={"cup": "gripper", "lid": "on_cup"}, + ): return 1 + if self._resume_store is not None: + self._resume_store.complete_run() self.get_logger().info("auto cup router: selected flow completed; router exiting") return 0 finally: @@ -182,6 +214,69 @@ def _confirmed(self) -> bool: return False return True + def _prepare_resume_store(self) -> bool: + mode = str(self.get_parameter("resume_mode").value or "normal").strip() + colors = str(self.get_parameter("recipe_colors").value or "").strip() + self._resume_store = AutoFlowResumeStore( + state_path=str(self.get_parameter("resume_state_file").value), + events_path=str(self.get_parameter("resume_events_file").value), + mode=mode, + recipe_colors=colors, + ) + if not self._resume_store.prepare(): + self.get_logger().error("resume store blocked this run") + return False + self.get_logger().info( + f"auto_flow_resume: mode={mode} state={self._resume_store.state_path} " + f"next_stage={self._resume_store.next_stage()}" + ) + return True + + def _run_resumable_stage( + self, + stage: str, + action, + *, + verified: dict[str, bool] | None = None, + held_objects: dict[str, str] | None = None, + ) -> bool: + if self._resume_store is not None and self._resume_store.should_skip(stage): + self.get_logger().info(f"resume_state skip completed stage: {stage}") + return True + if self._resume_store is not None: + self._resume_store.start_stage(stage) + try: + ok = bool(action()) + except Exception as exc: + self.get_logger().exception(f"{stage}: unexpected exception") + if self._resume_store is not None: + self._resume_store.fail_stage(stage, f"{stage}_exception:{exc}", auto_recoverable=True) + return False + if ok: + if self._resume_store is not None: + self._resume_store.complete_stage(stage, verified=verified, held_objects=held_objects) + return True + if self._resume_store is not None: + reason = self._stage_failure_reasons.pop(stage, f"{stage}_failed") + self._resume_store.fail_stage(stage, reason, auto_recoverable=True) + return False + + def _run_cup_pick_stage(self) -> bool: + perception = None + try: + perception = self._start_perception() + decision = self._wait_for_route_decision() + if decision is None: + self.get_logger().error("route decision failed: no stable upright/lying classification") + return False + self._stop_process(perception, "perception") + perception = None + if decision.route == "side_grasp": + return self._run_side_grasp(decision) + return self._run_cup_uprighting(decision) + finally: + self._stop_process(perception, "perception") + def _on_detection(self, msg: CupDetection) -> None: self._latest_detection = msg @@ -411,6 +506,7 @@ def _compact_status(status: str) -> str: def _run_side_grasp(self, decision: RouteDecision) -> bool: self.get_logger().info(f"route=side_grasp: launching existing side grasp flow ({decision.status})") + helpers = self._start_side_grasp_support_processes() cmd = self._launch_command(str(self.get_parameter("side_launch").value)) cmd.extend([ "auto_pick:=true", @@ -440,6 +536,7 @@ def _run_side_grasp(self, decision: RouteDecision) -> bool: "side_low_retry_attempts:=5", "workspace_xy_clamp_enabled:=false", "table_collision_enabled:=true", + "workspace_collision_scene_enabled:=false", "table_surface_z:=0.0", "table_thickness:=0.04", "table_size_x:=1.10", @@ -447,13 +544,88 @@ def _run_side_grasp(self, decision: RouteDecision) -> bool: "table_center_x:=0.29", "table_center_y:=0.0", "dispenser_collision_enabled:=true", + "trajectory_execution_allowed_duration_scaling:=" + f"{float(self.get_parameter('side_trajectory_execution_duration_scaling').value)}", + "trajectory_execution_allowed_goal_duration_margin:=" + f"{float(self.get_parameter('side_trajectory_execution_goal_margin_sec').value)}", f"moveit_controller_name:={self.get_parameter('moveit_controller_name').value}", f"side_target_x_offset_m:={float(self.get_parameter('side_target_x_offset_m').value)}", "start_joint_state_relay:=false", f"model_path:={self.get_parameter('yolo_model_path').value}", ]) cmd.extend(self._split_extra_args(str(self.get_parameter("side_extra_args").value))) - return self._run_process(cmd, "side_grasp") + try: + return self._run_process(cmd, "side_grasp") + finally: + for proc, label in helpers: + self._stop_process(proc, label) + + def _start_side_grasp_support_processes(self) -> list[tuple[subprocess.Popen[str], str]]: + prefix = str(self.get_parameter("service_prefix").value or "dsr01").strip().strip("/") or "dsr01" + helpers: list[tuple[subprocess.Popen[str], str]] = [] + helper_cmds = [ + ( + [ + "ros2", + "run", + "tf2_ros", + "static_transform_publisher", + "--x", + "0", + "--y", + "0", + "--z", + "0", + "--yaw", + "0", + "--pitch", + "0", + "--roll", + "0", + "--frame-id", + "world", + "--child-frame-id", + "base_link", + ], + "world_base_tf", + ), + ( + [ + "ros2", + "run", + "azas_perception", + "hand_eye_static_tf_node", + "--ros-args", + "-p", + "compose_timeout_sec:=30.0", + "-p", + "allow_direct_fallback:=false", + ], + "hand_eye_static_tf", + ), + ( + [ + sys.executable, + "/home/ssu/Azas/src/dsr_practice/dsr_practice/joint_state_relay.py", + "--ros-args", + "-r", + "__node:=azas_auto_cup_joint_state_relay", + "-p", + f"input_topic:=/{prefix}/joint_states", + "-p", + "output_topic:=/joint_states", + ], + "joint_state_relay", + ), + ] + for cmd, label in helper_cmds: + try: + helpers.append((self._popen(cmd, label), label)) + except OSError as exc: + self.get_logger().warn(f"{label}: failed to start support process: {exc}") + self._stage_failure_reasons.setdefault("cup_pick", f"{label}_start_failed") + time.sleep(1.0) + return helpers def _run_cup_uprighting(self, decision: RouteDecision) -> bool: self.get_logger().info(f"route=cup_uprighting: launching optimized cup-uprighting flow ({decision.status})") @@ -477,6 +649,8 @@ def _run_color_scan_sequence(self) -> bool: if not command: self.get_logger().warning("color_scan_command is empty; skipping dispenser color scan") return True + service_prefix = self._motion_service_prefix() + command = f"SERVICE_PREFIX={shlex.quote(service_prefix or '/')} {command}" self.get_logger().info("moving to color_scan_pose and scanning dispensers before cup pick") return self._run_process(["bash", "-c", command], "color_scan") @@ -492,6 +666,18 @@ def _run_recipe_sequence(self) -> bool: if colors: command += f" --colors {shlex.quote(colors)}" self.get_logger().info(f"recipe colors given directly: {colors}") + resume_mode = str(self.get_parameter("resume_mode").value or "normal").strip() + dispenser_resume_state = str(self.get_parameter("dispenser_resume_state_file").value).strip() + if dispenser_resume_state: + command += f" --resume-state-file {shlex.quote(dispenser_resume_state)}" + if resume_mode == "resume": + command += " --resume" + self.get_logger().info("recipe resume_state enabled by explicit resume_mode=resume") + else: + command += " --no-resume --clear-resume-state" + self.get_logger().info("recipe resume_state cleared for fresh dispenser placement") + service_prefix = self._motion_service_prefix() + command += f" --service-prefix {shlex.quote(service_prefix)}" cup_pre_x = float(self.get_parameter("cup_pre_from_place_x_offset_m").value) command += f" --cup-pre-from-place-x-offset-m {cup_pre_x}" dispenser_3_pre_x = float(self.get_parameter("dispenser_3_cup_pre_extra_x_offset_m").value) @@ -499,11 +685,13 @@ def _run_recipe_sequence(self) -> bool: regrasp_z = float(self.get_parameter("final_regrasp_z_offset_m").value) place_z = float(self.get_parameter("cup_holder_place_z_offset_m").value) place_y = float(self.get_parameter("cup_holder_place_y_offset_m").value) + place_rz = float(self.get_parameter("cup_holder_rz_offset_deg").value) z_min = float(self.get_parameter("cup_holder_z_min_m").value) command += ( f" --final-regrasp-extra-z-offset-m {regrasp_z}" f" --cup-holder-place-final-z-offset-m {place_z}" f" --cup-holder-place-final-y-offset-m {place_y}" + f" --cup-holder-rz-offset-deg {place_rz}" f" --cup-holder-z-min-m {z_min}" ) self.get_logger().info("pick flow succeeded; starting integrated dispenser recipe sequence") @@ -513,15 +701,47 @@ def _run_lid_shake_sequence(self) -> bool: if not bool(self.get_parameter("lid_shake_after_recipe").value): self.get_logger().info("lid_shake_after_recipe=false; skipping lid close / shake chain") return True - command = str(self.get_parameter("lid_shake_command").value).strip() + command = self._lid_shake_command_for_current_resume_state() if not command: self.get_logger().warning("lid_shake_command is empty; skipping lid close / shake chain") return True self.get_logger().info("recipe succeeded; starting lid close -> holder re-pick -> shake chain") return self._run_process(["bash", "-c", command], "lid_shake") + def _lid_shake_command_for_current_resume_state(self) -> str: + if self._resume_store is None: + return str(self.get_parameter("lid_shake_command").value).strip() + snapshot = self._resume_store.snapshot + verified = snapshot.get("verified") if isinstance(snapshot.get("verified"), dict) else {} + held = snapshot.get("held_objects") if isinstance(snapshot.get("held_objects"), dict) else {} + if bool(verified.get("shake_done")): + self.get_logger().info("resume_state: lid/shake already verified done") + return "true" + if bool(verified.get("lid_closed")): + skip_holder_pick = held.get("cup") == "gripper_for_shake" + if skip_holder_pick: + self.get_logger().info("resume_state: resuming shake with cup already grasped") + else: + self.get_logger().info("resume_state: lid already closed; resuming cup-holder pick then shake") + return self._holder_pick_shake_command(skip_holder_pick=skip_holder_pick) + return str(self.get_parameter("lid_shake_command").value).strip() + + def _holder_pick_shake_command(self, *, skip_holder_pick: bool) -> str: + param_name = "shake_only_command" if skip_holder_pick else "holder_pick_shake_command" + configured = str(self.get_parameter(param_name).value or "").strip() + if configured: + return configured + prefix = self._motion_service_prefix() + env_prefix = prefix if prefix else "/" + skip_value = "true" if skip_holder_pick else "false" + return ( + f"SERVICE_PREFIX={shlex.quote(env_prefix)} " + f"SKIP_CUP_HOLDER_PICK={skip_value} " + "bash /home/ssu/Azas/tools/run/run_holder_pick_then_shake_chain.sh" + ) + def _move_observe(self, label: str) -> bool: - prefix = str(self.get_parameter("service_prefix").value).strip().strip("/") + prefix = self._motion_service_prefix() base = f"/{prefix}/motion" if prefix else "/motion" service = f"{base}/move_joint" wait_service = f"{base}/move_wait" @@ -554,6 +774,22 @@ def _move_observe(self, label: str) -> bool: self.get_logger().info(f"{label}: MoveWait completed") return True + def _motion_service_prefix(self) -> str: + configured_raw = str(self.get_parameter("motion_service_prefix").value or "").strip() + configured = configured_raw.strip("/") + if configured_raw and configured_raw.lower() != "auto": + return configured + + service_prefix = str(self.get_parameter("service_prefix").value or "").strip().strip("/") + services = {name for name, _types in self.get_service_names_and_types()} + if "/motion/move_joint" in services: + return "" + if service_prefix and f"/{service_prefix}/motion/move_joint" in services: + return service_prefix + if "/dsr01/motion/move_joint" in services: + return "dsr01" + return service_prefix + def _open_gripper(self, label: str) -> bool: service = str(self.get_parameter("gripper_open_service").value) client = self.create_client(Trigger, service) @@ -645,6 +881,153 @@ def _run_process(self, cmd: list[str], label: str) -> bool: self.get_logger().error(f"{label}: process exited with code {code}") return False + def _record_child_progress_from_output(self, label: str, text: str) -> None: + if self._resume_store is None: + return + if label == "side_grasp": + self._record_side_grasp_progress(text) + if label == "lid_shake": + self._record_lid_shake_progress(text) + + def _record_side_grasp_progress(self, text: str) -> None: + if "Could not find a connection between 'world' and 'camera_" in text: + self._stage_failure_reasons["cup_pick"] = "side_grasp_tf_tree_disconnected" + self._resume_store.update_progress("cup_pick", "side_grasp_tf_missing") + elif "Didn't receive robot state" in text: + self._stage_failure_reasons["cup_pick"] = "side_grasp_joint_state_stale" + self._resume_store.update_progress("cup_pick", "side_grasp_joint_state_stale") + elif "Unable to configure planning scene monitor" in text: + self._stage_failure_reasons["cup_pick"] = "side_grasp_moveit_planning_scene_monitor_failed" + self._resume_store.update_progress("cup_pick", "side_grasp_moveit_blocked") + + def _record_lid_shake_progress(self, text: str) -> None: + if self._resume_store is None: + return + + payload = self._lid_status_payload(text) + if payload is not None: + self._record_lid_status_payload(payload) + return + + if "ArUco lid_grip_close 성공 status 확인" in text: + self._resume_store.update_progress( + "lid_shake", + "lid_closed", + verified={"lid_grasped": True, "lid_closed": True}, + held_objects={"cup": "in_holder", "lid": "on_cup"}, + ) + elif "Cup-holder pick is required before shake" in text: + self._resume_store.update_progress("lid_shake", "cup_holder_pick_for_shake") + elif ( + "Cup-holder pick completed; continuing to shake" in text + or "[PASS] cup holder side-grip pick sequence completed" in text + ): + self._resume_store.update_progress( + "lid_shake", + "cup_holder_pick_done", + held_objects={"cup": "gripper_for_shake", "lid": "on_cup"}, + ) + elif "Cup-holder pick skipped" in text: + self._resume_store.update_progress( + "lid_shake", + "shake_start", + held_objects={"cup": "gripper_for_shake", "lid": "on_cup"}, + ) + elif "shake sequence finished without failure markers" in text or "SHAKE DONE" in text: + self._resume_store.update_progress( + "lid_shake", + "shake_done", + verified={"shake_done": True}, + held_objects={"cup": "gripper", "lid": "on_cup"}, + ) + elif "Refusing real robot shake" in text: + self._stage_failure_reasons["lid_shake"] = "lid_shake_hardware_blocked" + self._resume_store.update_progress("lid_shake", "hardware_blocked") + + @staticmethod + def _lid_status_payload(text: str) -> dict[str, object] | None: + match = re.search(r"lid_grip_status=[^\s]+\s+payload=(\{.*\})", text) + if match is None: + return None + try: + payload = ast.literal_eval(match.group(1)) + except (SyntaxError, ValueError): + return None + return payload if isinstance(payload, dict) else None + + def _record_lid_status_payload(self, payload: dict[str, object]) -> None: + if self._resume_store is None: + return + status = str(payload.get("status") or "").strip() + step = str(payload.get("step") or "").strip() + if not status: + return + if status == "failed": + reason = str(payload.get("error") or "lid_grip_failed") + self._stage_failure_reasons["lid_shake"] = reason + self._resume_store.fail_stage("lid_shake", reason, auto_recoverable=True) + return + if status == "trigger_received": + self._resume_store.update_progress("lid_shake", "lid_trigger_received") + elif status == "planned": + self._resume_store.update_progress("lid_shake", "lid_pick_planned") + elif status == "gripper_preopen_requested": + self._resume_store.update_progress("lid_shake", "lid_gripper_open") + elif status == "gripper_grasp_requested": + self._resume_store.update_progress( + "lid_shake", + "lid_gripper_grasp_requested", + held_objects={"lid": "gripper_request"}, + ) + elif status == "gripper_result": + command = str(payload.get("command") or "") + success = bool(payload.get("success")) + if command == "grasp" and success: + self._resume_store.update_progress( + "lid_shake", + "lid_grasped", + verified={"lid_grasped": True}, + held_objects={"lid": "gripper"}, + ) + elif command == "preopen": + self._resume_store.update_progress("lid_shake", "lid_gripper_open") + elif status == "motion_target_reached": + self._resume_store.update_progress( + "lid_shake", + self._lid_motion_step_name(step), + verified={"lid_grasped": step == "lift_lid"} if step == "lift_lid" else None, + held_objects={"lid": "gripper"} if step == "lift_lid" else None, + ) + elif status == "motion_sequence_requested": + self._resume_store.update_progress( + "lid_shake", + "lid_closed", + verified={"lid_grasped": True, "lid_closed": True}, + held_objects={"cup": "in_holder", "lid": "on_cup"}, + ) + elif status.startswith("lid_twist"): + self._resume_store.update_progress("lid_shake", self._lid_motion_step_name(step or status)) + + @staticmethod + def _lid_motion_step_name(step: str) -> str: + if step == "approach_lid": + return "lid_approach_reached" + if step == "grasp_lid": + return "lid_grasp_pose_reached" + if step == "lift_lid": + return "lid_lifted" + if step.startswith("lid_twist_transfer"): + return "lid_transfer_to_cup" + if step.startswith("lid_twist_press"): + return "lid_pressed_on_cup" + if step.startswith("lid_twist_preseat"): + return "lid_preseat" + if step.startswith("lid_twist_turn"): + return "lid_twisting_on_cup" + if step.startswith("lid_twist_release") or step.startswith("lid_twist_home"): + return "lid_twist_released" + return step or "lid_progress" + _NODE_DIED_PATTERN = re.compile(r"\[ERROR\] \[(?P[^\]]+)\]: process has died.*exit code (?P-?\d+)") _SHUTDOWN_PATTERN = re.compile(r"sending signal 'SIG(INT|TERM)'|user interrupted with ctrl-c") @@ -661,6 +1044,9 @@ def _forward_output(self, proc: subprocess.Popen[str], label: str) -> None: text = line.rstrip() self.get_logger().info(f"{label}> {text}") log_file.write(text + "\n") + if self._resume_store is not None: + self._resume_store.heartbeat(process_label=label) + self._record_child_progress_from_output(label, text) if not shutting_down and self._SHUTDOWN_PATTERN.search(text): shutting_down = True match = self._NODE_DIED_PATTERN.search(text) diff --git a/src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py b/src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py new file mode 100644 index 0000000..d565f6a --- /dev/null +++ b/src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py @@ -0,0 +1,317 @@ +from __future__ import annotations + +import json +import time +import uuid +from pathlib import Path +from threading import Lock +from typing import Any + + +ROOT = Path("/home/ssu/Azas") +DEFAULT_RESUME_STATE = ROOT / "outputs" / "auto_cup_flow_resume.json" +DEFAULT_EVENTS_LOG = ROOT / "outputs" / "auto_cup_flow_events.jsonl" + +FLOW_STAGES = ( + "color_scan", + "observe", + "open_gripper", + "cup_pick", + "recipe", + "lid_shake", +) + +STAGE_LABELS = { + "color_scan": "dispenser color scan", + "observe": "cup observe pose", + "open_gripper": "initial gripper open", + "cup_pick": "cup route and pick", + "recipe": "measured dispenser recipe", + "lid_shake": "lid close and shake", +} + + +def now_stamp() -> str: + return time.strftime("%Y-%m-%dT%H:%M:%S%z") + + +def load_resume_snapshot(path: str | Path = DEFAULT_RESUME_STATE) -> dict[str, Any] | None: + state_path = Path(path) + if not state_path.is_file(): + return None + try: + payload = json.loads(state_path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError): + return None + return payload if isinstance(payload, dict) else None + + +def safe_recipe_colors_from_snapshot(snapshot: dict[str, Any] | None) -> str: + if not isinstance(snapshot, dict): + return "" + recipe = snapshot.get("recipe") + if not isinstance(recipe, dict): + return "" + colors = str(recipe.get("recipe_colors") or "").strip() + return colors + + +class AutoFlowResumeStore: + """Durable stage journal for the top-level cocktail flow. + + The store records symbolic stage progress and verified facts only. It does + not store cup/lid coordinates or synthesize robot poses. + """ + + def __init__( + self, + *, + state_path: str | Path = DEFAULT_RESUME_STATE, + events_path: str | Path = DEFAULT_EVENTS_LOG, + mode: str = "normal", + recipe_colors: str = "", + recipe_id: str = "", + ) -> None: + self.state_path = Path(state_path) + self.events_path = Path(events_path) + self.mode = mode if mode in {"normal", "resume", "restart"} else "normal" + self.recipe_colors = recipe_colors + self.recipe_id = recipe_id + self._lock = Lock() + self._last_heartbeat_write = 0.0 + self.snapshot: dict[str, Any] = {} + + def prepare(self) -> bool: + self.state_path.parent.mkdir(parents=True, exist_ok=True) + self.events_path.parent.mkdir(parents=True, exist_ok=True) + previous = load_resume_snapshot(self.state_path) + if self.mode == "restart": + self.clear() + previous = None + if self.mode == "resume": + if not previous: + self.block( + "no_resume_state", + "저장된 복구 상태가 없습니다. 새 주문을 먼저 시작하세요.", + auto_recoverable=False, + ) + return False + previous_colors = safe_recipe_colors_from_snapshot(previous) + if self.recipe_colors and previous_colors and self.recipe_colors != previous_colors: + self.block( + "resume_recipe_mismatch", + "저장된 주문과 요청한 주문이 다릅니다. 처음부터 다시 시작해야 합니다.", + auto_recoverable=False, + ) + return False + self.recipe_colors = self.recipe_colors or previous_colors + self.recipe_id = self.recipe_id or str((previous.get("recipe") or {}).get("recipe_id") or "") + self.snapshot = previous + self.snapshot["status"] = "running" + self.snapshot["resume_mode"] = "resume" + self.snapshot["heartbeat_at"] = now_stamp() + self.snapshot["updated_at"] = now_stamp() + self._write_snapshot() + self._append_event("resume_loaded", {"next_stage": self.next_stage()}) + return True + + self.snapshot = self._new_snapshot(status="running") + self._write_snapshot() + self._append_event("run_started", {"mode": self.mode}) + return True + + def clear(self) -> None: + try: + self.state_path.unlink() + except FileNotFoundError: + pass + + def _new_snapshot(self, *, status: str) -> dict[str, Any]: + return { + "version": 1, + "run_id": uuid.uuid4().hex, + "status": status, + "resume_mode": self.mode, + "stage": None, + "step": None, + "next_stage": FLOW_STAGES[0], + "completed_stages": [], + "recipe": { + "recipe_id": self.recipe_id, + "recipe_colors": self.recipe_colors, + }, + "held_objects": { + "cup": "unknown", + "lid": "unknown", + }, + "verified": { + "color_map": False, + "cup_picked": False, + "dispenser_sequence_done": False, + "cup_in_holder": False, + "lid_grasped": False, + "lid_closed": False, + "shake_done": False, + }, + "stop_reason": None, + "blocker": None, + "auto_recoverable": True, + "required_user_action": None, + "created_at": now_stamp(), + "updated_at": now_stamp(), + "heartbeat_at": now_stamp(), + } + + def _write_snapshot(self) -> None: + with self._lock: + self.state_path.parent.mkdir(parents=True, exist_ok=True) + self.state_path.write_text( + json.dumps(self.snapshot, ensure_ascii=False, indent=2) + "\n", + encoding="utf-8", + ) + + def _append_event(self, event: str, fields: dict[str, Any] | None = None) -> None: + payload = { + "event": event, + "run_id": self.snapshot.get("run_id"), + "stage": self.snapshot.get("stage"), + "status": self.snapshot.get("status"), + "created_at": now_stamp(), + } + if fields: + payload.update(fields) + with self._lock: + self.events_path.parent.mkdir(parents=True, exist_ok=True) + with self.events_path.open("a", encoding="utf-8") as handle: + handle.write(json.dumps(payload, ensure_ascii=False) + "\n") + + def next_stage(self) -> str: + completed = set(self.snapshot.get("completed_stages") or []) + for stage in FLOW_STAGES: + if stage not in completed: + return stage + return "complete" + + def should_skip(self, stage: str) -> bool: + return self.mode == "resume" and stage in set(self.snapshot.get("completed_stages") or []) + + def start_stage(self, stage: str, *, step: str | None = None) -> None: + self.snapshot["status"] = "running" + self.snapshot["stage"] = stage + self.snapshot["step"] = step or stage + self.snapshot["next_stage"] = stage + self.snapshot["stop_reason"] = None + self.snapshot["blocker"] = None + self.snapshot["required_user_action"] = None + self.snapshot["auto_recoverable"] = True + self.snapshot["updated_at"] = now_stamp() + self.snapshot["heartbeat_at"] = now_stamp() + self._write_snapshot() + self._append_event("stage_started", {"stage_label": STAGE_LABELS.get(stage, stage)}) + + def complete_stage( + self, + stage: str, + *, + verified: dict[str, bool] | None = None, + held_objects: dict[str, str] | None = None, + ) -> None: + completed = list(self.snapshot.get("completed_stages") or []) + if stage not in completed: + completed.append(stage) + self.snapshot["completed_stages"] = completed + if verified: + current_verified = dict(self.snapshot.get("verified") or {}) + current_verified.update(verified) + self.snapshot["verified"] = current_verified + if held_objects: + current_held = dict(self.snapshot.get("held_objects") or {}) + current_held.update(held_objects) + self.snapshot["held_objects"] = current_held + self.snapshot["status"] = "running" + self.snapshot["stage"] = stage + self.snapshot["step"] = f"{stage}_done" + self.snapshot["next_stage"] = self.next_stage() + self.snapshot["updated_at"] = now_stamp() + self.snapshot["heartbeat_at"] = now_stamp() + self._write_snapshot() + self._append_event("stage_completed", {"next_stage": self.snapshot["next_stage"]}) + + def heartbeat(self, *, process_label: str | None = None) -> None: + now = time.monotonic() + if now - self._last_heartbeat_write < 2.0: + return + self._last_heartbeat_write = now + self.snapshot["heartbeat_at"] = now_stamp() + self.snapshot["updated_at"] = now_stamp() + if process_label: + self.snapshot["last_process_label"] = process_label + self._write_snapshot() + + def update_progress( + self, + stage: str, + step: str, + *, + verified: dict[str, bool] | None = None, + held_objects: dict[str, str] | None = None, + ) -> None: + if self.snapshot.get("stage") == stage and self.snapshot.get("step") == step: + self.heartbeat() + return + current_verified = dict(self.snapshot.get("verified") or {}) + if verified: + current_verified.update(verified) + self.snapshot["verified"] = current_verified + current_held = dict(self.snapshot.get("held_objects") or {}) + if held_objects: + current_held.update(held_objects) + self.snapshot["held_objects"] = current_held + self.snapshot["status"] = "running" + self.snapshot["stage"] = stage + self.snapshot["step"] = step + self.snapshot["next_stage"] = stage + self.snapshot["updated_at"] = now_stamp() + self.snapshot["heartbeat_at"] = now_stamp() + self._write_snapshot() + self._append_event("stage_progress", {"step": step}) + + def fail_stage(self, stage: str, reason: str, *, auto_recoverable: bool = True) -> None: + self.snapshot["status"] = "stopped" + self.snapshot["stage"] = stage + self.snapshot["step"] = f"{stage}_failed" + self.snapshot["next_stage"] = stage + self.snapshot["stop_reason"] = reason + self.snapshot["blocker"] = reason + self.snapshot["auto_recoverable"] = auto_recoverable + self.snapshot["required_user_action"] = ( + "하드웨어 상태를 확인한 뒤 '복구 다시 확인' 또는 '이어서 해줘'라고 말하세요." + ) + self.snapshot["updated_at"] = now_stamp() + self.snapshot["heartbeat_at"] = now_stamp() + self._write_snapshot() + self._append_event("stage_failed", {"reason": reason, "auto_recoverable": auto_recoverable}) + + def block(self, reason: str, required_user_action: str, *, auto_recoverable: bool) -> None: + if not self.snapshot: + self.snapshot = self._new_snapshot(status="blocked") + self.snapshot["status"] = "blocked" + self.snapshot["stop_reason"] = reason + self.snapshot["blocker"] = reason + self.snapshot["auto_recoverable"] = auto_recoverable + self.snapshot["required_user_action"] = required_user_action + self.snapshot["updated_at"] = now_stamp() + self.snapshot["heartbeat_at"] = now_stamp() + self._write_snapshot() + self._append_event("blocked", {"reason": reason, "auto_recoverable": auto_recoverable}) + + def complete_run(self) -> None: + self.snapshot["status"] = "completed" + self.snapshot["stage"] = "complete" + self.snapshot["step"] = "complete" + self.snapshot["next_stage"] = "complete" + self.snapshot["completed_stages"] = list(FLOW_STAGES) + self.snapshot["updated_at"] = now_stamp() + self.snapshot["heartbeat_at"] = now_stamp() + self._write_snapshot() + self._append_event("run_completed") diff --git a/src/azas_task_manager/test/test_auto_flow_resume_state.py b/src/azas_task_manager/test/test_auto_flow_resume_state.py new file mode 100644 index 0000000..add1efa --- /dev/null +++ b/src/azas_task_manager/test/test_auto_flow_resume_state.py @@ -0,0 +1,89 @@ +from azas_task_manager.auto_flow_resume_state import ( + AutoFlowResumeStore, + FLOW_STAGES, + load_resume_snapshot, + safe_recipe_colors_from_snapshot, +) + + +def make_store(tmp_path, *, mode="normal", recipe_colors="red:1,blue:1"): + return AutoFlowResumeStore( + state_path=tmp_path / "resume.json", + events_path=tmp_path / "events.jsonl", + mode=mode, + recipe_colors=recipe_colors, + recipe_id="recipe_test", + ) + + +def test_normal_run_records_stage_progress_and_resume_skips_completed_stages(tmp_path): + store = make_store(tmp_path) + assert store.prepare() + assert store.next_stage() == "color_scan" + + store.start_stage("color_scan") + store.complete_stage("color_scan", verified={"color_map": True}) + store.start_stage("observe") + store.complete_stage("observe") + + snapshot = load_resume_snapshot(tmp_path / "resume.json") + assert snapshot is not None + assert snapshot["completed_stages"] == ["color_scan", "observe"] + assert safe_recipe_colors_from_snapshot(snapshot) == "red:1,blue:1" + + resumed = make_store(tmp_path, mode="resume", recipe_colors="") + assert resumed.prepare() + assert resumed.should_skip("color_scan") + assert resumed.should_skip("observe") + assert not resumed.should_skip("open_gripper") + assert resumed.next_stage() == "open_gripper" + + +def test_failed_stage_records_next_stage_and_recovery_instruction(tmp_path): + store = make_store(tmp_path) + assert store.prepare() + store.start_stage("cup_pick") + store.fail_stage("cup_pick", "side_grasp_joint_state_stale") + + snapshot = load_resume_snapshot(tmp_path / "resume.json") + assert snapshot is not None + assert snapshot["status"] == "stopped" + assert snapshot["stage"] == "cup_pick" + assert snapshot["next_stage"] == "cup_pick" + assert snapshot["blocker"] == "side_grasp_joint_state_stale" + assert snapshot["auto_recoverable"] is True + assert "이어서" in snapshot["required_user_action"] + + +def test_progress_updates_verified_facts_without_recording_coordinates(tmp_path): + store = make_store(tmp_path) + assert store.prepare() + store.start_stage("lid_shake") + store.update_progress( + "lid_shake", + "lid_closed", + verified={"lid_grasped": True, "lid_closed": True}, + held_objects={"cup": "in_holder", "lid": "on_cup"}, + ) + + snapshot = load_resume_snapshot(tmp_path / "resume.json") + assert snapshot is not None + assert snapshot["step"] == "lid_closed" + assert snapshot["verified"]["lid_grasped"] is True + assert snapshot["verified"]["lid_closed"] is True + assert snapshot["held_objects"] == {"cup": "in_holder", "lid": "on_cup"} + assert "pose" not in snapshot + assert snapshot["next_stage"] == "lid_shake" + + +def test_complete_run_marks_every_stage_completed(tmp_path): + store = make_store(tmp_path) + assert store.prepare() + store.complete_run() + + snapshot = load_resume_snapshot(tmp_path / "resume.json") + assert snapshot is not None + assert snapshot["status"] == "completed" + assert snapshot["stage"] == "complete" + assert snapshot["next_stage"] == "complete" + assert tuple(snapshot["completed_stages"]) == FLOW_STAGES diff --git a/src/azas_voice/azas_voice/command_parser.py b/src/azas_voice/azas_voice/command_parser.py index c8a2340..8755bb3 100644 --- a/src/azas_voice/azas_voice/command_parser.py +++ b/src/azas_voice/azas_voice/command_parser.py @@ -61,6 +61,88 @@ def _contains_any(normalized: str, words: tuple[str, ...]) -> bool: return any(normalize_text(word) in normalized for word in words) +RECOVERY_RESTART_WORDS = ( + "처음부터다시", + "처음부터시작", + "처음부터해", + "새로시작", + "처음부터", +) +RECOVERY_CLEAR_WORDS = ( + "복구기록초기화", + "복구기록삭제", + "체크포인트삭제", + "체크포인트초기화", + "재개기록삭제", +) +RECOVERY_RECHECK_WORDS = ( + "복구다시확인", + "복구상태확인", + "상태다시확인", + "다시확인", + "점검해", + "점검해줘", +) +RECOVERY_RESUME_WORDS = ( + "이어서해", + "이어서해줘", + "이어서진행", + "마저해", + "마저진행", + "계속진행", + "멈춘데서", + "멈춘곳에서", + "멈춘부분", + "재개해", + "재개해줘", + "복구시작", +) + + +def _recovery_decision(utterance: str, normalized: str) -> RecipeDecision | None: + if _contains_any(normalized, RECOVERY_CLEAR_WORDS): + return RecipeDecision( + True, + utterance, + normalized, + "clear_recovery", + None, + (), + "복구 기록을 초기화합니다.", + ) + if _contains_any(normalized, RECOVERY_RESTART_WORDS): + return RecipeDecision( + True, + utterance, + normalized, + "restart_flow", + None, + (), + "이전 주문을 처음부터 다시 시작할 수 있는지 확인합니다.", + ) + if _contains_any(normalized, RECOVERY_RECHECK_WORDS): + return RecipeDecision( + True, + utterance, + normalized, + "recheck_recovery", + None, + (), + "복구 상태를 다시 확인합니다.", + ) + if _contains_any(normalized, RECOVERY_RESUME_WORDS): + return RecipeDecision( + True, + utterance, + normalized, + "resume_flow", + None, + (), + "이전 작업을 이어서 진행할 수 있는지 확인합니다.", + ) + return None + + def _match_recipe(normalized: str) -> str | None: if "디스펜서" in normalized: return None @@ -89,6 +171,10 @@ def _is_random_recipe_request(normalized: str) -> bool: return has_mood or has_random +def _is_preference_mix_request(normalized: str) -> bool: + return _contains_any(normalized, PREFERENCE_WORDS) + + def _recipe_name(recipe_id: str) -> str: return RECIPE_DISPLAY_NAMES.get(recipe_id, recipe_id) @@ -220,6 +306,10 @@ def parse_recipe_command(text: str) -> RecipeDecision: if not normalized: return RecipeDecision(False, utterance, normalized, "unknown", None, (), "", "empty utterance") + recovery = _recovery_decision(utterance, normalized) + if recovery is not None: + return recovery + if _contains_any(normalized, REROLL_RECOMMENDATION_WORDS): return _random_recipe_decision(utterance, normalized) @@ -229,12 +319,30 @@ def parse_recipe_command(text: str) -> RecipeDecision: recipe_id = _match_recipe(normalized) dispenser_ids = _match_colors(normalized) + if recipe_id is None and _is_preference_mix_request(normalized): + if not dispenser_ids or any( + marker in normalized + for marker in ( + "적게", + "많이", + "진하게", + "약하게", + "강하게", + "덜", + "안", + "않", + "부담", + "추천", + ) + ): + return _custom_preference_decision(utterance, normalized) + if recipe_id is None and not dispenser_ids and _is_random_recipe_request(normalized): - if _contains_any(normalized, PREFERENCE_WORDS): + if _is_preference_mix_request(normalized): return _custom_preference_decision(utterance, normalized) return _random_recipe_decision(utterance, normalized) - if recipe_id is None and not dispenser_ids and _contains_any(normalized, PREFERENCE_WORDS): + if recipe_id is None and not dispenser_ids and _is_preference_mix_request(normalized): return _custom_preference_decision(utterance, normalized) if recipe_id is None and not dispenser_ids and _contains_any(normalized, CONFIRM_WORDS): diff --git a/src/azas_voice/azas_voice/conversation_manager_node.py b/src/azas_voice/azas_voice/conversation_manager_node.py index 2179ffa..1cb4be3 100644 --- a/src/azas_voice/azas_voice/conversation_manager_node.py +++ b/src/azas_voice/azas_voice/conversation_manager_node.py @@ -18,6 +18,7 @@ def __init__(self): self.declare_parameter("decision_topic", "/azas/voice/recipe_decision") self.declare_parameter("confirmation_topic", "/azas/voice/confirmation") self.declare_parameter("confirmed_decision_topic", "/azas/voice/confirmed_recipe_decision") + self.declare_parameter("recovery_command_topic", "/azas/voice/recovery_command") self.declare_parameter("pending_timeout_s", 30.0) self._pending: dict[str, object] | None = None @@ -34,6 +35,11 @@ def __init__(self): str(self.get_parameter("confirmed_decision_topic").value), 10, ) + self._recovery_pub = self.create_publisher( + String, + str(self.get_parameter("recovery_command_topic").value), + 10, + ) self.create_subscription( String, str(self.get_parameter("decision_topic").value), @@ -62,6 +68,8 @@ def _on_decision(self, msg: String) -> None: elif intent == "cancel": self._pending = None self._publish_confirmation(str(decision.get("confirmation") or "취소했습니다.")) + elif intent in {"resume_flow", "restart_flow", "recheck_recovery", "clear_recovery"}: + self._handle_recovery_command(decision) elif decision.get("valid"): self._publish_confirmation(str(decision.get("confirmation") or "명령을 확인했습니다.")) else: @@ -105,6 +113,19 @@ def _handle_confirm(self, decision: dict[str, object]) -> None: self.get_logger().info(msg.data) self._publish_confirmation("제조를 시작합니다.") + def _handle_recovery_command(self, decision: dict[str, object]) -> None: + if not decision.get("valid"): + self._publish_confirmation("복구 명령을 다시 말씀해주세요.") + return + command = copy.deepcopy(decision) + command["confirmed"] = True + command["confirmed_by"] = "voice_recovery" + msg = String() + msg.data = json.dumps(command, ensure_ascii=False) + self._recovery_pub.publish(msg) + self.get_logger().info(msg.data) + self._publish_confirmation(str(command.get("confirmation") or "복구 상태를 확인합니다.")) + def _publish_confirmation(self, text: str) -> None: msg = String() msg.data = text diff --git a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py index 0cd437c..bfc5edf 100644 --- a/src/azas_voice/azas_voice/llm_recipe_mapper_node.py +++ b/src/azas_voice/azas_voice/llm_recipe_mapper_node.py @@ -22,7 +22,16 @@ ) -ALLOWED_INTENTS = {"make_cocktail", "confirm", "cancel", "unknown"} +ALLOWED_INTENTS = { + "make_cocktail", + "confirm", + "cancel", + "resume_flow", + "restart_flow", + "recheck_recovery", + "clear_recovery", + "unknown", +} ALLOWED_CUSTOM_RECIPE_IDS = {"custom_color_selection", "custom_preference_mix"} DISPENSER_NUMBER_TO_COLOR = { "1": "red", @@ -96,7 +105,14 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: return _fallback_decision(text, f"invalid_intent:{intent}") fallback = parse_recipe_command(text) - if fallback.valid and fallback.intent in {"confirm", "cancel"}: + if fallback.valid and fallback.intent in { + "confirm", + "cancel", + "resume_flow", + "restart_flow", + "recheck_recovery", + "clear_recovery", + }: return fallback if fallback.valid and fallback.intent == "make_cocktail" and fallback.recipe_id in RECIPE_DISPENSERS and "추천" in fallback.confirmation: return fallback @@ -140,7 +156,7 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: if intent == "make_cocktail" and recipe_id is None and not dispenser_ids: return _fallback_decision(text, "missing_recipe_or_dispenser") - valid = intent in {"make_cocktail", "confirm", "cancel"} + valid = intent in ALLOWED_INTENTS - {"unknown"} if intent == "make_cocktail" and recipe_id is None: recipe_id = "custom_preference_mix" if dispenser_amounts else "custom_color_selection" @@ -166,6 +182,14 @@ def _sanitize_llm_decision(text: str, payload: dict) -> RecipeDecision: confirmation = "칵테일 제조 요청을 취소합니다." elif intent == "confirm": confirmation = "선택한 칵테일 제조를 확인했습니다." + elif intent == "resume_flow": + confirmation = "이전 작업을 이어서 진행할 수 있는지 확인합니다." + elif intent == "restart_flow": + confirmation = "이전 주문을 처음부터 다시 시작할 수 있는지 확인합니다." + elif intent == "recheck_recovery": + confirmation = "복구 상태를 다시 확인합니다." + elif intent == "clear_recovery": + confirmation = "복구 기록을 초기화합니다." elif recipe_id == "custom_preference_mix" and fallback.confirmation: confirmation = fallback.confirmation elif fallback.confirmation and "추천" in fallback.confirmation: @@ -294,6 +318,8 @@ def _call_chat_api(self, text: str, api_key: str) -> dict: f"{', '.join(RECIPE_DISPENSERS)}. " "Only choose a catalog recipe when the user explicitly asks for a numbered/named menu or gives no preferences. " "Allowed dispenser_ids values: red, yellow, green, blue only. " + "For recovery commands such as 이어서 해줘, 복구 다시 확인, 처음부터 다시, or 복구 기록 초기화, " + "use intents resume_flow, recheck_recovery, restart_flow, or clear_recovery. " "Do not output dispenser_amounts; the application calculates amounts from traits. " "Never output robot coordinates, calibration values, trajectories, or safety approvals." ), diff --git a/src/azas_voice/azas_voice/recipe_catalog.py b/src/azas_voice/azas_voice/recipe_catalog.py index ba10f45..f90b7df 100644 --- a/src/azas_voice/azas_voice/recipe_catalog.py +++ b/src/azas_voice/azas_voice/recipe_catalog.py @@ -329,12 +329,20 @@ "오케", "콜", "가자", + "가보자", "시작", "시작해", "시작해줘", "진행", "진행해", "진행해줘", + "만들어", + "만들어줘", + "만들어주세요", + "제조", + "제조해", + "제조해줘", + "제조해주세요", "계속해", "계속해줘", "계속", diff --git a/src/azas_voice/azas_voice/voice_pipeline_executor_node.py b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py index 957cbbf..4c8d30e 100644 --- a/src/azas_voice/azas_voice/voice_pipeline_executor_node.py +++ b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py @@ -2,6 +2,7 @@ import json import os +from pathlib import Path import signal import subprocess import threading @@ -17,6 +18,9 @@ ALLOWED_DISPENSERS = ("red", "yellow", "green", "blue") +DEFAULT_RESUME_STATE_FILE = Path("/home/ssu/Azas/outputs/auto_cup_flow_resume.json") +DEFAULT_RESUME_EVENTS_FILE = Path("/home/ssu/Azas/outputs/auto_cup_flow_events.jsonl") +DEFAULT_DISPENSER_RESUME_STATE_FILE = Path("/home/ssu/Azas/outputs/measured_dispenser_recipe_resume.json") # 라우터 stdout에서 단계 전환을 감지해 UI에 보여줄 한국어 단계명으로 변환한다. # (auto_cup_flow_router의 로그 문구가 바뀌면 여기도 같이 갱신할 것) @@ -79,6 +83,26 @@ def stage_from_line(line: str) -> str | None: return None +def load_resume_snapshot(path: str | Path = DEFAULT_RESUME_STATE_FILE) -> dict[str, object] | None: + state_path = Path(path) + if not state_path.is_file(): + return None + try: + payload = json.loads(state_path.read_text(encoding="utf-8")) + except (OSError, json.JSONDecodeError): + return None + return payload if isinstance(payload, dict) else None + + +def recipe_colors_from_resume_snapshot(snapshot: dict[str, object] | None) -> str: + if not isinstance(snapshot, dict): + return "" + recipe = snapshot.get("recipe") + if not isinstance(recipe, dict): + return "" + return str(recipe.get("recipe_colors") or "").strip() + + class VoicePipelineExecutorNode(Node): """Confirmed voice recipe -> full auto cup flow (pick -> recipe -> lid -> shake). @@ -89,6 +113,7 @@ class VoicePipelineExecutorNode(Node): def __init__(self): super().__init__("voice_pipeline_executor_node") self.declare_parameter("confirmed_decision_topic", "/azas/voice/confirmed_recipe_decision") + self.declare_parameter("recovery_command_topic", "/azas/voice/recovery_command") self.declare_parameter("status_topic", "/azas/voice/pipeline_status") self.declare_parameter("enable_hardware_execution", False) self.declare_parameter("require_confirmed", True) @@ -96,6 +121,9 @@ def __init__(self): self.declare_parameter("service_prefix", "dsr01") self.declare_parameter("max_repeats_per_dispenser", 3) self.declare_parameter("default_amount", 1) + self.declare_parameter("resume_state_file", str(DEFAULT_RESUME_STATE_FILE)) + self.declare_parameter("resume_events_file", str(DEFAULT_RESUME_EVENTS_FILE)) + self.declare_parameter("dispenser_resume_state_file", str(DEFAULT_DISPENSER_RESUME_STATE_FILE)) self._status_pub = self.create_publisher( String, @@ -108,6 +136,12 @@ def __init__(self): self._on_confirmed_decision, 10, ) + self.create_subscription( + String, + str(self.get_parameter("recovery_command_topic").value), + self._on_recovery_command, + 10, + ) self._lock = threading.Lock() self._active_proc: subprocess.Popen[str] | None = None @@ -138,6 +172,105 @@ def _on_confirmed_decision(self, msg: String) -> None: self._publish_status("blocked", reason="no_executable_recipe_colors", decision=decision) return + self._start_pipeline(decision, recipe_colors, resume_mode="normal", trigger="confirmed_recipe") + + def _on_recovery_command(self, msg: String) -> None: + try: + command = json.loads(msg.data) + except json.JSONDecodeError as exc: + self._publish_status("blocked", reason="invalid_recovery_command_json", error=str(exc)) + return + intent = str(command.get("intent") or "") + if intent == "clear_recovery": + self._clear_recovery_state() + self._publish_status("recovery_cleared", stage="복구 기록 초기화") + return + + snapshot = load_resume_snapshot(str(self.get_parameter("resume_state_file").value)) + if intent == "recheck_recovery": + self._publish_recovery_check(snapshot) + return + if intent not in {"resume_flow", "restart_flow"}: + self._publish_status("blocked", reason="unsupported_recovery_intent", intent=intent) + return + if not snapshot: + self._publish_status( + "blocked", + reason="no_resume_state", + required_user_action="저장된 복구 상태가 없습니다. 새 주문을 먼저 시작하세요.", + ) + return + + recipe_colors = recipe_colors_from_resume_snapshot(snapshot) + if not recipe_colors: + self._publish_status( + "blocked", + reason="resume_state_missing_recipe", + recovery_snapshot=snapshot, + required_user_action="저장된 주문 정보가 없어 처음부터 새 메뉴를 주문해야 합니다.", + ) + return + status = str(snapshot.get("status") or "") + if intent == "resume_flow" and status == "completed": + self._publish_status("completed", reason="resume_state_already_completed", recovery_snapshot=snapshot) + return + if intent == "resume_flow" and snapshot.get("auto_recoverable") is False: + self._publish_status( + "blocked", + reason=str(snapshot.get("blocker") or "manual_recovery_required"), + recovery_snapshot=snapshot, + required_user_action=snapshot.get("required_user_action") + or "하드웨어 상태를 조치한 뒤 복구 다시 확인이라고 말하세요.", + ) + return + + resume_mode = "restart" if intent == "restart_flow" else "resume" + self._start_pipeline(command, recipe_colors, resume_mode=resume_mode, trigger="voice_recovery") + + def _publish_recovery_check(self, snapshot: dict[str, object] | None) -> None: + if not snapshot: + self._publish_status( + "blocked", + reason="no_resume_state", + required_user_action="저장된 복구 상태가 없습니다.", + ) + return + if snapshot.get("auto_recoverable") is False: + self._publish_status( + "blocked", + reason=str(snapshot.get("blocker") or "manual_recovery_required"), + recovery_snapshot=snapshot, + required_user_action=snapshot.get("required_user_action"), + ) + return + self._publish_status( + "recovery_ready", + stage="복구 가능 상태", + next_stage=snapshot.get("next_stage"), + recovery_snapshot=snapshot, + ) + + def _clear_recovery_state(self) -> None: + for raw_path in ( + self.get_parameter("resume_state_file").value, + self.get_parameter("resume_events_file").value, + self.get_parameter("dispenser_resume_state_file").value, + ): + try: + Path(str(raw_path)).unlink() + except FileNotFoundError: + pass + except OSError as exc: + self.get_logger().warn(f"failed to clear recovery file {raw_path}: {exc}") + + def _start_pipeline( + self, + decision: dict[str, object], + recipe_colors: str, + *, + resume_mode: str, + trigger: str, + ) -> None: with self._lock: if self._active_proc is not None and self._active_proc.poll() is None: self._publish_status( @@ -160,15 +293,29 @@ def _on_confirmed_decision(self, msg: String) -> None: recipe_colors=recipe_colors, command=command, hardware_enabled=bool(self.get_parameter("enable_hardware_execution").value), + resume_mode=resume_mode, + trigger=trigger, ) if not bool(self.get_parameter("enable_hardware_execution").value): - self._publish_status("dry_run", recipe_colors=recipe_colors, command=command) + self._publish_status( + "dry_run", + recipe_colors=recipe_colors, + command=command, + resume_mode=resume_mode, + trigger=trigger, + ) return env = os.environ.copy() env["ROUTER_CONFIRM"] = "ENABLE_AUTO_CUP_ROUTER" env["SERVICE_PREFIX"] = str(self.get_parameter("service_prefix").value) + env["AUTO_FLOW_RESUME_MODE"] = resume_mode + env["AUTO_FLOW_RESUME_STATE_FILE"] = str(self.get_parameter("resume_state_file").value) + env["AUTO_FLOW_RESUME_EVENTS_FILE"] = str(self.get_parameter("resume_events_file").value) + env["AUTO_FLOW_DISPENSER_RESUME_STATE_FILE"] = str( + self.get_parameter("dispenser_resume_state_file").value + ) try: proc = subprocess.Popen( command, @@ -187,11 +334,11 @@ def _on_confirmed_decision(self, msg: String) -> None: self._active_proc = proc threading.Thread( target=self._monitor_pipeline, - args=(proc, recipe_colors), + args=(proc, recipe_colors, resume_mode), daemon=True, ).start() - def _monitor_pipeline(self, proc: subprocess.Popen[str], recipe_colors: str) -> None: + def _monitor_pipeline(self, proc: subprocess.Popen[str], recipe_colors: str, resume_mode: str) -> None: last_stage = "" if proc.stdout is not None: for line in proc.stdout: @@ -211,6 +358,7 @@ def _monitor_pipeline(self, proc: subprocess.Popen[str], recipe_colors: str) -> recipe_colors=recipe_colors, returncode=code, last_stage=last_stage, + resume_mode=resume_mode, ) def _publish_status(self, status: str, **fields: object) -> None: diff --git a/src/azas_voice/test/test_command_parser.py b/src/azas_voice/test/test_command_parser.py index 7948c18..e63eccb 100644 --- a/src/azas_voice/test/test_command_parser.py +++ b/src/azas_voice/test/test_command_parser.py @@ -183,6 +183,22 @@ def test_cancel_intent(): assert decision.intent == "cancel" +def test_recovery_voice_commands_map_to_operational_intents(): + expected = { + "이어서 해줘": "resume_flow", + "멈춘 데서 다시 진행해": "resume_flow", + "복구 다시 확인": "recheck_recovery", + "처음부터 다시 해줘": "restart_flow", + "복구 기록 초기화": "clear_recovery", + } + for utterance, intent in expected.items(): + decision = parse_recipe_command(utterance) + assert decision.valid + assert decision.intent == intent + assert decision.recipe_id is None + assert decision.dispenser_ids == () + + def test_proceed_phrase_maps_to_confirm_intent(): decision = parse_recipe_command("진행해줘") assert decision.valid @@ -199,6 +215,7 @@ def test_common_acknowledgements_map_to_confirm_intent(): "괜찮아", "콜", "가자", + "가보자", "계속해줘", ): decision = parse_recipe_command(utterance) @@ -206,6 +223,21 @@ def test_common_acknowledgements_map_to_confirm_intent(): assert decision.intent == "confirm" +def test_natural_start_phrases_map_to_confirm_intent(): + for utterance in ( + "만들어줘", + "어 만들어줘", + "그래 좋아 시작해", + "가보자 그래 좋아", + "만들어 진행해 시작해", + "시작 만들어", + "제조해줘", + ): + decision = parse_recipe_command(utterance) + assert decision.valid + assert decision.intent == "confirm" + + def test_order_phrase_with_make_word_still_maps_to_preference_order(): decision = parse_recipe_command("술 약하게 해서 만들어줘") assert decision.valid diff --git a/src/azas_voice/test/test_voice_pipeline_recovery_helpers.py b/src/azas_voice/test/test_voice_pipeline_recovery_helpers.py new file mode 100644 index 0000000..aa63912 --- /dev/null +++ b/src/azas_voice/test/test_voice_pipeline_recovery_helpers.py @@ -0,0 +1,30 @@ +from azas_voice.voice_pipeline_executor_node import ( + load_resume_snapshot, + recipe_colors_from_resume_snapshot, +) + + +def test_recipe_colors_from_resume_snapshot_uses_stored_recipe_only(): + snapshot = { + "recipe": { + "recipe_id": "recipe_05", + "recipe_colors": "red:2,yellow:1,blue:1", + }, + "stage": "recipe", + } + + assert recipe_colors_from_resume_snapshot(snapshot) == "red:2,yellow:1,blue:1" + assert recipe_colors_from_resume_snapshot({"recipe": {}}) == "" + assert recipe_colors_from_resume_snapshot(None) == "" + + +def test_load_resume_snapshot_rejects_missing_and_invalid_files(tmp_path): + assert load_resume_snapshot(tmp_path / "missing.json") is None + + invalid = tmp_path / "invalid.json" + invalid.write_text("{not json", encoding="utf-8") + assert load_resume_snapshot(invalid) is None + + valid = tmp_path / "valid.json" + valid.write_text('{"status": "stopped"}\n', encoding="utf-8") + assert load_resume_snapshot(valid) == {"status": "stopped"} diff --git a/src/azas_voice/web/voice.js b/src/azas_voice/web/voice.js index 394191d..1831078 100644 --- a/src/azas_voice/web/voice.js +++ b/src/azas_voice/web/voice.js @@ -183,6 +183,8 @@ function renderSteps(pipeline) { activeIndex = 0; } else if (status === "completed") { activeIndex = pipelineSteps.length; + } else if (status === "recovery_ready" && pipeline.stage in STAGE_TO_STEP) { + activeIndex = STAGE_TO_STEP[pipeline.stage]; } pipelineSteps.forEach((step, index) => { step.classList.toggle("done", activeIndex > index); @@ -198,9 +200,9 @@ function renderRobot(activeIndex, pipeline, hasMenu) { return; } const status = pipeline.status || ""; - if (status === "failed") { + if (status === "failed" || status === "blocked") { robotScene.dataset.step = "idle"; - robotStatusText.textContent = "제조 중단"; + robotStatusText.textContent = status === "blocked" ? "복구 대기" : "제조 중단"; return; } if (status === "completed" || activeIndex >= pipelineSteps.length) { @@ -293,7 +295,15 @@ function renderMenu(state) { menuCard.hidden = true; menuEmpty.hidden = false; renderRobot(-1, pipeline, false); - setBadge("idle", "대기 중"); + if (pipeline.status === "blocked") { + setBadge("failed", "복구 조치 필요"); + } else if (pipeline.status === "recovery_ready") { + setBadge("confirmed", "복구 가능"); + } else if (pipeline.status === "recovery_cleared") { + setBadge("idle", "복구 기록 초기화"); + } else { + setBadge("idle", "대기 중"); + } return; } @@ -322,10 +332,14 @@ function renderMenu(state) { renderRobot(activeIndex, pipeline, true); const pipelineStatus = pipeline.status || ""; - if (pipelineStatus === "failed") { - setBadge("failed", "제조 실패"); + if (pipelineStatus === "failed" || pipelineStatus === "blocked") { + setBadge("failed", pipelineStatus === "blocked" ? "복구 조치 필요" : "제조 실패"); } else if (pipelineStatus === "completed") { setBadge("done", "완성! 맛있게 드세요"); + } else if (pipelineStatus === "recovery_ready") { + setBadge("confirmed", "복구 가능"); + } else if (pipelineStatus === "recovery_cleared") { + setBadge("idle", "복구 기록 초기화"); } else if (pipelineStatus === "running" || pipelineStatus === "starting") { setBadge("making", pipeline.stage ? `제조 중 · ${pipeline.stage}` : "제조 중"); } else if (pipelineStatus === "dry_run") { diff --git a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py index 9034dbf..9b33e70 100644 --- a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py +++ b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py @@ -26,6 +26,16 @@ def _as_bool(value): def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params): controller_name = LaunchConfiguration("moveit_controller_name").perform(context) runtime_moveit_params = deepcopy(moveit_params) + trajectory_execution = runtime_moveit_params.setdefault("trajectory_execution", {}) + trajectory_execution["allowed_execution_duration_scaling"] = float( + LaunchConfiguration("trajectory_execution_allowed_duration_scaling").perform(context) + ) + trajectory_execution["allowed_goal_duration_margin"] = float( + LaunchConfiguration("trajectory_execution_allowed_goal_duration_margin").perform(context) + ) + trajectory_execution["allowed_start_tolerance"] = float( + LaunchConfiguration("trajectory_execution_allowed_start_tolerance").perform(context) + ) runtime_moveit_params["moveit_simple_controller_manager"] = { "controller_names": [controller_name], controller_name: { @@ -845,6 +855,21 @@ def generate_launch_description(): default_value="true", description="Start /dsr01/joint_states -> /joint_states relay. Disable if another relay already runs.", ) + trajectory_execution_allowed_duration_scaling_arg = DeclareLaunchArgument( + "trajectory_execution_allowed_duration_scaling", + default_value="3.0", + description="MoveIt execution timeout scaling for real-controller low side-grip moves.", + ) + trajectory_execution_allowed_goal_duration_margin_arg = DeclareLaunchArgument( + "trajectory_execution_allowed_goal_duration_margin", + default_value="3.0", + description="Extra seconds MoveIt waits past expected trajectory duration before cancelling.", + ) + trajectory_execution_allowed_start_tolerance_arg = DeclareLaunchArgument( + "trajectory_execution_allowed_start_tolerance", + default_value="0.01", + description="Allowed start-state tolerance for trajectory execution.", + ) return LaunchDescription( [ @@ -949,6 +974,9 @@ def generate_launch_description(): auto_pick_arg, moveit_controller_name_arg, start_joint_state_relay_arg, + trajectory_execution_allowed_duration_scaling_arg, + trajectory_execution_allowed_goal_duration_margin_arg, + trajectory_execution_allowed_start_tolerance_arg, OpaqueFunction( function=_runtime_nodes, args=[moveit_params, moveit_py_params, side_prepose_params], diff --git a/tools/run/pick_from_cup_holder_side_grip.py b/tools/run/pick_from_cup_holder_side_grip.py index 78d12f9..03059ae 100755 --- a/tools/run/pick_from_cup_holder_side_grip.py +++ b/tools/run/pick_from_cup_holder_side_grip.py @@ -206,7 +206,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--gripper-service", default="/jarvis/rg2/set_width") parser.add_argument("--gripper-open-width-m", type=float, default=0.110) parser.add_argument("--gripper-grasp-width-m", type=float, default=0.068) - parser.add_argument("--gripper-force-n", type=float, default=35.0) + parser.add_argument("--gripper-force-n", type=float, default=20.0) parser.add_argument("--gripper-timeout-sec", type=float, default=12.0) parser.add_argument("--post-grasp-settle-sec", type=float, default=0.8) parser.add_argument("--execute", action="store_true") diff --git a/tools/run/pick_from_measured_dispenser_front_hold.py b/tools/run/pick_from_measured_dispenser_front_hold.py index b5285b9..b59678a 100755 --- a/tools/run/pick_from_measured_dispenser_front_hold.py +++ b/tools/run/pick_from_measured_dispenser_front_hold.py @@ -404,7 +404,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--gripper-service", default="/jarvis/rg2/set_width") parser.add_argument("--gripper-open-width-m", type=float, default=0.110) parser.add_argument("--gripper-grasp-width-m", type=float, default=0.075) - parser.add_argument("--gripper-force-n", type=float, default=25.0) + parser.add_argument("--gripper-force-n", type=float, default=20.0) parser.add_argument("--gripper-timeout-sec", type=float, default=12.0) parser.add_argument("--joint1-clearance-deg", type=float, default=0.0) parser.add_argument("--joint1-clearance-velocity", type=float, default=20.0) diff --git a/tools/run/place_side_grip_cup_in_holder.py b/tools/run/place_side_grip_cup_in_holder.py index 4f72330..be767db 100755 --- a/tools/run/place_side_grip_cup_in_holder.py +++ b/tools/run/place_side_grip_cup_in_holder.py @@ -93,6 +93,12 @@ def offset_target_y(target: TargetPose, offset_m: float) -> TargetPose: return TargetPose(target.label, adjusted_xyz, list(target.rpy_rad)) +def offset_target_rz(target: TargetPose, offset_deg: float) -> TargetPose: + adjusted_rpy = list(target.rpy_rad) + adjusted_rpy[2] += math.radians(float(offset_deg)) + return TargetPose(target.label, list(target.xyz_m), adjusted_rpy) + + def print_target(target: TargetPose) -> None: rx, ry, rz = target.rpy_deg x, y, z = target.xyz_m @@ -305,6 +311,12 @@ def parse_args() -> argparse.Namespace: "the holder placement 10mm in negative Y without rewriting calibration.yaml." ), ) + parser.add_argument( + "--rz-offset-deg", + type=float, + default=0.0, + help="Add this RZ offset to all measured cup-holder side-grip poses without rewriting calibration.yaml.", + ) parser.add_argument("--timeout-sec", type=float, default=90.0) parser.add_argument("--wait-service-sec", type=float, default=8.0) parser.add_argument("--verify-timeout-sec", type=float, default=35.0) @@ -362,6 +374,10 @@ def main() -> int: place_final = offset_target_y(place_final, args.place_final_y_offset_m) if abs(args.place_final_z_offset_m) > 1e-9: place_final = offset_target_z(place_final, args.place_final_z_offset_m) + if abs(args.rz_offset_deg) > 1e-9: + pre_place = offset_target_rz(pre_place, args.rz_offset_deg) + place_final = offset_target_rz(place_final, args.rz_offset_deg) + retreat = offset_target_rz(retreat, args.rz_offset_deg) except (OSError, ValueError, yaml.YAMLError) as exc: print(f"[FAIL] {exc}") return 2 @@ -372,6 +388,7 @@ def main() -> int: print(f"[Azas] approach_lift_m={approach_lift_m:.3f}") print(f"[Azas] place_final_y_offset_m={args.place_final_y_offset_m:.4f}") print(f"[Azas] place_final_z_offset_m={args.place_final_z_offset_m:.4f}") + print(f"[Azas] rz_offset_deg={args.rz_offset_deg:.3f}") print_target(pre_place) print_target(place_final) print_target(retreat) diff --git a/tools/run/publish_color_recipe_sequence_rviz_preview.py b/tools/run/publish_color_recipe_sequence_rviz_preview.py index 90713dd..5553025 100644 --- a/tools/run/publish_color_recipe_sequence_rviz_preview.py +++ b/tools/run/publish_color_recipe_sequence_rviz_preview.py @@ -345,7 +345,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--move-release-offset-z-m", type=float, default=0.0) parser.add_argument("--press-pre-lift-retreat-x-m", type=float, default=-0.050) parser.add_argument("--press-pre-lift-retreat-y-m", type=float, default=0.0) - parser.add_argument("--press-min-transit-z-m", type=float, default=0.500) + parser.add_argument("--press-min-transit-z-m", type=float, default=0.350) parser.add_argument("--press-transit-height-m", type=float, default=0.080) parser.add_argument("--press-pre-lift-m", type=float, default=0.080) parser.add_argument("--press-depth-m", type=float, default=0.060) diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 093e9a9..b7ace0f 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -320,10 +320,16 @@ def _color_recipe_direct_arg(payload: dict[str, Any]) -> str: def color_recipe_sequence_command(payload: dict[str, Any]) -> str: + cup_holder_rz_offset_deg = str( + payload.get("cup_holder_rz_offset_deg") + or os.environ.get("CUP_HOLDER_RZ_OFFSET_DEG") + or "-1.0" + ).strip() return ( f"cd {ROOT} && {ROS_SETUP} && " "python3 tools/run/run_color_recipe_sequence.py --execute --confirm" f"{_color_recipe_direct_arg(payload)}" + f" --cup-holder-rz-offset-deg {shlex.quote(cup_holder_rz_offset_deg)}" ) @@ -3233,6 +3239,11 @@ def shell_env(payload: dict[str, Any]) -> dict[str, str]: or env.get("CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M") or "-0.010" ) + env["CUP_HOLDER_RZ_OFFSET_DEG"] = str( + payload.get("cup_holder_rz_offset_deg") + or env.get("CUP_HOLDER_RZ_OFFSET_DEG") + or "-1.0" + ) # Operational-only offset for the pre-shake cup-holder re-grasp. # This intentionally does not modify calibration.yaml and is separate from the # cup-holder placement offset so lowering the shake pickup does not push the @@ -3498,6 +3509,8 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"SERVICE_PREFIX={shlex.quote(service_prefix)} " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " + "LID_ROS_LOCALHOST_ONLY=${LID_ROS_LOCALHOST_ONLY:-0} " + "LID_TCP_GRASP_OFFSET_Z_M=${LID_TCP_GRASP_OFFSET_Z_M:--0.032} " "MOVE_TO_LID_VIEW_POSE=true " f"bash {shlex.quote(str(direct_script))}" ) @@ -3691,6 +3704,11 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe or os.environ.get("CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M") or "-0.010" ).strip() + cup_holder_rz_offset_deg = str( + payload.get("cup_holder_rz_offset_deg") + or os.environ.get("CUP_HOLDER_RZ_OFFSET_DEG") + or "-1.0" + ).strip() return ( f"cd {ROOT} && {ROS_SETUP} && python3 tools/run/place_side_grip_cup_in_holder.py " f"--service-prefix {service_prefix} " @@ -3702,6 +3720,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "--approach-velocity 80.0 --approach-acceleration 20.0 " f"--place-final-y-offset-m {shlex.quote(place_final_y_offset_m)} " f"--place-final-z-offset-m {shlex.quote(place_final_z_offset_m)} " + f"--rz-offset-deg {shlex.quote(cup_holder_rz_offset_deg)} " "--place-velocity 80.0 --place-acceleration 10.0 " "--retreat-velocity 80.0 --retreat-acceleration 16.0 " "--timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 " @@ -3720,13 +3739,13 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "python3 tools/run/pick_from_cup_holder_side_grip.py " f"--service-prefix {service_prefix} " "--config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml " - "--approach-velocity 12.0 --approach-acceleration 16.0 " - "--descend-velocity 6.0 --descend-acceleration 10.0 " - "--lift-velocity 12.0 --lift-acceleration 16.0 " + "--approach-velocity 40.0 --approach-acceleration 40.0 " + "--descend-velocity 40.0 --descend-acceleration 40.0 " + "--lift-velocity 40.0 --lift-acceleration 40.0 " f"--place-final-z-offset-m {shlex.quote(pick_z_offset_m)} " "--timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 " "--ikin-timeout-sec 20.0 --ikin-retries 2 " - "--gripper-grasp-width-m 0.068 --gripper-force-n 35.0 " + "--gripper-grasp-width-m 0.068 --gripper-force-n 25.0 " "--post-grasp-settle-sec 0.8 " "--z-max 0.28 " "--execute --confirm ENABLE_CUP_HOLDER_PICK" @@ -4343,7 +4362,10 @@ def do_GET(self) -> None: "CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M", "-0.010" ), "cup_holder_place_final_z_offset_m": os.environ.get( - "CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M", "-0.030" + "CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M", "-0.040" + ), + "cup_holder_rz_offset_deg": os.environ.get( + "CUP_HOLDER_RZ_OFFSET_DEG", "-1.0" ), } data = [] diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index e4f5c93..73495ad 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -13,6 +13,7 @@ import argparse import json +import os import re import subprocess import sys @@ -211,6 +212,11 @@ def main() -> int: help="직접 물리 디스펜서 지정: '1,2,2,3' 또는 '1x1,2x2,3x1'") parser.add_argument("--color-map-json", default="", help="패널이 현재 알고 있는 dispenser_id→color JSON. --colors 직접 입력 시 우선 사용") + parser.add_argument( + "--service-prefix", + default=os.environ.get("SERVICE_PREFIX", ""), + help="Doosan direct service namespace. 현재 스택이 /motion/* 루트 서비스를 쓰면 빈 값", + ) parser.add_argument( "--confirm", nargs="?", @@ -223,6 +229,12 @@ def main() -> int: ) parser.add_argument("--execute", action="store_true", help="실제 measured dispenser sequence를 실행") + parser.add_argument( + "--recipe-speed-scale", + type=float, + default=4.0, + help="디스펜서 레시피 사이클의 속도/가속도 배율. 기본 4.0배.", + ) parser.add_argument("--move-velocity", default="80.0") parser.add_argument("--move-acceleration", default="25.0") parser.add_argument("--move-prehold-velocity", default="80.0") @@ -250,6 +262,11 @@ def main() -> int: parser.add_argument("--press-contact-joint-velocity", default="35.0") parser.add_argument("--press-contact-joint-acceleration", default="15.0") parser.add_argument("--press-contact-entry-lift-m", default="0.050") + parser.add_argument( + "--dispenser-1-press-y-offset-m", + default="0.002", + help="1번 디스펜서 press target에만 적용할 Y 보정값(m). 기본 +0.002m.", + ) parser.add_argument( "--press-reset-before-press", action=argparse.BooleanOptionalAction, @@ -257,8 +274,8 @@ def main() -> int: help="컵을 놓은 뒤 CONTACT_ENTRY_LIFT 전에 PRESS_COMMON_PRE/HOME joint waypoint를 경유. 기본 false", ) parser.add_argument("--press-reset-joints-deg", default="0,0,90,0,90,0") - parser.add_argument("--press-reset-joint-velocity", default="80.0") - parser.add_argument("--press-reset-joint-acceleration", default="25.0") + parser.add_argument("--press-reset-joint-velocity", default="26.6666667") + parser.add_argument("--press-reset-joint-acceleration", default="8.33333333") parser.add_argument("--press-depth-m", default="0.070") parser.add_argument( "--press-extra-depth-m", @@ -304,7 +321,7 @@ def main() -> int: ) parser.add_argument("--regrasp-retreat-x-m", default="-0.080") parser.add_argument("--regrasp-retreat-y-m", default="0.0") - parser.add_argument("--post-press-safe-lift-z-m", default="0.470") + parser.add_argument("--post-press-safe-lift-z-m", default="0.350") parser.add_argument( "--start-safe-lift-z-m", default="0.15", @@ -352,10 +369,26 @@ def main() -> int: parser.add_argument("--final-regrasp-extra-x-offset-m", default="0.000") parser.add_argument("--skip-initial-move-release", action="store_true", help="복구 모드: 컵이 이미 첫 디스펜서 front-hold에 놓여 있다고 가정하고 press부터 시작") + parser.add_argument( + "--resume", + action=argparse.BooleanOptionalAction, + default=False, + help="명시 복구 모드에서만 디스펜서 resume_state를 읽음. 기본 false.", + ) + parser.add_argument( + "--resume-state-file", + default="", + help="run_measured_dispenser_recipe_sequence.py에 전달할 디스펜서 resume JSON 경로", + ) + parser.add_argument( + "--clear-resume-state", + action="store_true", + help="이번 실행 시작 전에 디스펜서 resume JSON을 삭제", + ) parser.add_argument("--final-regrasp-extra-y-offset-m", default="0.0") parser.add_argument("--final-regrasp-extra-z-offset-m", default="0.0") parser.add_argument("--final-regrasp-grasp-width-m", default="0.068") - parser.add_argument("--final-regrasp-force-n", default="35.0") + parser.add_argument("--final-regrasp-force-n", default="25.0") parser.add_argument( "--allow-tcp-set-failure", action="store_true", @@ -370,9 +403,14 @@ def main() -> int: default=True, help="마지막 디스펜서 처리 후 컵홀더에 컵을 놓음", ) - parser.add_argument("--cup-holder-place-final-z-offset-m", default="-0.030") + parser.add_argument("--cup-holder-place-final-z-offset-m", default="-0.040") parser.add_argument("--cup-holder-place-final-y-offset-m", default="-0.010") - parser.add_argument("--cup-holder-z-min-m", default="0.08", + parser.add_argument( + "--cup-holder-rz-offset-deg", + default="-1.0", + help="컵홀더 이동 전 구간의 RZ 자세 보정값. calibration.yaml은 수정하지 않음.", + ) + parser.add_argument("--cup-holder-z-min-m", default="0.06", help="컵홀더 place 목표 z 안전 하한. place z offset을 크게 낮출 때 함께 내려야 함") parser.add_argument("--cup-holder-approach-velocity", default="80.0") parser.add_argument("--cup-holder-approach-acceleration", default="20.0") @@ -396,21 +434,30 @@ def main() -> int: if args.execute and not args.confirm: print(f"[BLOCKED] --execute requires --confirm ({CONFIRM_PHRASE})", file=sys.stderr) return 2 + if args.recipe_speed_scale <= 0.0: + parser.error("--recipe-speed-scale must be > 0") + + def scaled_motion(value: str) -> str: + return f"{float(value) * args.recipe_speed_scale:.6g}" + + def scaled_motion_capped(value: str, cap: float) -> str: + return f"{min(float(value) * args.recipe_speed_scale, cap):.6g}" sequence_extra_args = [ - "--move-velocity", str(args.move_velocity), - "--move-acceleration", str(args.move_acceleration), - "--move-prehold-velocity", str(args.move_prehold_velocity), - "--move-prehold-acceleration", str(args.move_prehold_acceleration), - "--pick-approach-velocity", str(args.pick_approach_velocity), - "--pick-approach-acceleration", str(args.pick_approach_acceleration), - "--pick-lift-velocity", str(args.pick_lift_velocity), - "--pick-lift-acceleration", str(args.pick_lift_acceleration), - "--regrasp-approach-velocity", str(args.regrasp_approach_velocity), - "--regrasp-approach-acceleration", str(args.regrasp_approach_acceleration), + "--service-prefix", str(args.service_prefix), + "--move-velocity", scaled_motion(args.move_velocity), + "--move-acceleration", scaled_motion(args.move_acceleration), + "--move-prehold-velocity", scaled_motion(args.move_prehold_velocity), + "--move-prehold-acceleration", scaled_motion(args.move_prehold_acceleration), + "--pick-approach-velocity", scaled_motion(args.pick_approach_velocity), + "--pick-approach-acceleration", scaled_motion(args.pick_approach_acceleration), + "--pick-lift-velocity", scaled_motion(args.pick_lift_velocity), + "--pick-lift-acceleration", scaled_motion(args.pick_lift_acceleration), + "--regrasp-approach-velocity", scaled_motion(args.regrasp_approach_velocity), + "--regrasp-approach-acceleration", scaled_motion(args.regrasp_approach_acceleration), "--regrasp-reset-joints-deg", str(args.regrasp_reset_joints_deg), - "--regrasp-reset-joint-velocity", str(args.regrasp_reset_joint_velocity), - "--regrasp-reset-joint-acceleration", str(args.regrasp_reset_joint_acceleration), + "--regrasp-reset-joint-velocity", scaled_motion(args.regrasp_reset_joint_velocity), + "--regrasp-reset-joint-acceleration", scaled_motion(args.regrasp_reset_joint_acceleration), "--press-min-transit-z-m", str(args.press_min_transit_z_m), "--press-pre-lift-m", str(args.press_pre_lift_m), "--press-transit-height-m", str(args.press_transit_height_m), @@ -439,16 +486,17 @@ def main() -> int: "--final-regrasp-extra-z-offset-m", str(args.final_regrasp_extra_z_offset_m), "--final-regrasp-grasp-width-m", str(args.final_regrasp_grasp_width_m), "--final-regrasp-force-n", str(args.final_regrasp_force_n), - "--press-line-velocity", str(args.press_line_velocity), - "--press-line-acceleration", str(args.press_line_acceleration), - "--press-travel-velocity", str(args.press_travel_velocity), - "--press-travel-acceleration", str(args.press_travel_acceleration), - "--press-contact-joint-velocity", str(args.press_contact_joint_velocity), - "--press-contact-joint-acceleration", str(args.press_contact_joint_acceleration), + "--press-line-velocity", scaled_motion(args.press_line_velocity), + "--press-line-acceleration", scaled_motion(args.press_line_acceleration), + "--press-travel-velocity", scaled_motion(args.press_travel_velocity), + "--press-travel-acceleration", scaled_motion(args.press_travel_acceleration), + "--press-contact-joint-velocity", scaled_motion(args.press_contact_joint_velocity), + "--press-contact-joint-acceleration", scaled_motion(args.press_contact_joint_acceleration), "--press-contact-entry-lift-m", str(args.press_contact_entry_lift_m), + "--dispenser-1-press-y-offset-m", str(args.dispenser_1_press_y_offset_m), "--press-reset-joints-deg", str(args.press_reset_joints_deg), - "--press-reset-joint-velocity", str(args.press_reset_joint_velocity), - "--press-reset-joint-acceleration", str(args.press_reset_joint_acceleration), + "--press-reset-joint-velocity", scaled_motion_capped(args.press_reset_joint_velocity, 80.0), + "--press-reset-joint-acceleration", scaled_motion_capped(args.press_reset_joint_acceleration, 25.0), "--press-depth-m", str(args.press_depth_m), "--press-extra-depth-m", str(args.press_extra_depth_m), "--press-lock-contact-joints", str(args.press_lock_contact_joints), @@ -456,13 +504,14 @@ def main() -> int: "--gripper-settle-seconds", str(args.gripper_settle_seconds), "--cup-holder-place-final-z-offset-m", str(args.cup_holder_place_final_z_offset_m), "--cup-holder-place-final-y-offset-m", str(args.cup_holder_place_final_y_offset_m), + "--cup-holder-rz-offset-deg", str(args.cup_holder_rz_offset_deg), "--cup-holder-z-min-m", str(args.cup_holder_z_min_m), - "--cup-holder-approach-velocity", str(args.cup_holder_approach_velocity), - "--cup-holder-approach-acceleration", str(args.cup_holder_approach_acceleration), - "--cup-holder-place-velocity", str(args.cup_holder_place_velocity), - "--cup-holder-place-acceleration", str(args.cup_holder_place_acceleration), - "--cup-holder-retreat-velocity", str(args.cup_holder_retreat_velocity), - "--cup-holder-retreat-acceleration", str(args.cup_holder_retreat_acceleration), + "--cup-holder-approach-velocity", scaled_motion(args.cup_holder_approach_velocity), + "--cup-holder-approach-acceleration", scaled_motion(args.cup_holder_approach_acceleration), + "--cup-holder-place-velocity", scaled_motion(args.cup_holder_place_velocity), + "--cup-holder-place-acceleration", scaled_motion(args.cup_holder_place_acceleration), + "--cup-holder-retreat-velocity", scaled_motion(args.cup_holder_retreat_velocity), + "--cup-holder-retreat-acceleration", scaled_motion(args.cup_holder_retreat_acceleration), "--cup-holder-timeout-sec", str(args.cup_holder_timeout_sec), "--cup-holder-target-tolerance-mm", str(args.cup_holder_target_tolerance_mm), "--wait-service-sec", str(args.wait_service_sec), @@ -514,6 +563,13 @@ def main() -> int: sequence_extra_args.append("--allow-tcp-set-failure") if args.force_cartesian_press: sequence_extra_args.append("--force-cartesian-press") + sequence_extra_args.append("--resume" if args.resume else "--no-resume") + if str(args.resume_state_file).strip(): + sequence_extra_args += ["--resume-state-file", str(args.resume_state_file).strip()] + if args.clear_resume_state: + sequence_extra_args.append("--clear-resume-state") + + print(f"[run_color_recipe] 속도 배율: {args.recipe_speed_scale:.2f}x") direct_dispenser_ids = args.dispenser_ids.strip() if direct_dispenser_ids: diff --git a/tools/run/run_color_scan_stage.sh b/tools/run/run_color_scan_stage.sh index 46013d2..f023ca8 100755 --- a/tools/run/run_color_scan_stage.sh +++ b/tools/run/run_color_scan_stage.sh @@ -2,5 +2,59 @@ # 색상 스캔 단계: color_scan_pose(joints 0,10,32,0,100,90)로 이동한 뒤 디스펜서 색상을 스캔한다. # dispenser_color_scan_ros.sh가 outputs/dispenser_color_map.json을 새로 만들어야 # run_color_recipe_sequence.py가 진행되므로, 이 단계는 레시피 전에 반드시 성공해야 한다. +set -eo pipefail -cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 tools/run/direct_movej_joints.py --service-prefix dsr01 --j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90 --velocity 30 --acceleration 30 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ && tools/run/dispenser_color_scan_ros.sh +cd /home/ssu/Azas +source /opt/ros/humble/setup.bash +mkdir -p /tmp/azas_ros_logs +export ROS_LOG_DIR=/tmp/azas_ros_logs +export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} +export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-1} +export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} +if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then + source /home/ssu/ws_moveit/install/setup.bash +fi +if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then + source /home/ssu/ros2_ws/install/setup.bash +fi +if [ -f /home/ssu/Azas/install/setup.bash ]; then + source /home/ssu/Azas/install/setup.bash +else + source /home/ssu/Azas/install/local_setup.bash +fi +export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} + +COLOR_TOPIC="${COLOR_TOPIC:-/camera/camera/color/image_raw}" +CAMERA_READY_TIMEOUT_SEC="${CAMERA_READY_TIMEOUT_SEC:-8}" +if ! timeout "${CAMERA_READY_TIMEOUT_SEC}s" ros2 topic echo --no-daemon --once --qos-reliability best_effort "${COLOR_TOPIC}" >/tmp/azas_color_scan_camera_check.txt 2>&1; then + echo "[Azas][FAIL] color_scan camera preflight failed: no frame from ${COLOR_TOPIC} within ${CAMERA_READY_TIMEOUT_SEC}s" >&2 + echo "[Azas][FAIL] Ensure RealSense publishes ${COLOR_TOPIC} with ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}, then retry." >&2 + timeout 3s ros2 topic info --no-daemon -v "${COLOR_TOPIC}" 2>&1 | sed 's/^/[Azas][camera_info] /' >&2 || true + sed 's/^/[Azas][camera_check] /' /tmp/azas_color_scan_camera_check.txt >&2 || true + exit 1 +fi + +SERVICE_PREFIX="${SERVICE_PREFIX:-auto}" +if [[ "${SERVICE_PREFIX}" == "auto" ]]; then + SERVICE_PREFIX="" + if timeout 3s ros2 service list --no-daemon >/tmp/azas_color_scan_services.txt 2>/tmp/azas_color_scan_services.err; then + if grep -qx "/motion/move_joint" /tmp/azas_color_scan_services.txt; then + SERVICE_PREFIX="" + elif grep -qx "/dsr01/motion/move_joint" /tmp/azas_color_scan_services.txt; then + SERVICE_PREFIX="dsr01" + fi + fi +fi +if [[ -n "${SERVICE_PREFIX}" ]]; then + echo "[Azas] color_scan motion service_prefix=${SERVICE_PREFIX}" +else + echo "[Azas] color_scan motion service_prefix=" +fi + +python3 tools/run/direct_movej_joints.py \ + --service-prefix "${SERVICE_PREFIX}" \ + --j1 0 --j2 10 --j3 32 --j4 0 --j5 100 --j6 90 \ + --velocity 30 --acceleration 30 \ + --timeout-sec 60 --motion-timeout-sec 120 \ + --execute --confirm ENABLE_DIRECT_MOVEJ +tools/run/dispenser_color_scan_ros.sh diff --git a/tools/run/run_holder_pick_then_shake_chain.sh b/tools/run/run_holder_pick_then_shake_chain.sh new file mode 100644 index 0000000..7e003a2 --- /dev/null +++ b/tools/run/run_holder_pick_then_shake_chain.sh @@ -0,0 +1,89 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Resume helper for the post-lid phase. This intentionally reuses the measured +# cup-holder pickup and rule-based shake path instead of introducing new poses. + +ROOT="${ROOT:-/home/ssu/Azas}" +SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +SKIP_CUP_HOLDER_PICK="${SKIP_CUP_HOLDER_PICK:-false}" +ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" +FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" + +cd "${ROOT}" + +set +u +source /opt/ros/humble/setup.bash +if [[ -f /home/ssu/ws_moveit/install/setup.bash ]]; then + source /home/ssu/ws_moveit/install/setup.bash +fi +if [[ -f /home/ssu/ros2_ws/install/setup.bash ]]; then + source /home/ssu/ros2_ws/install/setup.bash +fi +if [[ -f "${ROOT}/install/setup.bash" ]]; then + source "${ROOT}/install/setup.bash" +else + source "${ROOT}/install/local_setup.bash" +fi +set -u + +export ROS_DOMAIN_ID ROS_LOCALHOST_ONLY FASTDDS_BUILTIN_TRANSPORTS SERVICE_PREFIX +export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +export PYTHONPATH="${ROOT}/tools/run/python_compat:${PYTHONPATH:-}" +mkdir -p "${ROS_LOG_DIR}" + +echo "[Azas] HOLDER_PICK_THEN_SHAKE START: skip_holder_pick=${SKIP_CUP_HOLDER_PICK}" +echo "[Azas] source=measured cup_holder.side_grip_place and existing shake sequence; no generated cup coordinates" + +SERVICE_PREFIX="${SERVICE_PREFIX}" \ +GRASPED_CUP_TEST_MODE=true \ +SKIP_CUP_HOLDER_PICK="${SKIP_CUP_HOLDER_PICK}" \ +REQUIRE_ROBOT_STANDBY=true \ +SHAKE_CONTROL_MODE=joint \ +SHAKE_CYCLES=3 \ +JOINT_SHAKE_BASE_J1_DEG=0.0 \ +JOINT_SHAKE_BASE_J2_DEG=-35.0 \ +JOINT_SHAKE_BASE_J3_DEG=50.0 \ +JOINT_SHAKE_BASE_J4_DEG=0.0 \ +JOINT_SHAKE_BASE_J5_DEG=70.0 \ +JOINT_SHAKE_BASE_J6_DEG=0.0 \ +JOINT_SHAKE_J3_AMPLITUDE_DEG=0.0 \ +JOINT_SHAKE_J4_AMPLITUDE_DEG=18.0 \ +JOINT_SHAKE_J5_AMPLITUDE_DEG=20.0 \ +JOINT_SHAKE_J6_AMPLITUDE_DEG=24.0 \ +JOINT_SHAKE_J1_MIN_DEG=-20.0 \ +JOINT_SHAKE_J1_MAX_DEG=5.0 \ +JOINT_SHAKE_J2_MIN_DEG=-80.0 \ +JOINT_SHAKE_J2_MAX_DEG=5.0 \ +JOINT_SHAKE_J3_MIN_DEG=0.0 \ +JOINT_SHAKE_J3_MAX_DEG=135.0 \ +JOINT_SHAKE_MAX_SINGLE_DELTA_DEG=75.0 \ +ENFORCE_WRIST_JOINT_LIMITS=false \ +WRIST_MIN_DEG=-135.0 \ +WRIST_MAX_DEG=135.0 \ +JOINT5_MIN_DEG=40.0 \ +JOINT5_MAX_DEG=100.0 \ +APPROACH_JOINT_VELOCITY=18.0 \ +APPROACH_JOINT_ACCELERATION=22.0 \ +APPROACH_JOINT_TIME=2.6 \ +SHAKE_JOINT_VELOCITY=90.0 \ +SHAKE_JOINT_ACCELERATION=120.0 \ +SHAKE_JOINT_TIME=0.0 \ +JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=130.0 \ +VERIFY_JOINT_TARGETS=true \ +JOINT_TARGET_TOLERANCE_DEG=8.0 \ +JOINT_TARGET_WAIT_EXTRA_SEC=3.0 \ +JOINT_TARGET_POLL_SEC=0.05 \ +REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true \ +REAL_ROBOT_MOTION_CONFIRM=ENABLE_REAL_ROBOT_MOTION \ +bash tools/run/run_rule_based_shake_real.sh + +echo "[Azas] SHAKE DONE: returning to camera pose with cup grasped." +python3 tools/run/direct_movej_joints.py \ + --service-prefix "${SERVICE_PREFIX}" \ + --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 \ + --velocity 15 --acceleration 15 \ + --j5-min-deg -150 --j5-max-deg 150 \ + --timeout-sec 60 --motion-timeout-sec 120 \ + --execute --confirm ENABLE_DIRECT_MOVEJ diff --git a/tools/run/run_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh index 1db5430..078922d 100755 --- a/tools/run/run_kang_lid_grip_close_direct.sh +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -11,9 +11,11 @@ ARUCO_MARKER_ID="${ARUCO_MARKER_ID:-14}" ARUCO_FALLBACK_MARKERS="${ARUCO_FALLBACK_MARKERS:-}" ARUCO_MARKER_LENGTH_M="${ARUCO_MARKER_LENGTH_M:-0.03}" ROS_DOMAIN_ID="${LID_ROS_DOMAIN_ID:-${ROS_DOMAIN_ID:-9}}" -ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +ROS_LOCALHOST_ONLY="${LID_ROS_LOCALHOST_ONLY:-${ROS_LOCALHOST_ONLY:-1}}" FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" MOVE_TO_LID_VIEW_POSE="${MOVE_TO_LID_VIEW_POSE:-false}" +LID_TCP_GRASP_OFFSET_Z_M="${LID_TCP_GRASP_OFFSET_Z_M:--0.032}" +LID_MIN_GRASP_Z_M="${LID_MIN_GRASP_Z_M:-0.020}" cd "${ROOT}" @@ -46,6 +48,7 @@ echo "[Azas] OpenCV window: confirm lid ArUco, then press p. Quit with q/Esc." echo "[Azas] service_prefix=${SERVICE_PREFIX} DISPLAY=${DISPLAY} XAUTHORITY=${XAUTHORITY}" echo "[Azas] ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY} FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS}" echo "[Azas] aruco=${ARUCO_DICTIONARY}:${ARUCO_MARKER_ID} fallback=${ARUCO_FALLBACK_MARKERS} length_m=${ARUCO_MARKER_LENGTH_M}" +echo "[Azas] lid grasp z: tcp_offset=${LID_TCP_GRASP_OFFSET_Z_M} min=${LID_MIN_GRASP_Z_M}" echo "[Azas] orientation: use_j6_yaw_for_pick=true (pick_j6_yaw_axis=y sign=-1.0 offset=1.2deg), preseat=j6_step_wiggle" if [[ ! -f "${MODEL_PATH}" ]]; then @@ -57,7 +60,7 @@ if [[ "${MOVE_TO_LID_VIEW_POSE}" == "true" ]]; then python3 "${ROOT}/tools/run/direct_movej_joints.py" \ --service-prefix "${SERVICE_PREFIX}" \ --j1 3.0 --j2 -20.0 --j3 52.0 --j4 -9.0 --j5 125.0 --j6 90.0 \ - --velocity 10 --acceleration 10 \ + --velocity 40 --acceleration 40 \ --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 \ --execute --confirm ENABLE_DIRECT_MOVEJ fi @@ -89,11 +92,11 @@ launch_args=( approach_lid_with_movej:=false approach_movej_velocity:=20.0 approach_movej_acceleration:=20.0 \ lid_overhead_approach_enabled:=false lid_overhead_min_z_m:=0.260 \ rx:=108.41 ry:=-176.32 rz:=175.98 offset_axis:=base_z surface_offset_m:=0.0 \ - tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:=-0.040 min_grasp_z_m:=0.025 \ + tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:="${LID_TCP_GRASP_OFFSET_Z_M}" min_grasp_z_m:="${LID_MIN_GRASP_Z_M}" \ approach_offset_m:=0.08 min_approach_z_m:=0.0 lift_offset_m:=0.10 settle_seconds_before_grasp:=0.5 hold_seconds_after_grasp:=3.0 \ line_velocity:=30.0 line_acceleration:=10.0 move_timeout_sec:=90.0 \ enable_gripper_service_calls:=true gripper_set_service:=/jarvis/rg2/set_width \ - gripper_preopen_width_m:=0.110 gripper_grasp_width_m:=0.020 gripper_force_n:=12.0 \ + gripper_preopen_width_m:=0.110 gripper_grasp_width_m:=0.020 gripper_force_n:=16.0 \ continue_after_gripper_grasp_failure:=true gripper_grasp_failure_wait_sec:=2.0 \ enable_lid_twist_after_grasp:=true \ lid_twist_target_x_m:=0.422959106 lid_twist_target_y_m:=0.223224869 lid_twist_target_z_m:=0.166827988 \ diff --git a/tools/run/run_lid_close_then_shake_chain.sh b/tools/run/run_lid_close_then_shake_chain.sh index cffcb9d..2825f50 100755 --- a/tools/run/run_lid_close_then_shake_chain.sh +++ b/tools/run/run_lid_close_then_shake_chain.sh @@ -3,4 +3,11 @@ # robot_pipeline_control_server.py chain_shake_after_lid_command()가 생성하는 패널 체인과 동일한 명령을 # auto_cup_flow_router가 직접 실행할 수 있도록 스크립트로 고정한 것이다. -( cd /home/ssu/Azas && SERVICE_PREFIX=dsr01 DISPLAY=${DISPLAY:-:0} XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} MOVE_TO_LID_VIEW_POSE=true bash /home/ssu/Azas/tools/run/run_kang_lid_grip_close_direct.sh ) & lid_pid=$!; ( cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 /home/ssu/Azas/tools/run/wait_for_lid_grip_status.py --timeout-sec 900 --success-status motion_sequence_requested ) & wait_pid=$!; while true; do if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; if ! kill -0 ${lid_pid} 2>/dev/null; then wait ${lid_pid}; lid_rc=$?; sleep 1; if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; echo '[Azas] lid_grip_close launch exited before ArUco success status; shake chain blocked.'; kill -TERM ${wait_pid} 2>/dev/null || true; wait ${wait_pid} 2>/dev/null || true; if [ ${lid_rc} -eq 0 ]; then exit 1; else exit ${lid_rc}; fi; fi; sleep 1; done; kill -TERM ${lid_pid} 2>/dev/null || true; wait ${lid_pid} 2>/dev/null || true; if [ ${wait_rc} -eq 0 ]; then echo '[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹으로 바로 넘어갑니다.'; echo '[Azas] auto_holder_pick_then_shake=true'; cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && echo '[Azas] SHAKE START: 컵홀더에 놓인 닫힌 컵을 측정 pose로 다시 side-grip 픽업한 뒤 흔듭니다.' && echo '[Azas] 순서: RG2 open -> 컵홀더 retreat 접근 -> holder final pose에서 soft grasp -> holder lift -> 관절 쉐이킹.' && echo '[Azas] 주의: 컵 좌표를 새로 만들지 않고 calibration.yaml cup_holder.side_grip_place 측정값만 사용합니다.' && python3 tools/run/pick_from_cup_holder_side_grip.py --service-prefix dsr01 --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml --approach-velocity 12.0 --approach-acceleration 16.0 --descend-velocity 6.0 --descend-acceleration 10.0 --lift-velocity 12.0 --lift-acceleration 16.0 --place-final-z-offset-m -0.020 --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 --ikin-timeout-sec 20.0 --ikin-retries 2 --gripper-grasp-width-m 0.068 --gripper-force-n 35.0 --post-grasp-settle-sec 0.8 --z-max 0.28 --execute --confirm ENABLE_CUP_HOLDER_PICK && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=remove_world -p object_id:=tumbler_in_holder -p dispenser_id:=1 -p publish_once:=true && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=attach -p object_id:=carried_tumbler -p dispenser_id:=1 -p publish_once:=true && SERVICE_PREFIX=dsr01 GRASPED_CUP_TEST_MODE=true SKIP_CUP_HOLDER_PICK=true REQUIRE_ROBOT_STANDBY=true SHAKE_CONTROL_MODE=joint SHAKE_CYCLES=3 JOINT_SHAKE_BASE_J1_DEG=0.0 JOINT_SHAKE_BASE_J2_DEG=-35.0 JOINT_SHAKE_BASE_J3_DEG=50.0 JOINT_SHAKE_BASE_J4_DEG=0.0 JOINT_SHAKE_BASE_J5_DEG=70.0 JOINT_SHAKE_BASE_J6_DEG=0.0 JOINT_SHAKE_J3_AMPLITUDE_DEG=0.0 JOINT_SHAKE_J4_AMPLITUDE_DEG=18.0 JOINT_SHAKE_J5_AMPLITUDE_DEG=20.0 JOINT_SHAKE_J6_AMPLITUDE_DEG=24.0 JOINT_SHAKE_J1_MIN_DEG=-20.0 JOINT_SHAKE_J1_MAX_DEG=5.0 JOINT_SHAKE_J2_MIN_DEG=-80.0 JOINT_SHAKE_J2_MAX_DEG=5.0 JOINT_SHAKE_J3_MIN_DEG=0.0 JOINT_SHAKE_J3_MAX_DEG=135.0 JOINT_SHAKE_MAX_SINGLE_DELTA_DEG=75.0 ENFORCE_WRIST_JOINT_LIMITS=false WRIST_MIN_DEG=-135.0 WRIST_MAX_DEG=135.0 JOINT5_MIN_DEG=40.0 JOINT5_MAX_DEG=100.0 APPROACH_JOINT_VELOCITY=18.0 APPROACH_JOINT_ACCELERATION=22.0 APPROACH_JOINT_TIME=2.6 SHAKE_JOINT_VELOCITY=90.0 SHAKE_JOINT_ACCELERATION=120.0 SHAKE_JOINT_TIME=0.0 JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=130.0 VERIFY_JOINT_TARGETS=true JOINT_TARGET_TOLERANCE_DEG=8.0 JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true REAL_ROBOT_MOTION_CONFIRM=ENABLE_REAL_ROBOT_MOTION tools/run/run_rule_based_shake_real.sh && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && python3 tools/run/direct_movej_joints.py --service-prefix dsr01 --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 --velocity 15 --acceleration 15 --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ; else echo '[Azas] ArUco lid_grip_close 실패/타임아웃 -> 컵홀더 재픽업/쉐이킹을 건너뜁니다.'; exit ${wait_rc}; fi +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" +export ROS_LOCALHOST_ONLY="${CHAIN_ROS_LOCALHOST_ONLY:-${ROS_LOCALHOST_ONLY:-1}}" +export LID_ROS_LOCALHOST_ONLY="${LID_ROS_LOCALHOST_ONLY:-${ROS_LOCALHOST_ONLY}}" +export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" +export LID_TCP_GRASP_OFFSET_Z_M="${LID_TCP_GRASP_OFFSET_Z_M:--0.032}" +export SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" + +( cd /home/ssu/Azas && SERVICE_PREFIX="${SERVICE_PREFIX}" DISPLAY=${DISPLAY:-:0} XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} MOVE_TO_LID_VIEW_POSE=true bash /home/ssu/Azas/tools/run/run_kang_lid_grip_close_direct.sh ) & lid_pid=$!; ( cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 /home/ssu/Azas/tools/run/wait_for_lid_grip_status.py --timeout-sec 900 --success-status motion_sequence_requested ) & wait_pid=$!; while true; do if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; if ! kill -0 ${lid_pid} 2>/dev/null; then wait ${lid_pid}; lid_rc=$?; sleep 1; if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; echo '[Azas] lid_grip_close launch exited before ArUco success status; shake chain blocked.'; kill -TERM ${wait_pid} 2>/dev/null || true; wait ${wait_pid} 2>/dev/null || true; if [ ${lid_rc} -eq 0 ]; then exit 1; else exit ${lid_rc}; fi; fi; sleep 1; done; kill -TERM ${lid_pid} 2>/dev/null || true; wait ${lid_pid} 2>/dev/null || true; if [ ${wait_rc} -eq 0 ]; then echo '[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹으로 바로 넘어갑니다.'; echo '[Azas] auto_holder_pick_then_shake=true'; cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && echo '[Azas] SHAKE START: 컵홀더에 놓인 닫힌 컵을 측정 pose로 다시 side-grip 픽업한 뒤 흔듭니다.' && echo '[Azas] 순서: RG2 open -> 컵홀더 retreat 접근 -> holder final pose에서 soft grasp -> holder lift -> 관절 쉐이킹.' && echo '[Azas] 주의: 컵 좌표를 새로 만들지 않고 calibration.yaml cup_holder.side_grip_place 측정값만 사용합니다.' && python3 tools/run/pick_from_cup_holder_side_grip.py --service-prefix "${SERVICE_PREFIX}" --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml --approach-velocity 40.0 --approach-acceleration 40.0 --descend-velocity 40.0 --descend-acceleration 40.0 --lift-velocity 40.0 --lift-acceleration 40.0 --place-final-z-offset-m -0.020 --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 --ikin-timeout-sec 20.0 --ikin-retries 2 --gripper-grasp-width-m 0.068 --gripper-force-n 25.0 --post-grasp-settle-sec 0.8 --z-max 0.28 --execute --confirm ENABLE_CUP_HOLDER_PICK && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=remove_world -p object_id:=tumbler_in_holder -p dispenser_id:=1 -p publish_once:=true && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=attach -p object_id:=carried_tumbler -p dispenser_id:=1 -p publish_once:=true && SERVICE_PREFIX="${SERVICE_PREFIX}" GRASPED_CUP_TEST_MODE=true SKIP_CUP_HOLDER_PICK=true REQUIRE_ROBOT_STANDBY=true SHAKE_CONTROL_MODE=joint SHAKE_CYCLES=3 JOINT_SHAKE_BASE_J1_DEG=0.0 JOINT_SHAKE_BASE_J2_DEG=-35.0 JOINT_SHAKE_BASE_J3_DEG=50.0 JOINT_SHAKE_BASE_J4_DEG=0.0 JOINT_SHAKE_BASE_J5_DEG=70.0 JOINT_SHAKE_BASE_J6_DEG=0.0 JOINT_SHAKE_J3_AMPLITUDE_DEG=0.0 JOINT_SHAKE_J4_AMPLITUDE_DEG=18.0 JOINT_SHAKE_J5_AMPLITUDE_DEG=20.0 JOINT_SHAKE_J6_AMPLITUDE_DEG=24.0 JOINT_SHAKE_J1_MIN_DEG=-20.0 JOINT_SHAKE_J1_MAX_DEG=5.0 JOINT_SHAKE_J2_MIN_DEG=-80.0 JOINT_SHAKE_J2_MAX_DEG=5.0 JOINT_SHAKE_J3_MIN_DEG=0.0 JOINT_SHAKE_J3_MAX_DEG=135.0 JOINT_SHAKE_MAX_SINGLE_DELTA_DEG=75.0 ENFORCE_WRIST_JOINT_LIMITS=false WRIST_MIN_DEG=-135.0 WRIST_MAX_DEG=135.0 JOINT5_MIN_DEG=40.0 JOINT5_MAX_DEG=100.0 APPROACH_JOINT_VELOCITY=18.0 APPROACH_JOINT_ACCELERATION=22.0 APPROACH_JOINT_TIME=2.6 SHAKE_JOINT_VELOCITY=120.0 SHAKE_JOINT_ACCELERATION=160.0 SHAKE_JOINT_TIME=0.0 JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=160.0 VERIFY_JOINT_TARGETS=true JOINT_TARGET_TOLERANCE_DEG=8.0 JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true REAL_ROBOT_MOTION_CONFIRM=ENABLE_REAL_ROBOT_MOTION tools/run/run_rule_based_shake_real.sh && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && python3 tools/run/direct_movej_joints.py --service-prefix "${SERVICE_PREFIX}" --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 --velocity 15 --acceleration 15 --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ; else echo '[Azas] ArUco lid_grip_close 실패/타임아웃 -> 컵홀더 재픽업/쉐이킹을 건너뜁니다.'; exit ${wait_rc}; fi diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index 9e19379..93b78a7 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -538,6 +538,39 @@ def print_dry_run_group_detail(args: argparse.Namespace, dispenser_id: str, pres f"[PLAN] dispenser {dispenser_id}: press Cartesian fallback xyz_m={press_xyz_m} " f"rpy_deg={press_rpy_deg} -> Z overdrive={z_overdrive_mm:.1f}mm x{press_count}" ) + press_y_offset_m = dispenser_press_y_offset_m(args, dispenser_id) + if abs(press_y_offset_m) > 1e-9: + print( + f"[PLAN] dispenser {dispenser_id}: press target Y offset " + f"{press_y_offset_m * 1000.0:+.1f}mm applied at runtime; calibration.yaml unchanged" + ) + + +def dispenser_press_y_offset_m(args: argparse.Namespace, dispenser_id: str) -> float: + if str(dispenser_id) == "1": + return float(args.dispenser_1_press_y_offset_m) + return 0.0 + + +def apply_dispenser_press_y_offset( + args: argparse.Namespace, + dispenser_id: str, + posx: list[float], + *, + label: str, +) -> list[float]: + adjusted = list(posx) + offset_m = dispenser_press_y_offset_m(args, dispenser_id) + if abs(offset_m) <= 1e-9: + return adjusted + before_y = adjusted[1] + adjusted[1] += offset_m * 1000.0 + print( + "[Azas] press target runtime offset: " + f"dispenser={dispenser_id} label={label} y_mm={before_y:.1f}->{adjusted[1]:.1f} " + f"offset={offset_m * 1000.0:+.1f}mm" + ) + return adjusted def group_consecutive_dispenser_ids(dispenser_ids: list[str]) -> list[tuple[str, int]]: @@ -1748,6 +1781,10 @@ def place_cup_in_holder(self) -> None: pre_place = load_cup_holder_target_posx("pre_place") place_final = load_cup_holder_target_posx("place_final") retreat = load_cup_holder_target_posx("retreat") + rz_offset_deg = self.args.cup_holder_rz_offset_deg + if abs(rz_offset_deg) > 1e-9: + for posx in (pre_place, place_final, retreat): + posx[5] += rz_offset_deg place_final[1] += self.args.cup_holder_place_final_y_offset_m * 1000.0 place_final[2] += self.args.cup_holder_place_final_z_offset_m * 1000.0 for label, posx in ( @@ -2439,6 +2476,12 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: contact_joints, label=f"measured PRESS_CONTACT FK for entry lift dispenser {dispenser_id}", ) + contact_fk_posx = apply_dispenser_press_y_offset( + self.args, + dispenser_id, + list(contact_fk_posx[:6]), + label="PRESS_CONTACT_FK", + ) contact_entry_posx = list(contact_fk_posx[:6]) contact_entry_posx[2] += entry_lift_m * 1000.0 self.validate_cartesian_target_z( @@ -2465,6 +2508,11 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: label="CONTACT_ENTRY_LIFT above measured PRESS_CONTACT", ) if self.args.press_contact_use_joint_move: + if abs(dispenser_press_y_offset_m(self.args, dispenser_id)) > 1e-9: + print( + "[WARN] press Y runtime offset is ignored for " + "press_contact_use_joint_move=true because measured joints are commanded directly" + ) self.movej( contact_joints, label="PRESS_CONTACT measured contact joints", @@ -2577,10 +2625,20 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: press_xyz_m, press_rpy_deg = load_press_pose(dispenser_id) joint_space_press = contact_joints is not None if contact_joints is None: - x_mm = press_xyz_m[0] * 1000.0 - y_mm = press_xyz_m[1] * 1000.0 - contact_z = press_xyz_m[2] * 1000.0 - rx, ry, rz = press_rpy_deg + press_posx = apply_dispenser_press_y_offset( + self.args, + dispenser_id, + [ + press_xyz_m[0] * 1000.0, + press_xyz_m[1] * 1000.0, + press_xyz_m[2] * 1000.0, + press_rpy_deg[0], + press_rpy_deg[1], + press_rpy_deg[2], + ], + label="press_pose_xyz_m fallback", + ) + x_mm, y_mm, contact_z, rx, ry, rz = press_posx[:6] print( f"[Azas] dispenser {dispenser_id}: no press contact joints in calibration; " "falling back to press_pose_xyz_m/rpy_deg" @@ -2603,6 +2661,12 @@ def press_dispenser(self, dispenser_id: str, press_count: int) -> None: contact_joints, label=f"measured PRESS_CONTACT FK dispenser {dispenser_id}", ) + contact_fk_posx = apply_dispenser_press_y_offset( + self.args, + dispenser_id, + list(contact_fk_posx[:6]), + label="fallback PRESS_CONTACT_FK", + ) x_mm, y_mm, contact_z, rx, ry, rz = contact_fk_posx[:6] if joint_space_press: # Fallback for older calibration where PRESS_CONTACT is the only @@ -3442,10 +3506,10 @@ def parse_args() -> argparse.Namespace: default=0.500, help="Minimum absolute TCP Z before moving from cup release toward dispenser press joints.", ) - parser.add_argument("--press-line-velocity", type=float, default=25.0) - parser.add_argument("--press-line-acceleration", type=float, default=10.0) - parser.add_argument("--press-travel-velocity", type=float, default=40.0) - parser.add_argument("--press-travel-acceleration", type=float, default=20.0) + parser.add_argument("--press-line-velocity", type=float, default=35.0) + parser.add_argument("--press-line-acceleration", type=float, default=30.0) + parser.add_argument("--press-travel-velocity", type=float, default=60.0) + parser.add_argument("--press-travel-acceleration", type=float, default=50.0) parser.add_argument("--press-timeout-sec", type=float, default=120.0) parser.add_argument("--press-hold-seconds", type=float, default=0.25) parser.add_argument("--press-gripper-close-width-m", type=float, default=0.0) @@ -3470,8 +3534,8 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--press-reset-joint-acceleration", type=float, default=25.0) parser.add_argument("--press-pre-joint-velocity", type=float, default=80.0) parser.add_argument("--press-pre-joint-acceleration", type=float, default=25.0) - parser.add_argument("--press-contact-joint-velocity", type=float, default=35.0) - parser.add_argument("--press-contact-joint-acceleration", type=float, default=15.0) + parser.add_argument("--press-contact-joint-velocity", type=float, default=50.0) + parser.add_argument("--press-contact-joint-acceleration", type=float, default=40.0) parser.add_argument( "--press-contact-use-joint-move", action=argparse.BooleanOptionalAction, @@ -3490,6 +3554,15 @@ def parse_args() -> argparse.Namespace: "Default stays in the 50-80mm hardware-safe range." ), ) + parser.add_argument( + "--dispenser-1-press-y-offset-m", + type=float, + default=0.002, + help=( + "Runtime Y offset applied only to dispenser 1 press Cartesian targets. " + "Default +0.002m; calibration.yaml measured values are not modified." + ), + ) parser.add_argument( "--skip-measured-press-pre", action=argparse.BooleanOptionalAction, @@ -3673,8 +3746,14 @@ def parse_args() -> argparse.Namespace: default=True, help="After the final dispenser re-grasp, place the held cup into calibration.yaml cup_holder.side_grip_place.", ) - parser.add_argument("--cup-holder-place-final-z-offset-m", type=float, default=-0.030) + parser.add_argument("--cup-holder-place-final-z-offset-m", type=float, default=-0.040) parser.add_argument("--cup-holder-place-final-y-offset-m", type=float, default=-0.010) + parser.add_argument( + "--cup-holder-rz-offset-deg", + type=float, + default=0.0, + help="Add this RZ offset to all measured cup-holder side-grip poses without editing calibration.yaml.", + ) parser.add_argument("--cup-holder-approach-velocity", type=float, default=80.0) parser.add_argument("--cup-holder-approach-acceleration", type=float, default=20.0) parser.add_argument("--cup-holder-place-velocity", type=float, default=80.0) @@ -3687,7 +3766,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument("--cup-holder-x-max-m", type=float, default=0.50) parser.add_argument("--cup-holder-y-min-m", type=float, default=0.15) parser.add_argument("--cup-holder-y-max-m", type=float, default=0.30) - parser.add_argument("--cup-holder-z-min-m", type=float, default=0.08) + parser.add_argument("--cup-holder-z-min-m", type=float, default=0.06) parser.add_argument("--cup-holder-z-max-m", type=float, default=0.28) parser.add_argument( "--gripper-settle-seconds", @@ -3772,10 +3851,10 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--resume", action=argparse.BooleanOptionalAction, - default=True, + default=False, help=( - "Automatically resume from --resume-state-file when it contains the same unfinished " - "recipe. Use --no-resume to ignore a completed/stale state file." + "Resume from --resume-state-file when it contains the same unfinished recipe. " + "Default false so a new order cannot silently skip move/release from stale state." ), ) parser.add_argument( @@ -3887,7 +3966,8 @@ def main() -> int: print( f"[Azas] cup_holder_after_sequence={str(args.place_cup_holder_after_sequence).lower()} " f"place_final_y_offset_m={args.cup_holder_place_final_y_offset_m:.3f} " - f"place_final_z_offset_m={args.cup_holder_place_final_z_offset_m:.3f}" + f"place_final_z_offset_m={args.cup_holder_place_final_z_offset_m:.3f} " + f"rz_offset_deg={args.cup_holder_rz_offset_deg:.1f}" ) try: resume_tracker = RecipeResumeTracker(args, dispenser_ids, grouped_dispenser_ids) @@ -4102,6 +4182,8 @@ def main() -> int: f"{args.cup_holder_place_final_z_offset_m:.6f}", "--place-final-y-offset-m", f"{args.cup_holder_place_final_y_offset_m:.6f}", + "--rz-offset-deg", + f"{args.cup_holder_rz_offset_deg:.6f}", "--timeout-sec", f"{args.cup_holder_timeout_sec:.6f}", "--target-tolerance-mm", diff --git a/tools/run/run_rule_based_shake_real.sh b/tools/run/run_rule_based_shake_real.sh index b540c09..cf1fa8d 100755 --- a/tools/run/run_rule_based_shake_real.sh +++ b/tools/run/run_rule_based_shake_real.sh @@ -19,7 +19,7 @@ CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M="${CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M:-0.0}" # Negative values lower the final grasp pose; calibration.yaml is not modified. CUP_HOLDER_PICK_Z_OFFSET_M="${CUP_HOLDER_PICK_Z_OFFSET_M:--0.020}" CUP_HOLDER_PICK_WIDTH_M="${CUP_HOLDER_PICK_WIDTH_M:-0.068}" -CUP_HOLDER_PICK_FORCE_N="${CUP_HOLDER_PICK_FORCE_N:-35.0}" +CUP_HOLDER_PICK_FORCE_N="${CUP_HOLDER_PICK_FORCE_N:-25.0}" USE_CURRENT_TCP_AS_SHAKE_CENTER="${USE_CURRENT_TCP_AS_SHAKE_CENTER:-false}" REQUIRE_JOINT_LIMITS="${REQUIRE_JOINT_LIMITS:-true}" REQUIRE_ROBOT_STANDBY="${REQUIRE_ROBOT_STANDBY:-true}" @@ -280,9 +280,9 @@ if [[ "${SKIP_CUP_HOLDER_PICK}" != "true" ]]; then python3 "${ROOT_DIR}/tools/run/pick_from_cup_holder_side_grip.py" \ --service-prefix "${SERVICE_PREFIX}" \ --config "${CUP_HOLDER_PICK_CONFIG}" \ - --approach-velocity 12.0 --approach-acceleration 16.0 \ - --descend-velocity 6.0 --descend-acceleration 10.0 \ - --lift-velocity 12.0 --lift-acceleration 16.0 \ + --approach-velocity 40.0 --approach-acceleration 40.0 \ + --descend-velocity 40.0 --descend-acceleration 40.0 \ + --lift-velocity 40.0 --lift-acceleration 40.0 \ --place-final-z-offset-m "${CUP_HOLDER_PICK_Z_OFFSET_M}" \ --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 \ --ikin-timeout-sec 20.0 --ikin-retries 2 \ diff --git a/tools/run/run_tmux_logic_sequence.sh b/tools/run/run_tmux_logic_sequence.sh index f33f6dc..5719ad9 100755 --- a/tools/run/run_tmux_logic_sequence.sh +++ b/tools/run/run_tmux_logic_sequence.sh @@ -6,6 +6,9 @@ LOG_DIR="${ROOT}/log/tmux_logic" SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" DISPLAY="${DISPLAY:-:0}" XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" +ROS_LOCALHOST_ONLY="${TMUX_LOGIC_ROS_LOCALHOST_ONLY:-0}" +LID_TCP_GRASP_OFFSET_Z_M="${LID_TCP_GRASP_OFFSET_Z_M:--0.032}" +LID_MIN_GRASP_Z_M="${LID_MIN_GRASP_Z_M:-0.020}" mkdir -p "${LOG_DIR}" /tmp/azas_ros_logs cd "${ROOT}" @@ -19,7 +22,7 @@ set -u export ROS_LOG_DIR=/tmp/azas_ros_logs export PYTHONPATH="${ROOT}/tools/run/python_compat:${PYTHONPATH:-}" -export DISPLAY XAUTHORITY +export DISPLAY XAUTHORITY ROS_LOCALHOST_ONLY support_pids=() @@ -197,11 +200,11 @@ run_lid_grip_close() { visual_refine_max_position_std_m:=0.005 visual_refine_apply_xy:=true visual_refine_apply_yaw:=true visual_refine_fallback_to_initial_plan:=true \ enable_hardware:=true hardware_confirm:=ENABLE_REAL_ROBOT_MOTION allow_service_control_without_moveit:=true service_prefix:=/${SERVICE_PREFIX} \ rx:=108.41 ry:=-176.32 rz:=175.98 offset_axis:=base_z surface_offset_m:=0.0 \ - tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:=-0.040 min_grasp_z_m:=0.025 \ + tcp_grasp_offset_x_m:=0.0 tcp_grasp_offset_y_m:=0.0 tcp_grasp_offset_z_m:="${LID_TCP_GRASP_OFFSET_Z_M}" min_grasp_z_m:="${LID_MIN_GRASP_Z_M}" \ approach_offset_m:=0.08 lift_offset_m:=0.10 settle_seconds_before_grasp:=0.5 hold_seconds_after_grasp:=3.0 \ line_velocity:=30.0 line_acceleration:=10.0 move_timeout_sec:=90.0 \ enable_gripper_service_calls:=true gripper_set_service:=/jarvis/rg2/set_width \ - gripper_preopen_width_m:=0.110 gripper_grasp_width_m:=0.020 gripper_force_n:=12.0 \ + gripper_preopen_width_m:=0.110 gripper_grasp_width_m:=0.020 gripper_force_n:=16.0 \ continue_after_gripper_grasp_failure:=true gripper_grasp_failure_wait_sec:=2.0 \ enable_lid_twist_after_grasp:=true \ lid_twist_target_x_m:=0.422959106 lid_twist_target_y_m:=0.223224869 lid_twist_target_z_m:=0.166827988 \ diff --git a/tools/run/run_voice_auto_cup_flow.sh b/tools/run/run_voice_auto_cup_flow.sh index d163002..5c07d41 100755 --- a/tools/run/run_voice_auto_cup_flow.sh +++ b/tools/run/run_voice_auto_cup_flow.sh @@ -17,6 +17,11 @@ if ! [[ "${RECIPE_COLORS}" =~ ^(red|yellow|green|blue):[0-9]+(,(red|yellow|green fi SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" +MOTION_SERVICE_PREFIX="${MOTION_SERVICE_PREFIX:-auto}" +AUTO_FLOW_RESUME_MODE="${AUTO_FLOW_RESUME_MODE:-normal}" +AUTO_FLOW_RESUME_STATE_FILE="${AUTO_FLOW_RESUME_STATE_FILE:-/home/ssu/Azas/outputs/auto_cup_flow_resume.json}" +AUTO_FLOW_RESUME_EVENTS_FILE="${AUTO_FLOW_RESUME_EVENTS_FILE:-/home/ssu/Azas/outputs/auto_cup_flow_events.jsonl}" +AUTO_FLOW_DISPENSER_RESUME_STATE_FILE="${AUTO_FLOW_DISPENSER_RESUME_STATE_FILE:-/home/ssu/Azas/outputs/measured_dispenser_recipe_resume.json}" ROUTER_CONFIRM="${ROUTER_CONFIRM:-}" if [[ "${ROUTER_CONFIRM}" != "ENABLE_AUTO_CUP_ROUTER" ]]; then echo "[voice_flow] BLOCKED: set ROUTER_CONFIRM=ENABLE_AUTO_CUP_ROUTER to run real motion." >&2 @@ -36,11 +41,16 @@ export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" -echo "[voice_flow] starting full auto cup flow: recipe_colors=${RECIPE_COLORS}" -exec ros2 launch azas_bringup auto_cup_flow_router.launch.py \ +echo "[voice_flow] starting full auto cup flow: recipe_colors=${RECIPE_COLORS} resume_mode=${AUTO_FLOW_RESUME_MODE}" +mkdir -p "${ROS_LOG_DIR:-/tmp/azas_ros_logs}" +VOICE_FLOW_LOG="${ROS_LOG_DIR:-/tmp/azas_ros_logs}/voice_auto_cup_flow_$(date +%Y%m%d_%H%M%S).log" + +set +e +ros2 launch azas_bringup auto_cup_flow_router.launch.py \ enable_real_motion:=true \ router_confirm:=ENABLE_AUTO_CUP_ROUTER \ service_prefix:="${SERVICE_PREFIX}" \ + motion_service_prefix:="${MOTION_SERVICE_PREFIX}" \ moveit_controller_name:=/${SERVICE_PREFIX}/dsr_moveit_controller \ controller_action_name:=/${SERVICE_PREFIX}/dsr_moveit_controller/follow_joint_trajectory \ classifier_path:=/home/ssu/Azas/cup_classifier_best.pth \ @@ -48,4 +58,17 @@ exec ros2 launch azas_bringup auto_cup_flow_router.launch.py \ route_hold_sec:=2.0 \ route_stable_required_samples:=5 \ route_stable_min_sec:=0.8 \ - recipe_colors:="${RECIPE_COLORS}" + recipe_colors:="${RECIPE_COLORS}" \ + resume_mode:="${AUTO_FLOW_RESUME_MODE}" \ + resume_state_file:="${AUTO_FLOW_RESUME_STATE_FILE}" \ + resume_events_file:="${AUTO_FLOW_RESUME_EVENTS_FILE}" \ + dispenser_resume_state_file:="${AUTO_FLOW_DISPENSER_RESUME_STATE_FILE}" 2>&1 | tee "${VOICE_FLOW_LOG}" +pipeline_status=("${PIPESTATUS[@]}") +set -e +launch_rc="${pipeline_status[0]}" + +if grep -Eq "\[auto_cup_flow_router-[0-9]+\]: process has died|auto_cup_flow_router.*exit code 1|lid_shake: process exited with code [1-9]" "${VOICE_FLOW_LOG}"; then + echo "[voice_flow] auto cup flow failed; see ${VOICE_FLOW_LOG}" >&2 + exit 1 +fi +exit "${launch_rc}" diff --git a/tools/run/stop_azas_all.sh b/tools/run/stop_azas_all.sh index a5ee890..990c0f8 100755 --- a/tools/run/stop_azas_all.sh +++ b/tools/run/stop_azas_all.sh @@ -3,8 +3,8 @@ set -euo pipefail # Stop the entire Azas field stack in one shot: the azas tmux logic sessions, # every ROS-related process (robot bringup, gripper, camera, relays, MoveIt, -# RViz, perception/preview nodes, stray ros2 CLI zombies), the ros2 daemon, -# and stale FastDDS shared-memory segments left behind by killed nodes. +# RViz, perception/preview/voice nodes, stray ros2 CLI zombies), the ros2 +# daemon, and stale FastDDS shared-memory segments left behind by killed nodes. # # Protected and never killed: the control panel (unless KILL_PANEL=1), the # tmux server itself, and Codex/OMX/Claude agent processes plus this script's @@ -19,10 +19,10 @@ set -euo pipefail DRY_RUN="${DRY_RUN:-0}" KILL_PANEL="${KILL_PANEL:-0}" CLEAN_FASTDDS_SHM="${CLEAN_FASTDDS_SHM:-1}" -SESSIONS="${SESSIONS:-azas-logic azas-rviz-exact}" +SESSIONS="${SESSIONS:-azas-logic azas-rviz-exact azas-voice}" GRACE_SEC="${GRACE_SEC:-6}" -ROS_PATTERN='run_doosan_real_m0609\.sh|dsr_bringup2|run_emulator|/DRCF|ros2_control_node|robot_state_publisher|move_group|rviz2|rg2_trigger|rg2_gripper_node|rs_launch\.py|realsense2_camera_node|joint_state_relay\.py|yolo_cup_pick_node|hand_eye_static_tf_node|static_transform_publisher|link6_gripper_collision_node|measured_dispenser_collision_scene_node|workspace_collision_scene_node|yolo_cup_uprighting|collision_scene_rviz_publisher\.py|publish_color_recipe_sequence_rviz_preview\.py|publish_collision_scene_rviz\.py|lid_sticker_detector_node|lid_grip_planner_node|lid_detection_pose_bridge_node|dispenser_sequence|run_changhyun_side_grip_direct\.sh|run_kang_lid_grip_close_direct\.sh|run_somyeong_cup_uprighting_direct\.sh|run_tmux_logic_sequence\.sh|run_color_recipe_sequence\.py|run_measured_dispenser_recipe_sequence\.py|run_minimal_dispenser_cycle\.py|/opt/ros/humble/bin/ros2 |ros2cli\.daemon' +ROS_PATTERN='run_doosan_real_m0609\.sh|dsr_bringup2|run_emulator|/DRCF|ros2_control_node|robot_state_publisher|move_group|rviz2|rg2_trigger|rg2_gripper_node|rs_launch\.py|realsense2_camera_node|joint_state_relay\.py|yolo_cup_pick_node|hand_eye_static_tf_node|static_transform_publisher|link6_gripper_collision_node|measured_dispenser_collision_scene_node|workspace_collision_scene_node|yolo_cup_uprighting|collision_scene_rviz_publisher\.py|publish_color_recipe_sequence_rviz_preview\.py|publish_collision_scene_rviz\.py|lid_sticker_detector_node|lid_grip_planner_node|lid_detection_pose_bridge_node|dispenser_sequence|azas_voice\.launch\.py|/azas_voice/(recipe_mapper_node|llm_recipe_mapper_node|conversation_manager_node|voice_pipeline_executor_node|voice_dispenser_executor_node|tts_node|voice_screen_node|stt_node)|run_voice_auto_cup_flow\.sh|start_azas_voice_stack\.sh|run_changhyun_side_grip_direct\.sh|run_kang_lid_grip_close_direct\.sh|run_somyeong_cup_uprighting_direct\.sh|run_tmux_logic_sequence\.sh|run_color_recipe_sequence\.py|run_measured_dispenser_recipe_sequence\.py|run_minimal_dispenser_cycle\.py|/opt/ros/humble/bin/ros2 |ros2cli\.daemon' PROTECT_PATTERN='codex|oh-my-codex|omx|claude|bwrap|stop_azas_all\.sh|grep -E|(^|[ /])tmux( |$|:)' if [[ "${KILL_PANEL}" != "1" && "${KILL_PANEL}" != "true" ]]; then diff --git a/tools/run/stop_azas_voice_stack.sh b/tools/run/stop_azas_voice_stack.sh index 7d6879e..3f530be 100755 --- a/tools/run/stop_azas_voice_stack.sh +++ b/tools/run/stop_azas_voice_stack.sh @@ -1,22 +1,53 @@ #!/usr/bin/env bash -# azas-voice tmux 세션(로봇/그리퍼/카메라/음성스택)을 정리한다. +# azas-voice tmux 세션(로봇/그리퍼/카메라/음성스택)과 수동으로 띄운 +# azas_voice 노드를 정리한다. set -euo pipefail SESSION="${SESSION:-azas-voice}" +GRACE_SEC="${GRACE_SEC:-3}" -if ! tmux has-session -t "${SESSION}" >/dev/null 2>&1; then - echo "[Azas] no '${SESSION}' session running." - exit 0 -fi +VOICE_PATTERN='azas_voice\.launch\.py|/azas_voice/(recipe_mapper_node|llm_recipe_mapper_node|conversation_manager_node|voice_pipeline_executor_node|voice_dispenser_executor_node|tts_node|voice_screen_node|stt_node)' +PROTECT_PATTERN='codex|oh-my-codex|omx|stop_azas_voice_stack\.sh|grep -E' -tmux list-panes -s -t "${SESSION}" -F '#{pane_id}' | while read -r pane; do - [[ -n "${pane}" ]] && tmux send-keys -t "${pane}" C-c >/dev/null 2>&1 || true -done +collect_voice_pids() { + ps -eo pid=,stat=,args= | grep -E "${VOICE_PATTERN}" | grep -Ev "${PROTECT_PATTERN}" \ + | while read -r pid stat args; do + [[ "${stat}" == Z* ]] && continue + echo "${pid}" + done +} -for _ in {1..25}; do - pgrep -f 'run_doosan_real_m0609|rg2_trigger.launch.py|rs_launch.py camera_name:=camera|azas_voice.launch.py|auto_cup_flow_router' >/dev/null 2>&1 || break - sleep 0.2 -done +if tmux has-session -t "${SESSION}" >/dev/null 2>&1; then + tmux list-panes -s -t "${SESSION}" -F '#{pane_id}' | while read -r pane; do + [[ -n "${pane}" ]] && tmux send-keys -t "${pane}" C-c >/dev/null 2>&1 || true + done -tmux kill-session -t "${SESSION}" >/dev/null 2>&1 || true -echo "[Azas] '${SESSION}' session stopped." + for _ in {1..25}; do + pgrep -f 'run_doosan_real_m0609|rg2_trigger.launch.py|rs_launch.py camera_name:=camera|azas_voice.launch.py|auto_cup_flow_router' >/dev/null 2>&1 || break + sleep 0.2 + done + + tmux kill-session -t "${SESSION}" >/dev/null 2>&1 || true + echo "[Azas] '${SESSION}' session stopped." +else + echo "[Azas] no '${SESSION}' session running." +fi + +mapfile -t voice_pids < <(collect_voice_pids) +if [[ "${#voice_pids[@]}" -gt 0 ]]; then + echo "[Azas] stopping ${#voice_pids[@]} azas_voice processes" + kill -TERM "${voice_pids[@]}" 2>/dev/null || true + deadline=$((SECONDS + GRACE_SEC)) + while [[ ${SECONDS} -lt ${deadline} ]]; do + mapfile -t voice_pids < <(collect_voice_pids) + [[ "${#voice_pids[@]}" -eq 0 ]] && break + sleep 0.2 + done + mapfile -t voice_pids < <(collect_voice_pids) + if [[ "${#voice_pids[@]}" -gt 0 ]]; then + echo "[Azas] force-killing ${#voice_pids[@]} lingering azas_voice processes" + kill -KILL "${voice_pids[@]}" 2>/dev/null || true + fi +else + echo "[Azas] no stray azas_voice processes found." +fi diff --git a/tools/run/wait_for_lid_grip_status.py b/tools/run/wait_for_lid_grip_status.py index 16c998c..8c2788b 100755 --- a/tools/run/wait_for_lid_grip_status.py +++ b/tools/run/wait_for_lid_grip_status.py @@ -36,14 +36,32 @@ def parse_args() -> argparse.Namespace: default=["failed"], help="status value that means the lid close sequence failed", ) + parser.add_argument( + "--ignore-pretrigger-failures", + action=argparse.BooleanOptionalAction, + default=True, + help=( + "ignore retryable failed statuses before an accepted lid trigger starts; " + "this keeps early p-key/no-pose events from aborting the shake chain" + ), + ) return parser.parse_args() class LidGripStatusWaiter(Node): - def __init__(self, topic: str, success_statuses: set[str], failure_statuses: set[str]): + def __init__( + self, + topic: str, + success_statuses: set[str], + failure_statuses: set[str], + *, + ignore_pretrigger_failures: bool, + ): super().__init__("azas_wait_for_lid_grip_status") self._success_statuses = success_statuses self._failure_statuses = failure_statuses + self._ignore_pretrigger_failures = ignore_pretrigger_failures + self._sequence_started = False self.result_code: int | None = None self.result_text = "" self.create_subscription(String, topic, self._on_status, 10) @@ -57,13 +75,42 @@ def _on_status(self, msg: String) -> None: status = str(payload.get("status", "")).strip() if not status: return - print(f"[Azas] lid_grip_status={status} payload={payload}", flush=True) + if self._marks_sequence_started(status, payload): + self._sequence_started = True if status in self._success_statuses: + print(f"[Azas] lid_grip_status={status} payload={payload}", flush=True) self.result_code = 0 self.result_text = f"success status observed: {status}" elif status in self._failure_statuses: + if self._should_ignore_failure(payload): + print(f"[Azas] lid_grip_status_ignored={status} payload={payload}", flush=True) + return + print(f"[Azas] lid_grip_status={status} payload={payload}", flush=True) self.result_code = 1 self.result_text = f"failure status observed: {status}" + else: + print(f"[Azas] lid_grip_status={status} payload={payload}", flush=True) + + @staticmethod + def _marks_sequence_started(status: str, payload: dict) -> bool: + if status == "trigger_received": + return True + if str(payload.get("request_source") or "") == "p_key": + return True + if payload.get("real_motion") is True: + return True + if payload.get("motion_allowed") is True: + return True + return False + + def _should_ignore_failure(self, payload: dict) -> bool: + if not self._ignore_pretrigger_failures: + return False + if self._sequence_started: + return False + if payload.get("real_motion") is True: + return False + return True def main() -> int: @@ -73,7 +120,12 @@ def main() -> int: timeout_sec = max(float(args.timeout_sec), 0.1) rclpy.init(args=None) - node = LidGripStatusWaiter(args.topic, success_statuses, failure_statuses) + node = LidGripStatusWaiter( + args.topic, + success_statuses, + failure_statuses, + ignore_pretrigger_failures=bool(args.ignore_pretrigger_failures), + ) deadline = time.monotonic() + timeout_sec try: while rclpy.ok() and node.result_code is None and time.monotonic() < deadline: From c578ee0e0ab4515f188387e1d3329edb38eaf202 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Sat, 13 Jun 2026 20:25:35 +0900 Subject: [PATCH 79/88] =?UTF-8?q?=EC=89=90=EC=9D=B4=ED=82=B9=20=EC=A0=84?= =?UTF-8?q?=EA=B9=8C=EC=A7=80=20=ED=85=8C=EC=8A=A4=ED=8A=B8=20=EC=99=84?= =?UTF-8?q?=EB=A3=8C=20=ED=99=95=EC=9D=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 2 +- .../azas_task_manager/auto_cup_flow_router.py | 6 ++--- .../azas_voice/recipe_mapper_node.py | 26 +++++++++++++++++++ .../voice_pipeline_executor_node.py | 8 ++++++ src/azas_voice/launch/azas_voice.launch.py | 24 ++++++++++++++++- src/azas_voice/web/voice.js | 11 +++++++- tools/run/pick_from_cup_holder_side_grip.py | 4 +-- tools/run/run_voice_auto_cup_flow.sh | 2 +- tools/run/start_azas_voice_stack.sh | 6 ++++- 9 files changed, 79 insertions(+), 10 deletions(-) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index 2faffd3..08b0345 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): DeclareLaunchArgument("dispenser_3_cup_pre_extra_x_offset_m", default_value="-0.01"), DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="0.0"), DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.04"), - DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="0.0"), + DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="-0.010"), DeclareLaunchArgument("cup_holder_rz_offset_deg", default_value="-1.0"), DeclareLaunchArgument("cup_holder_z_min_m", default_value="0.06"), DeclareLaunchArgument("lid_shake_after_recipe", default_value="true"), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index b942dff..51f6ff0 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -118,7 +118,7 @@ def __init__(self) -> None: self.declare_parameter("dispenser_3_cup_pre_extra_x_offset_m", -0.01) # 컵홀더에 놓을 때 보정값과 place 목표 z 안전 하한 (필요 시 조정) self.declare_parameter("cup_holder_place_z_offset_m", -0.04) - self.declare_parameter("cup_holder_place_y_offset_m", 0.0) + self.declare_parameter("cup_holder_place_y_offset_m", -0.010) self.declare_parameter("cup_holder_rz_offset_deg", -1.0) self.declare_parameter("cup_holder_z_min_m", 0.06) self.declare_parameter("lid_shake_after_recipe", True) @@ -782,12 +782,12 @@ def _motion_service_prefix(self) -> str: service_prefix = str(self.get_parameter("service_prefix").value or "").strip().strip("/") services = {name for name, _types in self.get_service_names_and_types()} - if "/motion/move_joint" in services: - return "" if service_prefix and f"/{service_prefix}/motion/move_joint" in services: return service_prefix if "/dsr01/motion/move_joint" in services: return "dsr01" + if "/motion/move_joint" in services: + return "" return service_prefix def _open_gripper(self, label: str) -> bool: diff --git a/src/azas_voice/azas_voice/recipe_mapper_node.py b/src/azas_voice/azas_voice/recipe_mapper_node.py index 8bf25e5..fc3a590 100644 --- a/src/azas_voice/azas_voice/recipe_mapper_node.py +++ b/src/azas_voice/azas_voice/recipe_mapper_node.py @@ -1,4 +1,5 @@ import json +import time import rclpy from rclpy.node import Node @@ -20,11 +21,19 @@ def __init__(self): self.declare_parameter("decision_topic", "/azas/voice/recipe_decision") self.declare_parameter("confirmation_topic", "/azas/voice/confirmation") self.declare_parameter("publish_confirmation", True) + self.declare_parameter("duplicate_utterance_window_s", 1.2) stt_topic = self.get_parameter("stt_topic").value decision_topic = self.get_parameter("decision_topic").value confirmation_topic = self.get_parameter("confirmation_topic").value self._publish_confirmation = bool(self.get_parameter("publish_confirmation").value) + self._duplicate_window_s = max( + 0.0, + float(self.get_parameter("duplicate_utterance_window_s").value), + ) + self._last_normalized = "" + self._last_intent = "" + self._last_decision_at = 0.0 self._decision_pub = self.create_publisher(String, decision_topic, 10) self._confirmation_pub = self.create_publisher(String, confirmation_topic, 10) @@ -36,6 +45,23 @@ def __init__(self): def _on_stt(self, msg: String) -> None: decision = parse_recipe_command(msg.data) + now = time.monotonic() + if ( + self._duplicate_window_s > 0.0 + and decision.normalized + and decision.normalized == self._last_normalized + and decision.intent == self._last_intent + and now - self._last_decision_at <= self._duplicate_window_s + ): + self.get_logger().info( + "ignored duplicate STT utterance within " + f"{self._duplicate_window_s:.1f}s: {decision.utterance}" + ) + return + self._last_normalized = decision.normalized + self._last_intent = decision.intent + self._last_decision_at = now + payload = String() payload.data = json.dumps(decision.to_dict(), ensure_ascii=False) self._decision_pub.publish(payload) diff --git a/src/azas_voice/azas_voice/voice_pipeline_executor_node.py b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py index 4c8d30e..ea20e3a 100644 --- a/src/azas_voice/azas_voice/voice_pipeline_executor_node.py +++ b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py @@ -6,6 +6,7 @@ import signal import subprocess import threading +from collections import deque try: import rclpy @@ -340,8 +341,14 @@ def _start_pipeline( def _monitor_pipeline(self, proc: subprocess.Popen[str], recipe_colors: str, resume_mode: str) -> None: last_stage = "" + output_tail: deque[str] = deque(maxlen=20) if proc.stdout is not None: for line in proc.stdout: + line = line.rstrip() + if line: + output_tail.append(line) + if any(marker in line for marker in ("[FAIL]", "[ERROR]", "process exited", "service=")): + self.get_logger().warn(f"flow> {line}") stage = stage_from_line(line) if stage and stage != last_stage: last_stage = stage @@ -359,6 +366,7 @@ def _monitor_pipeline(self, proc: subprocess.Popen[str], recipe_colors: str, res returncode=code, last_stage=last_stage, resume_mode=resume_mode, + output_tail=list(output_tail), ) def _publish_status(self, status: str, **fields: object) -> None: diff --git a/src/azas_voice/launch/azas_voice.launch.py b/src/azas_voice/launch/azas_voice.launch.py index 4f0152e..b3e3f42 100644 --- a/src/azas_voice/launch/azas_voice.launch.py +++ b/src/azas_voice/launch/azas_voice.launch.py @@ -17,6 +17,12 @@ def generate_launch_description(): tts_speech_rate = LaunchConfiguration("tts_speech_rate") tts_startup_prompt = LaunchConfiguration("tts_startup_prompt") stt_topic = LaunchConfiguration("stt_topic") + stt_language = LaunchConfiguration("stt_language") + stt_device_index = LaunchConfiguration("stt_device_index") + stt_energy_threshold = LaunchConfiguration("stt_energy_threshold") + stt_pause_threshold = LaunchConfiguration("stt_pause_threshold") + stt_phrase_time_limit = LaunchConfiguration("stt_phrase_time_limit") + stt_ambient_duration = LaunchConfiguration("stt_ambient_duration") return LaunchDescription( [ @@ -53,6 +59,12 @@ def generate_launch_description(): DeclareLaunchArgument("llm_api_key_env", default_value="OPENAI_API_KEY"), DeclareLaunchArgument("llm_request_timeout_sec", default_value="20.0"), DeclareLaunchArgument("stt_topic", default_value="/stt_result"), + DeclareLaunchArgument("stt_language", default_value="ko-KR"), + DeclareLaunchArgument("stt_device_index", default_value="-1"), + DeclareLaunchArgument("stt_energy_threshold", default_value="300.0"), + DeclareLaunchArgument("stt_pause_threshold", default_value="0.8"), + DeclareLaunchArgument("stt_phrase_time_limit", default_value="5.0"), + DeclareLaunchArgument("stt_ambient_duration", default_value="1.0"), Node( package="azas_voice", executable="recipe_mapper_node", @@ -147,7 +159,17 @@ def generate_launch_description(): executable="stt_node", name="stt_node", output="screen", - parameters=[{"stt_topic": stt_topic}], + parameters=[ + { + "stt_topic": stt_topic, + "language": stt_language, + "device_index": ParameterValue(stt_device_index, value_type=int), + "energy_threshold": ParameterValue(stt_energy_threshold, value_type=float), + "pause_threshold": ParameterValue(stt_pause_threshold, value_type=float), + "phrase_time_limit": ParameterValue(stt_phrase_time_limit, value_type=float), + "ambient_duration": ParameterValue(stt_ambient_duration, value_type=float), + } + ], condition=IfCondition(use_live_stt), ), Node( diff --git a/src/azas_voice/web/voice.js b/src/azas_voice/web/voice.js index 1831078..abd10d5 100644 --- a/src/azas_voice/web/voice.js +++ b/src/azas_voice/web/voice.js @@ -193,6 +193,15 @@ function renderSteps(pipeline) { return activeIndex; } +function pipelineFailureText(pipeline) { + const tail = Array.isArray(pipeline.output_tail) ? pipeline.output_tail : []; + const failureLine = [...tail] + .reverse() + .find((line) => line.includes("[FAIL]") || line.includes("[ERROR]") || line.includes("process has died")); + const text = failureLine || pipeline.reason || (pipeline.status === "blocked" ? "복구 대기" : "제조 중단"); + return text.length > 72 ? `${text.slice(0, 69)}...` : text; +} + function renderRobot(activeIndex, pipeline, hasMenu) { if (!hasMenu) { robotScene.dataset.step = "idle"; @@ -202,7 +211,7 @@ function renderRobot(activeIndex, pipeline, hasMenu) { const status = pipeline.status || ""; if (status === "failed" || status === "blocked") { robotScene.dataset.step = "idle"; - robotStatusText.textContent = status === "blocked" ? "복구 대기" : "제조 중단"; + robotStatusText.textContent = pipelineFailureText(pipeline); return; } if (status === "completed" || activeIndex >= pipelineSteps.length) { diff --git a/tools/run/pick_from_cup_holder_side_grip.py b/tools/run/pick_from_cup_holder_side_grip.py index 03059ae..7ddfaa9 100755 --- a/tools/run/pick_from_cup_holder_side_grip.py +++ b/tools/run/pick_from_cup_holder_side_grip.py @@ -188,8 +188,8 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--place-final-z-offset-m", type=float, - default=0.0, - help="Same measured adjustment used by place_cup_holder; added only to place_final Z.", + default=-0.020, + help="Operational Z adjustment for pre-shake cup-holder re-grasp; negative lowers place_final Z.", ) parser.add_argument("--timeout-sec", type=float, default=90.0) parser.add_argument("--wait-service-sec", type=float, default=8.0) diff --git a/tools/run/run_voice_auto_cup_flow.sh b/tools/run/run_voice_auto_cup_flow.sh index 5c07d41..001b659 100755 --- a/tools/run/run_voice_auto_cup_flow.sh +++ b/tools/run/run_voice_auto_cup_flow.sh @@ -17,7 +17,7 @@ if ! [[ "${RECIPE_COLORS}" =~ ^(red|yellow|green|blue):[0-9]+(,(red|yellow|green fi SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" -MOTION_SERVICE_PREFIX="${MOTION_SERVICE_PREFIX:-auto}" +MOTION_SERVICE_PREFIX="${MOTION_SERVICE_PREFIX:-${SERVICE_PREFIX}}" AUTO_FLOW_RESUME_MODE="${AUTO_FLOW_RESUME_MODE:-normal}" AUTO_FLOW_RESUME_STATE_FILE="${AUTO_FLOW_RESUME_STATE_FILE:-/home/ssu/Azas/outputs/auto_cup_flow_resume.json}" AUTO_FLOW_RESUME_EVENTS_FILE="${AUTO_FLOW_RESUME_EVENTS_FILE:-/home/ssu/Azas/outputs/auto_cup_flow_events.jsonl}" diff --git a/tools/run/start_azas_voice_stack.sh b/tools/run/start_azas_voice_stack.sh index de7bfc9..fc8d6d0 100755 --- a/tools/run/start_azas_voice_stack.sh +++ b/tools/run/start_azas_voice_stack.sh @@ -20,6 +20,9 @@ RG2_PORT="${RG2_PORT:-502}" VOICE_PORT="${VOICE_PORT:-8090}" # 기본은 실제 로봇 제조까지 켠다. 리허설만 하려면 HW_EXEC=false 로 실행. HW_EXEC="${HW_EXEC:-true}" +USE_LIVE_STT="${USE_LIVE_STT:-true}" +STT_DEVICE_INDEX="${STT_DEVICE_INDEX:--1}" +STT_LANGUAGE="${STT_LANGUAGE:-ko-KR}" USE_LLM="${USE_LLM:-false}" OPEN_BROWSER="${OPEN_BROWSER:-true}" @@ -45,7 +48,7 @@ stamp='$(date +%Y%m%d-%H%M%S)' robot_cmd="cd ${ROOT}; ${common_env}; export ROBOT_HOST=${ROBOT_HOST}; export ROBOT_NAME=${ROBOT_NAME}; export RT_HOST=${RT_HOST}; export DOOSAN_REAL_MOTION_CONFIRM=ENABLE_DOOSAN_REAL_MOTION_BRINGUP; bash tools/run/run_doosan_real_m0609.sh 2>&1 | tee ${ROOT}/log/tmux_logic/robot-${stamp}.log" gripper_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch ${ROOT}/install/azas_gripper/share/azas_gripper/launch/rg2_trigger.launch.py ip:=${RG2_IP} port:=${RG2_PORT} connect:=true open_width:=1100 close_width:=0 force:=300 settle_seconds:=0.6 2>&1 | tee ${ROOT}/log/tmux_logic/gripper-${stamp}.log" camera_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch realsense2_camera rs_launch.py camera_name:=camera initial_reset:=true reconnect_timeout:=5.0 enable_color:=true enable_depth:=true align_depth.enable:=true rgb_camera.color_profile:=640x480x30 depth_module.depth_profile:=640x480x30 2>&1 | tee ${ROOT}/log/tmux_logic/camera-${stamp}.log" -voice_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch azas_voice azas_voice.launch.py use_pipeline_executor:=true enable_pipeline_hardware_execution:=${HW_EXEC} use_llm:=${USE_LLM} enable_llm:=${USE_LLM} voice_screen_port:=${VOICE_PORT} 2>&1 | tee ${ROOT}/log/tmux_logic/voice-${stamp}.log" +voice_cmd="cd ${ROOT}; ${common_env}; source /opt/ros/humble/setup.bash; source ${ROOT}/install/setup.bash; ros2 launch azas_voice azas_voice.launch.py use_live_stt:=${USE_LIVE_STT} stt_device_index:=${STT_DEVICE_INDEX} stt_language:=${STT_LANGUAGE} use_pipeline_executor:=true enable_pipeline_hardware_execution:=${HW_EXEC} pipeline_service_prefix:=${ROBOT_NAME} use_llm:=${USE_LLM} enable_llm:=${USE_LLM} use_tts:=true voice_screen_port:=${VOICE_PORT} 2>&1 | tee ${ROOT}/log/tmux_logic/voice-${stamp}.log" echo "[Azas] starting robot bringup..." tmux new-session -d -s "${SESSION}" -n robot "${robot_cmd}" @@ -62,6 +65,7 @@ sleep 4 echo "" echo "[Azas] voice cocktail stack is up: tmux session '${SESSION}' (robot/gripper/camera/voice)" +echo "[Azas] live STT: ${USE_LIVE_STT} device_index=${STT_DEVICE_INDEX} language=${STT_LANGUAGE}" echo "[Azas] panel: http://localhost:${VOICE_PORT} — 말로 주문하고 '응'으로 확정하면 제조가 시작됩니다." echo "[Azas] logs: tmux attach -t ${SESSION} / stop: bash tools/run/stop_azas_voice_stack.sh" tmux list-windows -t "${SESSION}" From 088b92ba308f4e65a9186b797a556ba1e21ec459 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Mon, 15 Jun 2026 09:52:50 +0900 Subject: [PATCH 80/88] =?UTF-8?q?7=EC=B0=A8=20=ED=85=8C=EC=8A=A4=ED=8A=B8?= =?UTF-8?q?=20=20stt=20~=20=EC=89=90=EC=9D=B4=ED=82=B9=EA=B9=8C=EC=A7=80?= =?UTF-8?q?=20=EC=99=84=EB=A3=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 2 ++ .../azas_task_manager/auto_cup_flow_router.py | 5 +++- .../run/open_robot_pipeline_control_panel.sh | 4 +-- tools/run/place_side_grip_cup_in_holder.py | 18 +++++++++++++ tools/run/robot_pipeline_control_server.py | 24 ++++++++++++++++-- tools/run/run_changhyun_side_grip_direct.sh | 2 +- tools/run/run_color_recipe_sequence.py | 2 ++ tools/run/run_color_scan_stage.sh | 25 +++++++++++++++++-- tools/run/run_human_hand_detection.sh | 2 +- tools/run/run_lid_close_then_shake_chain.sh | 2 +- .../run_measured_dispenser_recipe_sequence.py | 5 ++++ tools/run/run_rule_based_shake_real.sh | 19 ++++++++++++-- .../run/run_somyeong_cup_uprighting_direct.sh | 2 +- tools/run/run_stt_order_then_router.sh | 2 +- tools/run/run_tmux_logic_sequence.sh | 2 +- tools/run/show_color_scan_pose_rviz.sh | 2 +- ...show_measured_recipe_joint_preview_rviz.sh | 2 +- tools/run/start_azas_tmux_stack.sh | 2 +- 18 files changed, 104 insertions(+), 18 deletions(-) diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index 08b0345..cb6cca1 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -34,6 +34,7 @@ def generate_launch_description(): DeclareLaunchArgument("dispenser_3_cup_pre_extra_x_offset_m", default_value="-0.01"), DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="0.0"), DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.04"), + DeclareLaunchArgument("cup_holder_place_x_offset_m", default_value="0.010"), DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="-0.010"), DeclareLaunchArgument("cup_holder_rz_offset_deg", default_value="-1.0"), DeclareLaunchArgument("cup_holder_z_min_m", default_value="0.06"), @@ -86,6 +87,7 @@ def generate_launch_description(): "dispenser_3_cup_pre_extra_x_offset_m": ParameterValue(LaunchConfiguration("dispenser_3_cup_pre_extra_x_offset_m"), value_type=float), "final_regrasp_z_offset_m": ParameterValue(LaunchConfiguration("final_regrasp_z_offset_m"), value_type=float), "cup_holder_place_z_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_z_offset_m"), value_type=float), + "cup_holder_place_x_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_x_offset_m"), value_type=float), "cup_holder_place_y_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_y_offset_m"), value_type=float), "cup_holder_rz_offset_deg": ParameterValue(LaunchConfiguration("cup_holder_rz_offset_deg"), value_type=float), "cup_holder_z_min_m": ParameterValue(LaunchConfiguration("cup_holder_z_min_m"), value_type=float), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 51f6ff0..ff51c62 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -100,7 +100,7 @@ def __init__(self) -> None: "cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && " "mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && " "export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && " - "export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && " + "export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-1} && " "export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && " "if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && " "if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && " @@ -118,6 +118,7 @@ def __init__(self) -> None: self.declare_parameter("dispenser_3_cup_pre_extra_x_offset_m", -0.01) # 컵홀더에 놓을 때 보정값과 place 목표 z 안전 하한 (필요 시 조정) self.declare_parameter("cup_holder_place_z_offset_m", -0.04) + self.declare_parameter("cup_holder_place_x_offset_m", 0.015) self.declare_parameter("cup_holder_place_y_offset_m", -0.010) self.declare_parameter("cup_holder_rz_offset_deg", -1.0) self.declare_parameter("cup_holder_z_min_m", 0.06) @@ -684,12 +685,14 @@ def _run_recipe_sequence(self) -> bool: command += f" --dispenser-3-cup-pre-extra-x-offset-m {dispenser_3_pre_x}" regrasp_z = float(self.get_parameter("final_regrasp_z_offset_m").value) place_z = float(self.get_parameter("cup_holder_place_z_offset_m").value) + place_x = float(self.get_parameter("cup_holder_place_x_offset_m").value) place_y = float(self.get_parameter("cup_holder_place_y_offset_m").value) place_rz = float(self.get_parameter("cup_holder_rz_offset_deg").value) z_min = float(self.get_parameter("cup_holder_z_min_m").value) command += ( f" --final-regrasp-extra-z-offset-m {regrasp_z}" f" --cup-holder-place-final-z-offset-m {place_z}" + f" --cup-holder-place-final-x-offset-m {place_x}" f" --cup-holder-place-final-y-offset-m {place_y}" f" --cup-holder-rz-offset-deg {place_rz}" f" --cup-holder-z-min-m {z_min}" diff --git a/tools/run/open_robot_pipeline_control_panel.sh b/tools/run/open_robot_pipeline_control_panel.sh index 0e1d426..84709c5 100755 --- a/tools/run/open_robot_pipeline_control_panel.sh +++ b/tools/run/open_robot_pipeline_control_panel.sh @@ -138,13 +138,13 @@ MSG start_panel_server() { cd "$ROOT" - setsid env AZAS_ROOT="$ROOT" ROS_DOMAIN_ID="$PANEL_ROS_DOMAIN_ID" ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" bash -lc ' + setsid env AZAS_ROOT="$ROOT" ROS_DOMAIN_ID="$PANEL_ROS_DOMAIN_ID" ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" bash -lc ' cd "$AZAS_ROOT" source /opt/ros/humble/setup.bash source /home/ssu/ros2_ws/install/setup.bash source "$AZAS_ROOT/install/local_setup.bash" export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" - export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" + export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" exec python3 tools/run/robot_pipeline_control_server.py ' >> "$LOG_FILE" 2>&1 < /dev/null & echo "$!" > "$PID_FILE" diff --git a/tools/run/place_side_grip_cup_in_holder.py b/tools/run/place_side_grip_cup_in_holder.py index be767db..ce101c5 100755 --- a/tools/run/place_side_grip_cup_in_holder.py +++ b/tools/run/place_side_grip_cup_in_holder.py @@ -93,6 +93,12 @@ def offset_target_y(target: TargetPose, offset_m: float) -> TargetPose: return TargetPose(target.label, adjusted_xyz, list(target.rpy_rad)) +def offset_target_x(target: TargetPose, offset_m: float) -> TargetPose: + adjusted_xyz = list(target.xyz_m) + adjusted_xyz[0] += float(offset_m) + return TargetPose(target.label, adjusted_xyz, list(target.rpy_rad)) + + def offset_target_rz(target: TargetPose, offset_deg: float) -> TargetPose: adjusted_rpy = list(target.rpy_rad) adjusted_rpy[2] += math.radians(float(offset_deg)) @@ -302,6 +308,15 @@ def parse_args() -> argparse.Namespace: "to lower the cup into the holder without rewriting calibration.yaml." ), ) + parser.add_argument( + "--place-final-x-offset-m", + type=float, + default=0.015, + help=( + "Measured adjustment added only to place_final X. Default +0.015m shifts " + "the holder placement 15mm in positive X without rewriting calibration.yaml." + ), + ) parser.add_argument( "--place-final-y-offset-m", type=float, @@ -370,6 +385,8 @@ def main() -> int: try: pre_place, place_final, retreat, approach_lift_m = load_sequence(args.config) + if abs(args.place_final_x_offset_m) > 1e-9: + place_final = offset_target_x(place_final, args.place_final_x_offset_m) if abs(args.place_final_y_offset_m) > 1e-9: place_final = offset_target_y(place_final, args.place_final_y_offset_m) if abs(args.place_final_z_offset_m) > 1e-9: @@ -386,6 +403,7 @@ def main() -> int: print(f"[Azas] config={args.config}") print(f"[Azas] service_prefix={args.service_prefix}") print(f"[Azas] approach_lift_m={approach_lift_m:.3f}") + print(f"[Azas] place_final_x_offset_m={args.place_final_x_offset_m:.4f}") print(f"[Azas] place_final_y_offset_m={args.place_final_y_offset_m:.4f}") print(f"[Azas] place_final_z_offset_m={args.place_final_z_offset_m:.4f}") print(f"[Azas] rz_offset_deg={args.rz_offset_deg:.3f}") diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index b7ace0f..34d7094 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -36,7 +36,7 @@ "source /opt/ros/humble/setup.bash && " "mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && " "export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && " - "export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && " + "export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-1} && " "export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && " "if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then " "source /home/ssu/ws_moveit/install/setup.bash; " @@ -320,6 +320,11 @@ def _color_recipe_direct_arg(payload: dict[str, Any]) -> str: def color_recipe_sequence_command(payload: dict[str, Any]) -> str: + cup_holder_x_offset_m = str( + payload.get("cup_holder_place_final_x_offset_m") + or os.environ.get("CUP_HOLDER_PLACE_FINAL_X_OFFSET_M") + or "0.015" + ).strip() cup_holder_rz_offset_deg = str( payload.get("cup_holder_rz_offset_deg") or os.environ.get("CUP_HOLDER_RZ_OFFSET_DEG") @@ -329,6 +334,7 @@ def color_recipe_sequence_command(payload: dict[str, Any]) -> str: f"cd {ROOT} && {ROS_SETUP} && " "python3 tools/run/run_color_recipe_sequence.py --execute --confirm" f"{_color_recipe_direct_arg(payload)}" + f" --cup-holder-place-final-x-offset-m {shlex.quote(cup_holder_x_offset_m)}" f" --cup-holder-rz-offset-deg {shlex.quote(cup_holder_rz_offset_deg)}" ) @@ -3234,6 +3240,11 @@ def shell_env(payload: dict[str, Any]) -> dict[str, str]: or env.get("CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M") or "-0.030" ) + env["CUP_HOLDER_PLACE_FINAL_X_OFFSET_M"] = str( + payload.get("cup_holder_place_final_x_offset_m") + or env.get("CUP_HOLDER_PLACE_FINAL_X_OFFSET_M") + or "0.015" + ) env["CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M"] = str( payload.get("cup_holder_place_final_y_offset_m") or env.get("CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M") @@ -3509,7 +3520,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe f"SERVICE_PREFIX={shlex.quote(service_prefix)} " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " - "LID_ROS_LOCALHOST_ONLY=${LID_ROS_LOCALHOST_ONLY:-0} " + "LID_ROS_LOCALHOST_ONLY=${LID_ROS_LOCALHOST_ONLY:-1} " "LID_TCP_GRASP_OFFSET_Z_M=${LID_TCP_GRASP_OFFSET_Z_M:--0.032} " "MOVE_TO_LID_VIEW_POSE=true " f"bash {shlex.quote(str(direct_script))}" @@ -3699,6 +3710,11 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe or os.environ.get("CUP_HOLDER_PLACE_FINAL_Z_OFFSET_M") or "-0.030" ).strip() + place_final_x_offset_m = str( + payload.get("cup_holder_place_final_x_offset_m") + or os.environ.get("CUP_HOLDER_PLACE_FINAL_X_OFFSET_M") + or "0.015" + ).strip() place_final_y_offset_m = str( payload.get("cup_holder_place_final_y_offset_m") or os.environ.get("CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M") @@ -3718,6 +3734,7 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe "--moveit-planning-time-sec 8.0 --moveit-planning-attempts 5 " "--moveit-velocity-scaling 0.08 --moveit-acceleration-scaling 0.06 " "--approach-velocity 80.0 --approach-acceleration 20.0 " + f"--place-final-x-offset-m {shlex.quote(place_final_x_offset_m)} " f"--place-final-y-offset-m {shlex.quote(place_final_y_offset_m)} " f"--place-final-z-offset-m {shlex.quote(place_final_z_offset_m)} " f"--rz-offset-deg {shlex.quote(cup_holder_rz_offset_deg)} " @@ -4358,6 +4375,9 @@ def do_GET(self) -> None: "DISPENSER_TCP_NAME", DEFAULT_DISPENSER_TCP_NAME ), "selected_dispenser_id": os.environ.get("SELECTED_DISPENSER_ID", "2"), + "cup_holder_place_final_x_offset_m": os.environ.get( + "CUP_HOLDER_PLACE_FINAL_X_OFFSET_M", "0.015" + ), "cup_holder_place_final_y_offset_m": os.environ.get( "CUP_HOLDER_PLACE_FINAL_Y_OFFSET_M", "-0.010" ), diff --git a/tools/run/run_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh index f406d45..175dde0 100755 --- a/tools/run/run_changhyun_side_grip_direct.sh +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -6,7 +6,7 @@ SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" DISPLAY="${DISPLAY:-:0}" XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" -ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" cd "${ROOT}" diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 73495ad..365a52a 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -404,6 +404,7 @@ def main() -> int: help="마지막 디스펜서 처리 후 컵홀더에 컵을 놓음", ) parser.add_argument("--cup-holder-place-final-z-offset-m", default="-0.040") + parser.add_argument("--cup-holder-place-final-x-offset-m", default="0.015") parser.add_argument("--cup-holder-place-final-y-offset-m", default="-0.010") parser.add_argument( "--cup-holder-rz-offset-deg", @@ -503,6 +504,7 @@ def scaled_motion_capped(value: str, cap: float) -> str: "--gripper-open-settle-seconds", str(args.gripper_open_settle_seconds), "--gripper-settle-seconds", str(args.gripper_settle_seconds), "--cup-holder-place-final-z-offset-m", str(args.cup_holder_place_final_z_offset_m), + "--cup-holder-place-final-x-offset-m", str(args.cup_holder_place_final_x_offset_m), "--cup-holder-place-final-y-offset-m", str(args.cup_holder_place_final_y_offset_m), "--cup-holder-rz-offset-deg", str(args.cup_holder_rz_offset_deg), "--cup-holder-z-min-m", str(args.cup_holder_z_min_m), diff --git a/tools/run/run_color_scan_stage.sh b/tools/run/run_color_scan_stage.sh index f023ca8..3015c78 100755 --- a/tools/run/run_color_scan_stage.sh +++ b/tools/run/run_color_scan_stage.sh @@ -24,9 +24,30 @@ else fi export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} +wait_for_camera_frame() { + local topic="$1" + local timeout_sec="$2" + local deadline=$((SECONDS + timeout_sec)) + local sample_timeout_sec="${CAMERA_READY_SAMPLE_TIMEOUT_SEC:-3}" + local check_log="/tmp/azas_color_scan_camera_check.txt" + + : >"${check_log}" + echo "[Azas] waiting for color camera frame from ${topic} (timeout=${timeout_sec}s)" + while (( SECONDS < deadline )); do + if timeout "${sample_timeout_sec}s" ros2 topic echo --no-daemon --once --qos-reliability best_effort "${topic}" >"${check_log}" 2>&1; then + return 0 + fi + if timeout "${sample_timeout_sec}s" ros2 topic echo --no-daemon --once --qos-reliability reliable "${topic}" >"${check_log}" 2>&1; then + return 0 + fi + sleep 1 + done + return 1 +} + COLOR_TOPIC="${COLOR_TOPIC:-/camera/camera/color/image_raw}" -CAMERA_READY_TIMEOUT_SEC="${CAMERA_READY_TIMEOUT_SEC:-8}" -if ! timeout "${CAMERA_READY_TIMEOUT_SEC}s" ros2 topic echo --no-daemon --once --qos-reliability best_effort "${COLOR_TOPIC}" >/tmp/azas_color_scan_camera_check.txt 2>&1; then +CAMERA_READY_TIMEOUT_SEC="${CAMERA_READY_TIMEOUT_SEC:-30}" +if ! wait_for_camera_frame "${COLOR_TOPIC}" "${CAMERA_READY_TIMEOUT_SEC}"; then echo "[Azas][FAIL] color_scan camera preflight failed: no frame from ${COLOR_TOPIC} within ${CAMERA_READY_TIMEOUT_SEC}s" >&2 echo "[Azas][FAIL] Ensure RealSense publishes ${COLOR_TOPIC} with ROS_DOMAIN_ID=${ROS_DOMAIN_ID} ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY}, then retry." >&2 timeout 3s ros2 topic info --no-daemon -v "${COLOR_TOPIC}" 2>&1 | sed 's/^/[Azas][camera_info] /' >&2 || true diff --git a/tools/run/run_human_hand_detection.sh b/tools/run/run_human_hand_detection.sh index 8feea5a..302c234 100755 --- a/tools/run/run_human_hand_detection.sh +++ b/tools/run/run_human_hand_detection.sh @@ -14,6 +14,6 @@ if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then fi export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" -export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" exec python3 "${ROOT_DIR}/tools/perception/human_hand_detection_node.py" "$@" diff --git a/tools/run/run_lid_close_then_shake_chain.sh b/tools/run/run_lid_close_then_shake_chain.sh index 2825f50..e685a44 100755 --- a/tools/run/run_lid_close_then_shake_chain.sh +++ b/tools/run/run_lid_close_then_shake_chain.sh @@ -10,4 +10,4 @@ export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" export LID_TCP_GRASP_OFFSET_Z_M="${LID_TCP_GRASP_OFFSET_Z_M:--0.032}" export SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" -( cd /home/ssu/Azas && SERVICE_PREFIX="${SERVICE_PREFIX}" DISPLAY=${DISPLAY:-:0} XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} MOVE_TO_LID_VIEW_POSE=true bash /home/ssu/Azas/tools/run/run_kang_lid_grip_close_direct.sh ) & lid_pid=$!; ( cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 /home/ssu/Azas/tools/run/wait_for_lid_grip_status.py --timeout-sec 900 --success-status motion_sequence_requested ) & wait_pid=$!; while true; do if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; if ! kill -0 ${lid_pid} 2>/dev/null; then wait ${lid_pid}; lid_rc=$?; sleep 1; if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; echo '[Azas] lid_grip_close launch exited before ArUco success status; shake chain blocked.'; kill -TERM ${wait_pid} 2>/dev/null || true; wait ${wait_pid} 2>/dev/null || true; if [ ${lid_rc} -eq 0 ]; then exit 1; else exit ${lid_rc}; fi; fi; sleep 1; done; kill -TERM ${lid_pid} 2>/dev/null || true; wait ${lid_pid} 2>/dev/null || true; if [ ${wait_rc} -eq 0 ]; then echo '[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹으로 바로 넘어갑니다.'; echo '[Azas] auto_holder_pick_then_shake=true'; cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-0} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && echo '[Azas] SHAKE START: 컵홀더에 놓인 닫힌 컵을 측정 pose로 다시 side-grip 픽업한 뒤 흔듭니다.' && echo '[Azas] 순서: RG2 open -> 컵홀더 retreat 접근 -> holder final pose에서 soft grasp -> holder lift -> 관절 쉐이킹.' && echo '[Azas] 주의: 컵 좌표를 새로 만들지 않고 calibration.yaml cup_holder.side_grip_place 측정값만 사용합니다.' && python3 tools/run/pick_from_cup_holder_side_grip.py --service-prefix "${SERVICE_PREFIX}" --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml --approach-velocity 40.0 --approach-acceleration 40.0 --descend-velocity 40.0 --descend-acceleration 40.0 --lift-velocity 40.0 --lift-acceleration 40.0 --place-final-z-offset-m -0.020 --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 --ikin-timeout-sec 20.0 --ikin-retries 2 --gripper-grasp-width-m 0.068 --gripper-force-n 25.0 --post-grasp-settle-sec 0.8 --z-max 0.28 --execute --confirm ENABLE_CUP_HOLDER_PICK && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=remove_world -p object_id:=tumbler_in_holder -p dispenser_id:=1 -p publish_once:=true && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=attach -p object_id:=carried_tumbler -p dispenser_id:=1 -p publish_once:=true && SERVICE_PREFIX="${SERVICE_PREFIX}" GRASPED_CUP_TEST_MODE=true SKIP_CUP_HOLDER_PICK=true REQUIRE_ROBOT_STANDBY=true SHAKE_CONTROL_MODE=joint SHAKE_CYCLES=3 JOINT_SHAKE_BASE_J1_DEG=0.0 JOINT_SHAKE_BASE_J2_DEG=-35.0 JOINT_SHAKE_BASE_J3_DEG=50.0 JOINT_SHAKE_BASE_J4_DEG=0.0 JOINT_SHAKE_BASE_J5_DEG=70.0 JOINT_SHAKE_BASE_J6_DEG=0.0 JOINT_SHAKE_J3_AMPLITUDE_DEG=0.0 JOINT_SHAKE_J4_AMPLITUDE_DEG=18.0 JOINT_SHAKE_J5_AMPLITUDE_DEG=20.0 JOINT_SHAKE_J6_AMPLITUDE_DEG=24.0 JOINT_SHAKE_J1_MIN_DEG=-20.0 JOINT_SHAKE_J1_MAX_DEG=5.0 JOINT_SHAKE_J2_MIN_DEG=-80.0 JOINT_SHAKE_J2_MAX_DEG=5.0 JOINT_SHAKE_J3_MIN_DEG=0.0 JOINT_SHAKE_J3_MAX_DEG=135.0 JOINT_SHAKE_MAX_SINGLE_DELTA_DEG=75.0 ENFORCE_WRIST_JOINT_LIMITS=false WRIST_MIN_DEG=-135.0 WRIST_MAX_DEG=135.0 JOINT5_MIN_DEG=40.0 JOINT5_MAX_DEG=100.0 APPROACH_JOINT_VELOCITY=18.0 APPROACH_JOINT_ACCELERATION=22.0 APPROACH_JOINT_TIME=2.6 SHAKE_JOINT_VELOCITY=120.0 SHAKE_JOINT_ACCELERATION=160.0 SHAKE_JOINT_TIME=0.0 JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=160.0 VERIFY_JOINT_TARGETS=true JOINT_TARGET_TOLERANCE_DEG=8.0 JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true REAL_ROBOT_MOTION_CONFIRM=ENABLE_REAL_ROBOT_MOTION tools/run/run_rule_based_shake_real.sh && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && python3 tools/run/direct_movej_joints.py --service-prefix "${SERVICE_PREFIX}" --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 --velocity 15 --acceleration 15 --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ; else echo '[Azas] ArUco lid_grip_close 실패/타임아웃 -> 컵홀더 재픽업/쉐이킹을 건너뜁니다.'; exit ${wait_rc}; fi +( cd /home/ssu/Azas && SERVICE_PREFIX="${SERVICE_PREFIX}" DISPLAY=${DISPLAY:-:0} XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} MOVE_TO_LID_VIEW_POSE=true bash /home/ssu/Azas/tools/run/run_kang_lid_grip_close_direct.sh ) & lid_pid=$!; ( cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-1} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && python3 /home/ssu/Azas/tools/run/wait_for_lid_grip_status.py --timeout-sec 900 --success-status motion_sequence_requested ) & wait_pid=$!; while true; do if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; if ! kill -0 ${lid_pid} 2>/dev/null; then wait ${lid_pid}; lid_rc=$?; sleep 1; if ! kill -0 ${wait_pid} 2>/dev/null; then wait ${wait_pid}; wait_rc=$?; break; fi; echo '[Azas] lid_grip_close launch exited before ArUco success status; shake chain blocked.'; kill -TERM ${wait_pid} 2>/dev/null || true; wait ${wait_pid} 2>/dev/null || true; if [ ${lid_rc} -eq 0 ]; then exit 1; else exit ${lid_rc}; fi; fi; sleep 1; done; kill -TERM ${lid_pid} 2>/dev/null || true; wait ${lid_pid} 2>/dev/null || true; if [ ${wait_rc} -eq 0 ]; then echo '[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹으로 바로 넘어갑니다.'; echo '[Azas] auto_holder_pick_then_shake=true'; cd /home/ssu/Azas && source /opt/ros/humble/setup.bash && mkdir -p /tmp/azas_ros_logs && export ROS_LOG_DIR=/tmp/azas_ros_logs && export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-9} && export ROS_LOCALHOST_ONLY=${ROS_LOCALHOST_ONLY:-1} && export FASTDDS_BUILTIN_TRANSPORTS=${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4} && if [ -f /home/ssu/ws_moveit/install/setup.bash ]; then source /home/ssu/ws_moveit/install/setup.bash; fi && if [ -f /home/ssu/ros2_ws/install/setup.bash ]; then source /home/ssu/ros2_ws/install/setup.bash; fi && if [ -f /home/ssu/Azas/install/setup.bash ]; then source /home/ssu/Azas/install/setup.bash; else source /home/ssu/Azas/install/local_setup.bash; fi && export PYTHONPATH=/home/ssu/Azas/tools/run/python_compat:${PYTHONPATH:-} && echo '[Azas] SHAKE START: 컵홀더에 놓인 닫힌 컵을 측정 pose로 다시 side-grip 픽업한 뒤 흔듭니다.' && echo '[Azas] 순서: RG2 open -> 컵홀더 retreat 접근 -> holder final pose에서 soft grasp -> holder lift -> 관절 쉐이킹.' && echo '[Azas] 주의: 컵 좌표를 새로 만들지 않고 calibration.yaml cup_holder.side_grip_place 측정값만 사용합니다.' && python3 tools/run/pick_from_cup_holder_side_grip.py --service-prefix "${SERVICE_PREFIX}" --config /home/ssu/Azas/install/azas_bringup/share/azas_bringup/config/calibration.yaml --approach-velocity 40.0 --approach-acceleration 40.0 --descend-velocity 40.0 --descend-acceleration 40.0 --lift-velocity 40.0 --lift-acceleration 40.0 --place-final-z-offset-m -0.020 --timeout-sec 90.0 --target-tolerance-mm 12.0 --verify-timeout-sec 45.0 --ikin-timeout-sec 20.0 --ikin-retries 2 --gripper-grasp-width-m 0.068 --gripper-force-n 25.0 --post-grasp-settle-sec 0.8 --z-max 0.28 --execute --confirm ENABLE_CUP_HOLDER_PICK && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=remove_world -p object_id:=tumbler_in_holder -p dispenser_id:=1 -p publish_once:=true && timeout 5s python3 -m azas_motion.tumbler_collision_scene_node --ros-args -p action:=attach -p object_id:=carried_tumbler -p dispenser_id:=1 -p publish_once:=true && SERVICE_PREFIX="${SERVICE_PREFIX}" GRASPED_CUP_TEST_MODE=true SKIP_CUP_HOLDER_PICK=true REQUIRE_ROBOT_STANDBY=true SHAKE_CONTROL_MODE=joint SHAKE_CYCLES=3 JOINT_SHAKE_BASE_J1_DEG=0.0 JOINT_SHAKE_BASE_J2_DEG=-35.0 JOINT_SHAKE_BASE_J3_DEG=50.0 JOINT_SHAKE_BASE_J4_DEG=0.0 JOINT_SHAKE_BASE_J5_DEG=70.0 JOINT_SHAKE_BASE_J6_DEG=0.0 JOINT_SHAKE_J3_AMPLITUDE_DEG=0.0 JOINT_SHAKE_J4_AMPLITUDE_DEG=18.0 JOINT_SHAKE_J5_AMPLITUDE_DEG=20.0 JOINT_SHAKE_J6_AMPLITUDE_DEG=24.0 JOINT_SHAKE_J1_MIN_DEG=-20.0 JOINT_SHAKE_J1_MAX_DEG=5.0 JOINT_SHAKE_J2_MIN_DEG=-80.0 JOINT_SHAKE_J2_MAX_DEG=5.0 JOINT_SHAKE_J3_MIN_DEG=0.0 JOINT_SHAKE_J3_MAX_DEG=135.0 JOINT_SHAKE_MAX_SINGLE_DELTA_DEG=75.0 ENFORCE_WRIST_JOINT_LIMITS=false WRIST_MIN_DEG=-135.0 WRIST_MAX_DEG=135.0 JOINT5_MIN_DEG=40.0 JOINT5_MAX_DEG=100.0 APPROACH_JOINT_VELOCITY=18.0 APPROACH_JOINT_ACCELERATION=22.0 APPROACH_JOINT_TIME=2.6 SHAKE_JOINT_VELOCITY=120.0 SHAKE_JOINT_ACCELERATION=160.0 SHAKE_JOINT_TIME=0.0 JOINT_SHAKE_PEAK_VELOCITY_LIMIT_DEG_S=160.0 VERIFY_JOINT_TARGETS=true JOINT_TARGET_TOLERANCE_DEG=8.0 JOINT_TARGET_WAIT_EXTRA_SEC=3.0 JOINT_TARGET_POLL_SEC=0.05 REQUIRE_STATE_VALIDITY_FOR_JOINT_SHAKE=true REAL_ROBOT_MOTION_CONFIRM=ENABLE_REAL_ROBOT_MOTION tools/run/run_rule_based_shake_real.sh && echo '[Azas] SHAKE DONE: 손 검출/핸드오버를 위해 카메라 포즈로 복귀합니다 (컵 파지 유지).' && python3 tools/run/direct_movej_joints.py --service-prefix "${SERVICE_PREFIX}" --j1 3.0 --j2 -12.7 --j3 44.0 --j4 -9.0 --j5 133.0 --j6 90.0 --velocity 15 --acceleration 15 --j5-min-deg -150 --j5-max-deg 150 --timeout-sec 60 --motion-timeout-sec 120 --execute --confirm ENABLE_DIRECT_MOVEJ; else echo '[Azas] ArUco lid_grip_close 실패/타임아웃 -> 컵홀더 재픽업/쉐이킹을 건너뜁니다.'; exit ${wait_rc}; fi diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index 93b78a7..aa98981 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -1785,6 +1785,7 @@ def place_cup_in_holder(self) -> None: if abs(rz_offset_deg) > 1e-9: for posx in (pre_place, place_final, retreat): posx[5] += rz_offset_deg + place_final[0] += self.args.cup_holder_place_final_x_offset_m * 1000.0 place_final[1] += self.args.cup_holder_place_final_y_offset_m * 1000.0 place_final[2] += self.args.cup_holder_place_final_z_offset_m * 1000.0 for label, posx in ( @@ -3747,6 +3748,7 @@ def parse_args() -> argparse.Namespace: help="After the final dispenser re-grasp, place the held cup into calibration.yaml cup_holder.side_grip_place.", ) parser.add_argument("--cup-holder-place-final-z-offset-m", type=float, default=-0.040) + parser.add_argument("--cup-holder-place-final-x-offset-m", type=float, default=0.015) parser.add_argument("--cup-holder-place-final-y-offset-m", type=float, default=-0.010) parser.add_argument( "--cup-holder-rz-offset-deg", @@ -3965,6 +3967,7 @@ def main() -> int: ) print( f"[Azas] cup_holder_after_sequence={str(args.place_cup_holder_after_sequence).lower()} " + f"place_final_x_offset_m={args.cup_holder_place_final_x_offset_m:.3f} " f"place_final_y_offset_m={args.cup_holder_place_final_y_offset_m:.3f} " f"place_final_z_offset_m={args.cup_holder_place_final_z_offset_m:.3f} " f"rz_offset_deg={args.cup_holder_rz_offset_deg:.1f}" @@ -4180,6 +4183,8 @@ def main() -> int: f"{args.cup_holder_retreat_acceleration:.6f}", "--place-final-z-offset-m", f"{args.cup_holder_place_final_z_offset_m:.6f}", + "--place-final-x-offset-m", + f"{args.cup_holder_place_final_x_offset_m:.6f}", "--place-final-y-offset-m", f"{args.cup_holder_place_final_y_offset_m:.6f}", "--rz-offset-deg", diff --git a/tools/run/run_rule_based_shake_real.sh b/tools/run/run_rule_based_shake_real.sh index cf1fa8d..a045cfc 100755 --- a/tools/run/run_rule_based_shake_real.sh +++ b/tools/run/run_rule_based_shake_real.sh @@ -48,6 +48,8 @@ APPROACH_LINE_TIME="${APPROACH_LINE_TIME:-3.5}" SHAKE_LINE_TIME="${SHAKE_LINE_TIME:-0.40}" SERVICE_WAIT_TIMEOUT_SEC="${SERVICE_WAIT_TIMEOUT_SEC:-5.0}" MOTION_RESPONSE_TIMEOUT_SEC="${MOTION_RESPONSE_TIMEOUT_SEC:-10.0}" +ROBOT_STATE_TIMEOUT_SEC="${ROBOT_STATE_TIMEOUT_SEC:-8.0}" +ROBOT_STATE_RETRIES="${ROBOT_STATE_RETRIES:-3}" RX="${RX:-180.0}" RY="${RY:-0.0}" RZ="${RZ:-180.0}" @@ -158,8 +160,21 @@ prefixed_service() { if [[ "${REQUIRE_ROBOT_STANDBY}" == "true" ]]; then echo "[Azas] Checking Doosan robot state before real motion." robot_state_service="$(prefixed_service system/get_robot_state)" - if ! robot_state_output="$(timeout 5s ros2 service call "${robot_state_service}" dsr_msgs2/srv/GetRobotState "{}")"; then - echo "[Azas] Refusing real robot shake: ${robot_state_service} did not respond." + robot_state_output="" + robot_state_ok=false + for attempt in $(seq 1 "${ROBOT_STATE_RETRIES}"); do + if robot_state_output="$( + timeout "${ROBOT_STATE_TIMEOUT_SEC}s" \ + ros2 service call "${robot_state_service}" dsr_msgs2/srv/GetRobotState "{}" + )"; then + robot_state_ok=true + break + fi + echo "[Azas] ${robot_state_service} did not respond (attempt ${attempt}/${ROBOT_STATE_RETRIES}); retrying..." + sleep 0.5 + done + if [[ "${robot_state_ok}" != "true" ]]; then + echo "[Azas] Refusing real robot shake: ${robot_state_service} did not respond after ${ROBOT_STATE_RETRIES} attempts." echo "[Azas] Start Doosan real bringup and confirm the robot network is connected." exit 1 fi diff --git a/tools/run/run_somyeong_cup_uprighting_direct.sh b/tools/run/run_somyeong_cup_uprighting_direct.sh index 13405ac..b14d619 100755 --- a/tools/run/run_somyeong_cup_uprighting_direct.sh +++ b/tools/run/run_somyeong_cup_uprighting_direct.sh @@ -13,7 +13,7 @@ PUBLISH_HAND_EYE_TF="${PUBLISH_HAND_EYE_TF:-true}" MOVEIT_CONTROLLER_NAME="${MOVEIT_CONTROLLER_NAME:-/dsr01/dsr_moveit_controller}" CONTROLLER_ACTION_NAME="${CONTROLLER_ACTION_NAME:-${MOVEIT_CONTROLLER_NAME}/follow_joint_trajectory}" ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" -ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" cd "${ROOT}" diff --git a/tools/run/run_stt_order_then_router.sh b/tools/run/run_stt_order_then_router.sh index def6687..f316cc0 100755 --- a/tools/run/run_stt_order_then_router.sh +++ b/tools/run/run_stt_order_then_router.sh @@ -31,7 +31,7 @@ source "${ROOT}/install/setup.bash" set -u export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" -export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" export ROS_LOG_DIR="${ROS_LOG_DIR:-/tmp/azas_ros_logs}" mkdir -p "${ROS_LOG_DIR}" diff --git a/tools/run/run_tmux_logic_sequence.sh b/tools/run/run_tmux_logic_sequence.sh index 5719ad9..c6cc3c7 100755 --- a/tools/run/run_tmux_logic_sequence.sh +++ b/tools/run/run_tmux_logic_sequence.sh @@ -6,7 +6,7 @@ LOG_DIR="${ROOT}/log/tmux_logic" SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}" DISPLAY="${DISPLAY:-:0}" XAUTHORITY="${XAUTHORITY:-/run/user/1000/gdm/Xauthority}" -ROS_LOCALHOST_ONLY="${TMUX_LOGIC_ROS_LOCALHOST_ONLY:-0}" +ROS_LOCALHOST_ONLY="${TMUX_LOGIC_ROS_LOCALHOST_ONLY:-1}" LID_TCP_GRASP_OFFSET_Z_M="${LID_TCP_GRASP_OFFSET_Z_M:--0.032}" LID_MIN_GRASP_Z_M="${LID_MIN_GRASP_Z_M:-0.020}" diff --git a/tools/run/show_color_scan_pose_rviz.sh b/tools/run/show_color_scan_pose_rviz.sh index ab69df8..7e9048d 100755 --- a/tools/run/show_color_scan_pose_rviz.sh +++ b/tools/run/show_color_scan_pose_rviz.sh @@ -8,7 +8,7 @@ set -euo pipefail ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-79}" -export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" set +u source /opt/ros/humble/setup.bash diff --git a/tools/run/show_measured_recipe_joint_preview_rviz.sh b/tools/run/show_measured_recipe_joint_preview_rviz.sh index 66e9dc0..af622b1 100755 --- a/tools/run/show_measured_recipe_joint_preview_rviz.sh +++ b/tools/run/show_measured_recipe_joint_preview_rviz.sh @@ -63,7 +63,7 @@ fi set -u export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" -export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +export ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" pkill -f 'publish_measured_recipe_joint_rviz_preview.py' 2>/dev/null || true diff --git a/tools/run/start_azas_tmux_stack.sh b/tools/run/start_azas_tmux_stack.sh index 0a3ae6c..f0b7ff4 100755 --- a/tools/run/start_azas_tmux_stack.sh +++ b/tools/run/start_azas_tmux_stack.sh @@ -9,7 +9,7 @@ RT_HOST="${RT_HOST:-0.0.0.0}" RG2_IP="${RG2_IP:-192.168.1.1}" RG2_PORT="${RG2_PORT:-502}" ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-9}" -ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-0}" +ROS_LOCALHOST_ONLY="${ROS_LOCALHOST_ONLY:-1}" FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" FORCE_RESTART="${FORCE_RESTART:-false}" CLEAN_FASTDDS_SHM="${CLEAN_FASTDDS_SHM:-false}" From c3e3a268c767c0c9fde6971be98f2d6d9322077f Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Mon, 15 Jun 2026 10:07:27 +0900 Subject: [PATCH 81/88] =?UTF-8?q?test/8=EC=B0=A8=20=ED=85=8C=EC=8A=A4?= =?UTF-8?q?=ED=8A=B8=20lid=EB=B2=84=ED=8A=BC=20=ED=81=B4=EB=A6=AD=EC=97=86?= =?UTF-8?q?=EC=9D=B4=20=EC=9E=90=EB=8F=99=ED=99=94=20=EB=A1=9C=EC=A7=81=20?= =?UTF-8?q?=EA=B5=AC=EC=B6=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../lid_sticker_grip_planning.launch.py | 25 ++++++ .../azas_motion/lid_grip_planner_node.py | 24 +++++- .../lid_sticker_detector_node.py | 82 +++++++++++++++++-- tools/run/run_kang_lid_grip_close_direct.sh | 2 + tools/run/wait_for_lid_grip_status.py | 11 +++ 5 files changed, 136 insertions(+), 8 deletions(-) diff --git a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py index b7b2377..42a5959 100644 --- a/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py +++ b/src/azas_bringup/launch/lid_sticker_grip_planning.launch.py @@ -66,6 +66,11 @@ def generate_launch_description(): DeclareLaunchArgument("log_pose_plans", default_value="false"), DeclareLaunchArgument("log_korean_status", default_value="true"), DeclareLaunchArgument("log_json_status", default_value="false"), + DeclareLaunchArgument("auto_grip_on_stable_detection", default_value="false"), + DeclareLaunchArgument("auto_grip_required_samples", default_value="5"), + DeclareLaunchArgument("auto_grip_min_stable_sec", default_value="0.8"), + DeclareLaunchArgument("auto_grip_cooldown_sec", default_value="30.0"), + DeclareLaunchArgument("auto_grip_once", default_value="true"), DeclareLaunchArgument("lid_detection_topic", default_value="/azas/lid_detection"), DeclareLaunchArgument("lid_pose_topic", default_value="/jarvis/lid_gripper/lid_pose"), DeclareLaunchArgument("grip_request_topic", default_value="/jarvis/lid_gripper/grip_request"), @@ -329,6 +334,26 @@ def generate_launch_description(): value_type=bool, ), "show_preview": ParameterValue(LaunchConfiguration("show_preview"), value_type=bool), + "auto_grip_on_stable_detection": ParameterValue( + LaunchConfiguration("auto_grip_on_stable_detection"), + value_type=bool, + ), + "auto_grip_required_samples": ParameterValue( + LaunchConfiguration("auto_grip_required_samples"), + value_type=int, + ), + "auto_grip_min_stable_sec": ParameterValue( + LaunchConfiguration("auto_grip_min_stable_sec"), + value_type=float, + ), + "auto_grip_cooldown_sec": ParameterValue( + LaunchConfiguration("auto_grip_cooldown_sec"), + value_type=float, + ), + "auto_grip_once": ParameterValue( + LaunchConfiguration("auto_grip_once"), + value_type=bool, + ), }], ), Node( diff --git a/src/azas_motion/azas_motion/lid_grip_planner_node.py b/src/azas_motion/azas_motion/lid_grip_planner_node.py index be6b20d..6fa34fe 100644 --- a/src/azas_motion/azas_motion/lid_grip_planner_node.py +++ b/src/azas_motion/azas_motion/lid_grip_planner_node.py @@ -380,7 +380,15 @@ def _on_grip_request(self, msg: String) -> None: payload = json.loads(msg.data) except json.JSONDecodeError: payload = {"accepted": True, "raw": msg.data} + request_source = self._request_source_from_payload(payload) if payload.get("accepted") is False: + if self._sequence_lock.locked(): + self._publish_status( + "trigger_ignored_no_valid_detection_motion_in_progress", + request=payload, + real_motion=self._hardware_armed, + ) + return self._publish_status( "failed", error="P_KEY_WITHOUT_VALID_LID_DETECTION", @@ -409,20 +417,28 @@ def _on_grip_request(self, msg: String) -> None: self._publish_status( "trigger_received", request=payload, + request_source=request_source, hardware_armed=self._hardware_armed, real_motion=self._hardware_armed, ) threading.Thread( target=self._run_trigger_sequence, - args=(pose_snapshot,), + args=(pose_snapshot, request_source), daemon=True, ).start() - def _run_trigger_sequence(self, pose_snapshot: PoseStamped) -> None: + @staticmethod + def _request_source_from_payload(payload: dict) -> str: + source = str(payload.get("trigger_source") or "").strip() + if source: + return source + return "p_key" + + def _run_trigger_sequence(self, pose_snapshot: PoseStamped, request_source: str) -> None: try: self._plan_from_pose( pose_snapshot, - request_source="p_key", + request_source=request_source, allow_gripper=bool(self.get_parameter("enable_gripper_service_calls").value), allow_motion=True, ) @@ -3437,6 +3453,8 @@ def _human_status_text(self, status: str, fields: dict) -> str: return "[뚜껑픽] 6/6 sequence 완료: approach -> grasp -> lift 요청 완료" if status == "trigger_ignored_motion_in_progress": return "[뚜껑픽] p 입력 무시: 이전 motion sequence가 아직 진행 중입니다" + if status == "trigger_ignored_no_valid_detection_motion_in_progress": + return "[뚜껑픽] 감지 끊김 요청 무시: 이전 motion sequence가 아직 진행 중입니다" if status == "trigger_planned_no_motion": return "[뚜껑픽] motion 미실행: enable_hardware=false라 plan만 생성했습니다" if status == "failed": diff --git a/src/azas_perception/azas_perception/lid_sticker_detector_node.py b/src/azas_perception/azas_perception/lid_sticker_detector_node.py index 044061b..d5a07e4 100644 --- a/src/azas_perception/azas_perception/lid_sticker_detector_node.py +++ b/src/azas_perception/azas_perception/lid_sticker_detector_node.py @@ -127,6 +127,11 @@ def __init__(self): self.declare_parameter("show_preview", True) self.declare_parameter("preview_window_name", "Azas Lid Detection - p grip, ESC quit") self.declare_parameter("preview_wait_ms", 1) + self.declare_parameter("auto_grip_on_stable_detection", False) + self.declare_parameter("auto_grip_required_samples", 5) + self.declare_parameter("auto_grip_min_stable_sec", 0.8) + self.declare_parameter("auto_grip_cooldown_sec", 30.0) + self.declare_parameter("auto_grip_once", True) self._latest_depth: Optional[np.ndarray] = None self._latest_depth_encoding = "" @@ -136,6 +141,9 @@ def __init__(self): self._latest_valid_status = "" self._latest_valid_stamp = None self._last_accepted_grip_request_time: float | None = None + self._accepted_grip_request_sent = False + self._auto_grip_stable_count = 0 + self._auto_grip_stable_since: float | None = None self._preview_window_created = False self._model = self._load_model() @@ -330,6 +338,7 @@ def _on_color(self, msg: Image) -> None: self._pub.publish(output) self._latest_valid_status = output.status self._latest_valid_stamp = output.header.stamp + self._maybe_publish_auto_grip_request(output.status) if bool(self.get_parameter("log_detections").value): self.get_logger().info( "Published lid marker detection: " @@ -766,6 +775,7 @@ def _log_depth_scale(self, depth_scale: float) -> None: def _publish_invalid(self, msg: Image, status: str) -> None: self._latest_valid_status = "" self._latest_valid_stamp = None + self._reset_auto_grip_stability() output = CupDetection() output.header.stamp = msg.header.stamp output.header.frame_id = msg.header.frame_id @@ -859,13 +869,63 @@ def _show_preview( cv2.imshow(window, frame) key = cv2.waitKey(max(int(self.get_parameter("preview_wait_ms").value), 1)) & 0xFF if key in (ord("p"), ord("P")): - self._publish_grip_request() + self._publish_grip_request(trigger_source="p_key") elif key in (27, ord("q"), ord("Q")): cv2.destroyWindow(window) if rclpy.ok(): rclpy.shutdown() - def _publish_grip_request(self) -> None: + def _maybe_publish_auto_grip_request(self, status: str) -> None: + if not bool(self.get_parameter("auto_grip_on_stable_detection").value): + return + if bool(self.get_parameter("auto_grip_once").value) and self._accepted_grip_request_sent: + return + if not self._grip_request_would_be_accepted(status): + self._reset_auto_grip_stability() + return + + now = time.monotonic() + if self._auto_grip_stable_count <= 0 or self._auto_grip_stable_since is None: + self._auto_grip_stable_since = now + self._auto_grip_stable_count = 0 + self._auto_grip_stable_count += 1 + + required_samples = max(int(self.get_parameter("auto_grip_required_samples").value), 1) + min_stable_sec = max(float(self.get_parameter("auto_grip_min_stable_sec").value), 0.0) + stable_sec = now - self._auto_grip_stable_since + if self._auto_grip_stable_count < required_samples or stable_sec < min_stable_sec: + return + + cooldown_sec = max(float(self.get_parameter("auto_grip_cooldown_sec").value), 0.0) + if ( + self._last_accepted_grip_request_time is not None + and now - self._last_accepted_grip_request_time < cooldown_sec + ): + return + + self._publish_grip_request( + trigger_source="auto_stable_detection", + stable_samples=self._auto_grip_stable_count, + stable_sec=stable_sec, + ) + + def _reset_auto_grip_stability(self) -> None: + self._auto_grip_stable_count = 0 + self._auto_grip_stable_since = None + + def _grip_request_would_be_accepted(self, status: str) -> bool: + accepted = bool(status.startswith("detected:lid")) + if bool(self.get_parameter("require_lid_detection").value): + accepted = accepted and not self._status_is_aruco_only(status) + return accepted + + def _publish_grip_request( + self, + *, + trigger_source: str, + stable_samples: int | None = None, + stable_sec: float | None = None, + ) -> None: accepted = bool(self._latest_valid_status.startswith("detected:lid")) if bool(self.get_parameter("require_lid_detection").value): accepted = accepted and not self._status_is_aruco_only(self._latest_valid_status) @@ -874,18 +934,30 @@ def _publish_grip_request(self) -> None: "command": "grip_lid", "accepted": accepted, "source": "lid_sticker_detector_node", + "trigger_source": trigger_source, "status": self._latest_valid_status if accepted else "no_valid_lid_detection", } if stamp is not None: payload["stamp"] = f"{stamp.sec}.{stamp.nanosec:09d}" + if stable_samples is not None: + payload["stable_samples"] = int(stable_samples) + if stable_sec is not None: + payload["stable_sec"] = round(float(stable_sec), 3) msg = String() msg.data = json.dumps(payload, sort_keys=True) self._grip_request_pub.publish(msg) if accepted: self._last_accepted_grip_request_time = time.monotonic() - self.get_logger().warn( - "Published supervised lid grip request from p key; downstream motion remains gated" - ) + self._accepted_grip_request_sent = True + if trigger_source == "auto_stable_detection": + self.get_logger().warn( + "Published automatic supervised lid grip request after stable detection; " + "downstream motion remains gated" + ) + else: + self.get_logger().warn( + "Published supervised lid grip request from p key; downstream motion remains gated" + ) else: self.get_logger().warn("Ignored p key because there is no valid detected:lid frame") diff --git a/tools/run/run_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh index 078922d..3ad5911 100755 --- a/tools/run/run_kang_lid_grip_close_direct.sh +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -79,6 +79,8 @@ launch_args=( model_path:="${MODEL_PATH}" \ marker_type:=aruco require_lid_detection:=true \ allow_aruco_only_after_grip_request:=true aruco_only_after_grip_request_sec:=20.0 \ + auto_grip_on_stable_detection:=true auto_grip_required_samples:=5 auto_grip_min_stable_sec:=0.8 \ + auto_grip_cooldown_sec:=30.0 auto_grip_once:=true \ aruco_dictionary:="${ARUCO_DICTIONARY}" aruco_marker_id:="${ARUCO_MARKER_ID}" \ aruco_marker_length_m:="${ARUCO_MARKER_LENGTH_M}" \ use_aruco_axis_for_orientation:=true aruco_finger_axis_quarter_turns:=0 \ diff --git a/tools/run/wait_for_lid_grip_status.py b/tools/run/wait_for_lid_grip_status.py index 8c2788b..08cadb9 100755 --- a/tools/run/wait_for_lid_grip_status.py +++ b/tools/run/wait_for_lid_grip_status.py @@ -106,12 +106,23 @@ def _marks_sequence_started(status: str, payload: dict) -> bool: def _should_ignore_failure(self, payload: dict) -> bool: if not self._ignore_pretrigger_failures: return False + if self._sequence_started and self._is_rejected_no_valid_lid_request(payload): + return True if self._sequence_started: return False if payload.get("real_motion") is True: return False return True + @staticmethod + def _is_rejected_no_valid_lid_request(payload: dict) -> bool: + request = payload.get("request") + if not isinstance(request, dict): + return False + if request.get("accepted") is not False: + return False + return str(request.get("status", "")).strip() == "no_valid_lid_detection" + def main() -> int: args = parse_args() From 07ae5816a7b9d74c317b36b13a9f93b65510d72e Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Mon, 15 Jun 2026 15:32:48 +0900 Subject: [PATCH 82/88] feat: Add ElevenLabs API configuration and update cup holder offset in automation script --- .env.local | 2 ++ tools/run/run_voice_auto_cup_flow.sh | 1 + 2 files changed, 3 insertions(+) create mode 100644 .env.local diff --git a/.env.local b/.env.local new file mode 100644 index 0000000..1bb9696 --- /dev/null +++ b/.env.local @@ -0,0 +1,2 @@ +ELEVENLABS_API_KEY=sk_76cef6a2940e749d15a65bcab0013911869653ae02770a29 +ELEVENLABS_AGENT_ID=agent_7701kv4epy77erzrytfb1d4xyr27 diff --git a/tools/run/run_voice_auto_cup_flow.sh b/tools/run/run_voice_auto_cup_flow.sh index 001b659..83a89ad 100755 --- a/tools/run/run_voice_auto_cup_flow.sh +++ b/tools/run/run_voice_auto_cup_flow.sh @@ -49,6 +49,7 @@ set +e ros2 launch azas_bringup auto_cup_flow_router.launch.py \ enable_real_motion:=true \ router_confirm:=ENABLE_AUTO_CUP_ROUTER \ + cup_holder_place_x_offset_m:=3.0 \ service_prefix:="${SERVICE_PREFIX}" \ motion_service_prefix:="${MOTION_SERVICE_PREFIX}" \ moveit_controller_name:=/${SERVICE_PREFIX}/dsr_moveit_controller \ From 2a766be906fae8be69732b0fcc5643872786b126 Mon Sep 17 00:00:00 2001 From: chris3471 Date: Mon, 15 Jun 2026 16:46:46 +0900 Subject: [PATCH 83/88] Apply validated MediaPipe palm handover --- tools/run/auto_handover_on_palm.py | 270 +++++++++++++++- tools/run/handover_cup_to_palm.py | 494 +++++++++++++++++++++++++---- 2 files changed, 698 insertions(+), 66 deletions(-) diff --git a/tools/run/auto_handover_on_palm.py b/tools/run/auto_handover_on_palm.py index 1f04164..071c810 100755 --- a/tools/run/auto_handover_on_palm.py +++ b/tools/run/auto_handover_on_palm.py @@ -23,6 +23,8 @@ from __future__ import annotations import argparse +import json +import os import subprocess import sys import time @@ -30,21 +32,59 @@ ROOT = Path(__file__).resolve().parents[2] HANDOVER_SCRIPT = ROOT / "tools" / "run" / "handover_cup_to_palm.py" +DIRECT_MOVEJ = ROOT / "tools" / "run" / "direct_movej_joints.py" HAND_TOPIC = "/azas/human_hand_detection" +STATUS_TOPIC = "/azas/human_hand_detection/status" CONFIRM_PHRASE = "AUTO_HANDOVER_ON_PALM" +MOVEJ_CONFIRM_PHRASE = "ENABLE_DIRECT_MOVEJ" def wait_for_stable_palm(args: argparse.Namespace) -> bool: """Spin a perception-only node until the palm trigger fires or we time out.""" import rclpy from geometry_msgs.msg import PointStamped + from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + from std_msgs.msg import String stamps: list[float] = [] + latest_status = {"detected": False, "reason": "no status yet"} + point_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + + def status_ready(status: dict[str, object]) -> bool: + depth_m = status.get("depth_m") + if depth_m is None: + return False + depth = float(depth_m) + return ( + bool(status.get("detected")) + and bool(status.get("hand_open")) + and bool(status.get("stable")) + and args.trigger_min_depth_m <= depth <= args.trigger_max_depth_m + and int(status.get("open_fingers", 0)) >= args.min_trigger_open_fingers + ) + + def on_status(msg: String) -> None: + nonlocal latest_status + try: + latest_status = json.loads(msg.data) + except json.JSONDecodeError: + latest_status = {"raw": msg.data} + if not status_ready(latest_status): + stamps.clear() + + def on_hand(_msg: PointStamped) -> None: + if status_ready(latest_status): + stamps.append(time.monotonic()) + rclpy.init() node = rclpy.create_node("azas_auto_handover_watch") - node.create_subscription( - PointStamped, HAND_TOPIC, lambda _msg: stamps.append(time.monotonic()), 10 - ) + node.create_subscription(PointStamped, HAND_TOPIC, on_hand, point_qos) + node.create_subscription(String, STATUS_TOPIC, on_status, 10) print( f"[Azas] 손 대기 시작: {args.trigger_window_sec:.1f}초 안에 안정 검출 " f"{args.trigger_stable_count}개가 쌓이면 핸드오버를 1회 실행합니다 " @@ -58,7 +98,8 @@ def wait_for_stable_palm(args: argparse.Namespace) -> bool: rclpy.spin_once(node, timeout_sec=0.2) now = time.monotonic() stamps[:] = [t for t in stamps if now - t <= args.trigger_window_sec] - if len(stamps) >= args.trigger_stable_count: + stable_duration = (now - stamps[0]) if stamps else 0.0 + if len(stamps) >= args.trigger_stable_count and stable_duration >= args.trigger_min_stable_sec: triggered = True break if now - last_report >= 5.0: @@ -66,8 +107,9 @@ def wait_for_stable_palm(args: argparse.Namespace) -> bool: remain = deadline - now print( f"[Azas] 대기 중... 최근 {args.trigger_window_sec:.1f}초 안정 검출 " - f"{len(stamps)}/{args.trigger_stable_count}개 (남은 시간 {remain:.0f}초). " - "손바닥을 펴고 정지해 주세요." + f"{len(stamps)}/{args.trigger_stable_count}개, 지속 {stable_duration:.1f}/" + f"{args.trigger_min_stable_sec:.1f}초 (남은 시간 {remain:.0f}초). " + f"status={latest_status}. 손바닥을 펴고 정지해 주세요." ) finally: node.destroy_node() @@ -76,15 +118,146 @@ def wait_for_stable_palm(args: argparse.Namespace) -> bool: return triggered +def parse_joint_csv(value: str) -> list[float]: + joints = [float(part.strip()) for part in str(value).split(",") if part.strip()] + if len(joints) != 6: + raise ValueError(f"--observe-joints must contain 6 comma-separated values, got {len(joints)}") + return joints + + +def move_to_observe(args: argparse.Namespace) -> int: + joints = parse_joint_csv(args.observe_joints) + cmd = [ + sys.executable, str(DIRECT_MOVEJ), + "--service-prefix", args.service_prefix, + "--j1", f"{joints[0]:.6f}", + "--j2", f"{joints[1]:.6f}", + "--j3", f"{joints[2]:.6f}", + "--j4", f"{joints[3]:.6f}", + "--j5", f"{joints[4]:.6f}", + "--j6", f"{joints[5]:.6f}", + "--velocity", f"{args.observe_velocity:.3f}", + "--acceleration", f"{args.observe_acceleration:.3f}", + "--timeout-sec", f"{args.observe_timeout_sec:.1f}", + "--motion-timeout-sec", f"{args.observe_motion_timeout_sec:.1f}", + "--j5-min-deg", f"{args.observe_j5_min_deg:.3f}", + "--j5-max-deg", f"{args.observe_j5_max_deg:.3f}", + ] + if args.execute: + cmd += ["--execute", "--confirm", MOVEJ_CONFIRM_PHRASE] + print( + "[Azas] observe 위치로 먼저 이동합니다: joints_deg=[" + + ", ".join(f"{value:.1f}" for value in joints) + + f"] vel={args.observe_velocity:.1f}" + ) + rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode + if rc == 0: + print("[PASS] observe 위치 이동 완료.") + else: + print(f"[FAIL] observe 위치 이동 실패(rc={rc}); 손 대기/핸드오버를 시작하지 않습니다.") + return rc + + def main() -> int: parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--service-prefix", default=os.environ.get("SERVICE_PREFIX", "")) + parser.add_argument("--no-service-prefix-fallback", action="store_true", + help="use exactly --service-prefix in the handover script; do not fall back to dsr01") parser.add_argument("--trigger-stable-count", type=int, default=12, help="trigger when this many stable detections land inside the window") parser.add_argument("--trigger-window-sec", type=float, default=3.0) + parser.add_argument("--trigger-min-stable-sec", type=float, default=0.0, + help="also require the stable detections to span at least this many seconds") + parser.add_argument("--min-trigger-open-fingers", type=int, default=3, + help="auto trigger also requires latest status.open_fingers >= this value") + parser.add_argument("--trigger-min-depth-m", type=float, default=0.30, + help="auto trigger requires latest palm depth to be at least this close/far range") + parser.add_argument("--trigger-max-depth-m", type=float, default=0.75, + help="auto trigger rejects hands farther than this camera depth") parser.add_argument("--wait-timeout-sec", type=float, default=180.0, help="give up (exit 3, no motion) when no stable palm appears in time") parser.add_argument("--release-tcp-above-palm-m", default="0.08") + parser.add_argument("--skip-observe", action="store_true", + help="do not move to the observe/camera-home joint pose before waiting for a palm") + parser.add_argument("--observe-joints", default="3.0,-12.7,44.0,-9.0,133.0,90.0", + help="comma-separated J1..J6 degrees for the initial observe/camera-home pose") + parser.add_argument("--observe-velocity", type=float, default=45.0) + parser.add_argument("--observe-acceleration", type=float, default=60.0) + parser.add_argument("--observe-timeout-sec", type=float, default=20.0) + parser.add_argument("--observe-motion-timeout-sec", type=float, default=120.0) + parser.add_argument("--observe-j5-min-deg", type=float, default=-150.0) + parser.add_argument("--observe-j5-max-deg", type=float, default=150.0) + parser.add_argument("--hand-sample-count", type=int, default=None) + parser.add_argument("--hand-sample-timeout-sec", type=float, default=None) + parser.add_argument("--hand-sample-spread-max-m", type=float, default=None) + parser.add_argument("--hand-recheck-tolerance-m", type=float, default=None) + parser.add_argument("--skip-hand-recheck", action="store_true", + help="handover to the initially sampled palm without the pre-descent re-check") + parser.add_argument("--transit-velocity", type=float, default=75.0) + parser.add_argument("--transit-acceleration", type=float, default=95.0) + parser.add_argument("--descent-velocity", type=float, default=22.0) + parser.add_argument("--descent-acceleration", type=float, default=32.0) + parser.add_argument("--descent-step-m", type=float, default=0.03) + parser.add_argument("--max-descent-steps", type=int, default=0, + help="maximum staged descent steps; 0 means use the Z floor only") + parser.add_argument("--force-search-start-above-palm-m", type=float, default=0.16, + help="with contact release, start force-only descent this far above the detected palm") + parser.add_argument("--force-search-below-palm-m", type=float, default=0.10, + help="with contact release, search down to this far below the detected palm before aborting") + parser.add_argument("--move-timeout-sec", type=float, default=None) + parser.add_argument("--verify-timeout-sec", type=float, default=None) + parser.add_argument("--target-tolerance-mm", type=float, default=None) + parser.add_argument("--ikin-timeout-sec", type=float, default=None) + parser.add_argument("--ikin-retries", type=int, default=None) + parser.add_argument("--ikin-sol-spaces", default=None) + parser.add_argument("--j5-min-deg", type=float, default=None) + parser.add_argument("--j5-max-deg", type=float, default=None) + parser.add_argument("--skip-force-monitor", action="store_true", + help="pass through to handover script; staged descent remains but force abort is disabled") + parser.add_argument("--force-abort-delta-n", type=float, default=2.0, + help="force rise over baseline that counts as palm contact during descent") + parser.add_argument("--force-axis-delta-n", type=float, default=1.0, + help="also count contact when any single force axis changes by this much") + parser.add_argument("--contact-axis", choices=("z", "xy", "all"), default="z", + help="force axes used for contact release; z is safest for vertical handover") + parser.add_argument("--contact-z-direction", choices=("positive", "negative", "any"), default="positive", + help="when --contact-axis z, require this signed Z force delta for contact") + parser.add_argument("--contact-step-delta-n", type=float, default=2.0, + help="contact candidate also requires this force jump from the previous descent step") + parser.add_argument("--require-force-magnitude-delta", action=argparse.BooleanOptionalAction, default=True, + help="also require total force magnitude to rise before contact release") + parser.add_argument("--force-magnitude-delta-n", type=float, default=1.5, + help="minimum total force magnitude rise required with --require-force-magnitude-delta") + parser.add_argument("--force-baseline-samples", type=int, default=5, + help="average this many GetToolForce samples before descent") + parser.add_argument("--force-baseline-interval-sec", type=float, default=0.05, + help="delay between baseline force samples") + parser.add_argument("--force-read-settle-sec", type=float, default=0.15, + help="wait after each descent step before reading force") + parser.add_argument("--release-on-contact", action=argparse.BooleanOptionalAction, default=True, + help="open the gripper at the first force/contact trigger during descent") + parser.add_argument("--require-contact-for-release", action=argparse.BooleanOptionalAction, default=True, + help="with --release-on-contact, retreat with the cup if contact is never detected") + parser.add_argument("--contact-confirm-samples", type=int, default=5, + help="consecutive above-threshold force samples required before opening RG2") + parser.add_argument("--contact-confirm-min-hits", type=int, default=0, + help="minimum hit samples needed within --contact-confirm-samples; " + "0 means all samples") + parser.add_argument("--contact-confirm-interval-sec", type=float, default=0.12, + help="delay between force confirmation samples") + parser.add_argument("--contact-relief-lift-m", type=float, default=0.0, + help="deprecated/ignored: contact release now opens RG2 at the confirmed contact pose") + parser.add_argument("--contact-search-below-release-m", type=float, default=0.20, + help="with --release-on-contact, keep descending this far below release height while seeking contact") + parser.add_argument("--gripper-open-retries", type=int, default=None) + parser.add_argument("--gripper-open-retry-sleep-sec", type=float, default=None) + parser.add_argument("--x-min", type=float, default=None) + parser.add_argument("--x-max", type=float, default=None) + parser.add_argument("--y-min", type=float, default=None) + parser.add_argument("--y-max", type=float, default=None) + parser.add_argument("--z-min", type=float, default=None) + parser.add_argument("--z-max", type=float, default=None) + parser.add_argument("--palm-z-max-m", type=float, default=None) parser.add_argument("--execute", action="store_true") parser.add_argument("--confirm", default="", help=f"must equal {CONFIRM_PHRASE} with --execute") args = parser.parse_args() @@ -92,9 +265,17 @@ def main() -> int: if args.execute and args.confirm != CONFIRM_PHRASE: print(f"[BLOCKED] --execute requires --confirm {CONFIRM_PHRASE}") return 2 + if args.release_on_contact and args.skip_force_monitor: + print("[BLOCKED] contact-release mode requires force monitoring; remove --skip-force-monitor") + return 2 if not args.execute: print("[DRY-RUN] --execute 미지정: 손 트리거 후 핸드오버도 dry-run(인식+계획만)으로 실행합니다.") + if not args.skip_observe: + rc = move_to_observe(args) + if rc != 0: + return rc + if not wait_for_stable_palm(args): print( f"[FAIL] {args.wait_timeout_sec:.0f}초 안에 안정적인 손바닥이 없어 종료합니다 (로봇 모션 없음). " @@ -107,16 +288,85 @@ def main() -> int: sys.executable, str(HANDOVER_SCRIPT), "--service-prefix", args.service_prefix, "--release-tcp-above-palm-m", str(args.release_tcp_above_palm_m), - "--transit-velocity", "10.0", "--transit-acceleration", "14.0", - "--descent-velocity", "4.0", "--descent-acceleration", "6.0", - "--force-abort-delta-n", "10.0", + "--transit-velocity", f"{args.transit_velocity:.3f}", + "--transit-acceleration", f"{args.transit_acceleration:.3f}", + "--descent-velocity", f"{args.descent_velocity:.3f}", + "--descent-acceleration", f"{args.descent_acceleration:.3f}", + "--descent-step-m", f"{args.descent_step_m:.3f}", + "--max-descent-steps", str(args.max_descent_steps), + "--force-search-start-above-palm-m", f"{args.force_search_start_above_palm_m:.3f}", + "--force-search-below-palm-m", f"{args.force_search_below_palm_m:.3f}", + "--force-abort-delta-n", f"{args.force_abort_delta_n:.3f}", + "--force-axis-delta-n", f"{args.force_axis_delta_n:.3f}", + "--contact-axis", args.contact_axis, + "--contact-z-direction", args.contact_z_direction, + "--contact-step-delta-n", f"{args.contact_step_delta_n:.3f}", + "--force-magnitude-delta-n", f"{args.force_magnitude_delta_n:.3f}", + "--force-baseline-samples", str(args.force_baseline_samples), + "--force-baseline-interval-sec", f"{args.force_baseline_interval_sec:.3f}", + "--force-read-settle-sec", f"{args.force_read_settle_sec:.3f}", + "--contact-confirm-samples", str(args.contact_confirm_samples), + "--contact-confirm-min-hits", str(args.contact_confirm_min_hits), + "--contact-confirm-interval-sec", f"{args.contact_confirm_interval_sec:.3f}", + "--contact-relief-lift-m", f"{args.contact_relief_lift_m:.3f}", + "--contact-search-below-release-m", f"{args.contact_search_below_release_m:.3f}", ] + if args.hand_sample_count is not None: + cmd += ["--hand-sample-count", str(args.hand_sample_count)] + if args.hand_sample_timeout_sec is not None: + cmd += ["--hand-sample-timeout-sec", f"{args.hand_sample_timeout_sec:.1f}"] + if args.hand_sample_spread_max_m is not None: + cmd += ["--hand-sample-spread-max-m", f"{args.hand_sample_spread_max_m:.3f}"] + if args.hand_recheck_tolerance_m is not None: + cmd += ["--hand-recheck-tolerance-m", f"{args.hand_recheck_tolerance_m:.3f}"] + if args.skip_hand_recheck: + cmd += ["--skip-hand-recheck"] + if args.move_timeout_sec is not None: + cmd += ["--move-timeout-sec", f"{args.move_timeout_sec:.1f}"] + if args.verify_timeout_sec is not None: + cmd += ["--verify-timeout-sec", f"{args.verify_timeout_sec:.1f}"] + if args.target_tolerance_mm is not None: + cmd += ["--target-tolerance-mm", f"{args.target_tolerance_mm:.1f}"] + if args.ikin_timeout_sec is not None: + cmd += ["--ikin-timeout-sec", f"{args.ikin_timeout_sec:.1f}"] + if args.ikin_retries is not None: + cmd += ["--ikin-retries", str(args.ikin_retries)] + if args.ikin_sol_spaces: + cmd += ["--ikin-sol-spaces", args.ikin_sol_spaces] + if args.j5_min_deg is not None: + cmd += ["--j5-min-deg", f"{args.j5_min_deg:.3f}"] + if args.j5_max_deg is not None: + cmd += ["--j5-max-deg", f"{args.j5_max_deg:.3f}"] + if args.skip_force_monitor: + cmd += ["--skip-force-monitor"] + if args.release_on_contact: + cmd += ["--release-on-contact"] + if args.require_contact_for_release: + cmd += ["--require-contact-for-release"] + else: + cmd += ["--no-require-contact-for-release"] + if args.require_force_magnitude_delta: + cmd += ["--require-force-magnitude-delta"] + else: + cmd += ["--no-require-force-magnitude-delta"] + if args.gripper_open_retries is not None: + cmd += ["--gripper-open-retries", str(args.gripper_open_retries)] + if args.gripper_open_retry_sleep_sec is not None: + cmd += ["--gripper-open-retry-sleep-sec", f"{args.gripper_open_retry_sleep_sec:.1f}"] + for name in ("x_min", "x_max", "y_min", "y_max", "z_min", "z_max"): + value = getattr(args, name) + if value is not None: + cmd += [f"--{name.replace('_', '-')}", f"{value:.3f}"] + if args.palm_z_max_m is not None: + cmd += ["--palm-z-max-m", f"{args.palm_z_max_m:.3f}"] if args.execute: cmd += [ "--execute", "--confirm", "ENABLE_HUMAN_PALM_HANDOVER", "--approve-motion", "ENABLE_HUMAN_PALM_HANDOVER_MOTION", "--approve-release", "RELEASE_CUP_NOW", ] + if args.no_service_prefix_fallback: + cmd += ["--no-service-prefix-fallback"] rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode if rc == 0: print("[PASS] 자동 핸드오버 완료.") diff --git a/tools/run/handover_cup_to_palm.py b/tools/run/handover_cup_to_palm.py index 11c0fbd..c68796d 100755 --- a/tools/run/handover_cup_to_palm.py +++ b/tools/run/handover_cup_to_palm.py @@ -8,8 +8,8 @@ bash tools/run/run_human_hand_detection.sh) and transform the palm into base frame via live TF base_link->link_6 and the measured T_gripper2camera hand-eye calibration. - 2. PLAN compute LIFT -> ABOVE_HIGH -> ABOVE_PALM -> staged descent - -> RELEASE -> RETREAT, all with the CURRENT side-grip + 2. PLAN compute LIFT -> APPROACH -> ABOVE_PALM -> staged descent + -> RELEASE/CONTACT_RELEASE -> RETREAT, all with the CURRENT side-grip orientation preserved (--use-current-rpy on every MoveLine). 3. GATES default is dry-run. --execute needs --confirm, a typed operator approval before any motion, a hand re-check right @@ -49,6 +49,23 @@ DIRECT_CONFIRM_PHRASE = "ENABLE_DIRECT_MOVEL" +def prefixed_service(prefix: str, suffix: str) -> str: + clean = prefix.strip("/") + return f"/{clean}/{suffix}" if clean else f"/{suffix}" + + +def resolve_service_prefix(node, srv_type, requested_prefix: str, wait_sec: float, *, allow_fallback: bool) -> str: + requested = requested_prefix.strip("/") + if requested or not allow_fallback: + return requested + for candidate in ("", "dsr01"): + name = prefixed_service(candidate, "aux_control/get_current_posx") + client = node.create_client(srv_type, name) + if client.wait_for_service(timeout_sec=max(0.1, wait_sec)): + return candidate + return requested + + class HandoverPerception: """rclpy helpers: palm sampling, live TCP pose, tool force. No motion.""" @@ -57,6 +74,7 @@ def __init__(self, args: argparse.Namespace) -> None: import tf2_ros from dsr_msgs2.srv import GetCurrentPosx, GetToolForce from geometry_msgs.msg import PointStamped + from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy self.args = args self.rclpy = rclpy @@ -64,11 +82,29 @@ def __init__(self, args: argparse.Namespace) -> None: self.node = rclpy.create_node("azas_handover_cup_to_palm") self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) - prefix = args.service_prefix - self.get_posx = self.node.create_client(GetCurrentPosx, f"/{prefix}/aux_control/get_current_posx") - self.get_tool_force = self.node.create_client(GetToolForce, f"/{prefix}/aux_control/get_tool_force") + prefix = resolve_service_prefix( + self.node, + GetCurrentPosx, + args.service_prefix, + min(self.args.wait_service_sec, 1.0), + allow_fallback=not args.no_service_prefix_fallback, + ) + self.args.service_prefix = prefix + print(f"[Azas] Doosan service prefix: {prefix or ''}") + self.get_posx = self.node.create_client( + GetCurrentPosx, prefixed_service(prefix, "aux_control/get_current_posx") + ) + self.get_tool_force = self.node.create_client( + GetToolForce, prefixed_service(prefix, "aux_control/get_tool_force") + ) self.hand_points: list[tuple[float, list[float]]] = [] - self.node.create_subscription(PointStamped, HAND_TOPIC, self._on_hand, 10) + hand_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + self.node.create_subscription(PointStamped, HAND_TOPIC, self._on_hand, hand_qos) self.gripper2cam = np.load(str(args.hand_eye_npy)).astype(float) if abs(self.gripper2cam[:3, 3]).max() > 10.0: self.gripper2cam[:3, 3] /= 1000.0 @@ -114,6 +150,17 @@ def tool_force_n(self) -> list[float]: raise RuntimeError("GetToolForce returned success=false") return [float(v) for v in list(response.tool_force)[:3]] + def averaged_tool_force_n(self, *, samples: int, interval_sec: float) -> list[float]: + count = max(int(samples), 1) + total = [0.0, 0.0, 0.0] + for index in range(count): + force = self.tool_force_n() + for axis in range(3): + total[axis] += force[axis] + if index + 1 < count: + time.sleep(max(interval_sec, 0.0)) + return [value / count for value in total] + def base_to_camera(self) -> np.ndarray: import rclpy.time @@ -178,31 +225,163 @@ def sample_palm_base(self, *, label: str) -> list[float]: return palm -def run_movel(args: argparse.Namespace, xyz_m: list[float], *, label: str, velocity: float, acceleration: float) -> None: +def run_movel( + args: argparse.Namespace, + xyz_m: list[float], + *, + label: str, + velocity: float, + acceleration: float, + rpy_deg: list[float], + fallback_movej: bool = True, +) -> None: cmd = [ sys.executable, str(DIRECT_MOVEL), "--service-prefix", args.service_prefix, "--x", f"{xyz_m[0]:.6f}", "--y", f"{xyz_m[1]:.6f}", "--z", f"{xyz_m[2]:.6f}", - "--use-current-rpy", + "--rx", f"{rpy_deg[0]:.6f}", "--ry", f"{rpy_deg[1]:.6f}", "--rz", f"{rpy_deg[2]:.6f}", "--velocity", f"{velocity:.3f}", "--acceleration", f"{acceleration:.3f}", "--timeout-sec", f"{args.move_timeout_sec:.1f}", + "--motion-timeout-sec", f"{args.move_timeout_sec:.1f}", "--wait-service-sec", f"{args.wait_service_sec:.1f}", + "--verify-timeout-sec", f"{args.verify_timeout_sec:.1f}", + "--target-tolerance-mm", f"{args.target_tolerance_mm:.1f}", + "--ikin-timeout-sec", f"{args.ikin_timeout_sec:.1f}", + "--ikin-retries", str(args.ikin_retries), + "--ikin-sol-spaces", args.ikin_sol_spaces, + "--j5-min-deg", f"{args.j5_min_deg:.3f}", + "--j5-max-deg", f"{args.j5_max_deg:.3f}", "--x-min", f"{args.x_min:.3f}", "--x-max", f"{args.x_max:.3f}", "--y-min", f"{args.y_min:.3f}", "--y-max", f"{args.y_max:.3f}", "--z-min", f"{args.z_min:.3f}", "--z-max", f"{args.z_max:.3f}", ] if args.execute: cmd += ["--precheck-ikin", "--verify-target", "--execute", "--confirm", DIRECT_CONFIRM_PHRASE] + if fallback_movej: + cmd += [ + "--fallback-movej-on-verify-fail", + "--fallback-movej-velocity", f"{min(max(velocity, 5.0), args.transit_velocity):.3f}", + "--fallback-movej-acceleration", f"{min(max(acceleration, 10.0), args.transit_acceleration):.3f}", + ] print(f"[Azas] MOVE {label}: xyz_m=[{xyz_m[0]:.3f}, {xyz_m[1]:.3f}, {xyz_m[2]:.3f}] vel={velocity:.1f}") rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode if rc != 0: raise RuntimeError(f"MoveLine step failed: {label} (rc={rc})") +def monitored_axis_indices(mode: str) -> list[int]: + if mode == "all": + return [0, 1, 2] + if mode == "xy": + return [0, 1] + if mode == "z": + return [2] + raise ValueError(f"unsupported contact axis mode: {mode}") + + +def force_contact_metrics( + force: list[float], + baseline: list[float], + baseline_mag: float, + *, + contact_axis: str, +) -> tuple[float, list[float], float]: + force_mag = math.sqrt(sum(v * v for v in force)) + axis_delta = [force[i] - baseline[i] for i in range(3)] + max_axis_delta = max(abs(axis_delta[i]) for i in monitored_axis_indices(contact_axis)) + mag_delta = force_mag - baseline_mag + return mag_delta, axis_delta, max_axis_delta + + +def contact_axis_hit(axis_delta: list[float], mag_delta: float, *, args: argparse.Namespace) -> bool: + if args.require_force_magnitude_delta and mag_delta < args.force_magnitude_delta_n: + return False + if args.contact_axis == "z": + z_delta = axis_delta[2] + if args.contact_z_direction == "positive": + return z_delta > args.force_axis_delta_n + if args.contact_z_direction == "negative": + return z_delta < -args.force_axis_delta_n + return abs(z_delta) > args.force_axis_delta_n + if args.contact_axis == "xy": + return max(abs(axis_delta[0]), abs(axis_delta[1])) > args.force_axis_delta_n + return ( + max(abs(axis_delta[0]), abs(axis_delta[1]), abs(axis_delta[2])) > args.force_axis_delta_n + or mag_delta > args.force_abort_delta_n + ) + + +def contact_step_hit(force: list[float], previous_force: list[float], *, args: argparse.Namespace) -> bool: + step_delta = [force[i] - previous_force[i] for i in range(3)] + if args.contact_axis == "z": + z_delta = step_delta[2] + if args.contact_z_direction == "positive": + return z_delta > args.contact_step_delta_n + if args.contact_z_direction == "negative": + return z_delta < -args.contact_step_delta_n + return abs(z_delta) > args.contact_step_delta_n + if args.contact_axis == "xy": + return max(abs(step_delta[0]), abs(step_delta[1])) > args.contact_step_delta_n + return max(abs(step_delta[0]), abs(step_delta[1]), abs(step_delta[2])) > args.contact_step_delta_n + + +def force_contact_confirmed( + perception: HandoverPerception, + reference_force: list[float], + reference_mag: float, + *, + force_delta_n: float, + axis_delta_n: float, + contact_axis: str, + samples: int, + min_hits: int, + interval_sec: float, +) -> bool: + needed = max(int(samples), 1) + required_hits = min(max(int(min_hits), 1), needed) + hits = 0 + for index in range(needed): + force = perception.tool_force_n() + mag_delta, axis_delta, max_axis_delta = force_contact_metrics( + force, + reference_force, + reference_mag, + contact_axis=contact_axis, + ) + confirm_args = argparse.Namespace( + contact_axis=contact_axis, + contact_z_direction=perception.args.contact_z_direction, + force_axis_delta_n=axis_delta_n, + force_abort_delta_n=force_delta_n, + require_force_magnitude_delta=perception.args.require_force_magnitude_delta, + force_magnitude_delta_n=perception.args.force_magnitude_delta_n, + ) + hit = contact_axis_hit(axis_delta, mag_delta, args=confirm_args) + hits += 1 if hit else 0 + print( + "[Azas] contact confirm " + f"{index + 1}/{needed}: hit={hit} hits={hits}/{required_hits} " + f"fx={force[0]:.2f} fy={force[1]:.2f} fz={force[2]:.2f} " + f"delta_mag={mag_delta:.2f}N " + f"delta_axis=[{axis_delta[0]:.2f}, {axis_delta[1]:.2f}, {axis_delta[2]:.2f}] " + f"max_{contact_axis}_axis={max_axis_delta:.2f}N" + ) + if hits >= required_hits: + return True + remaining = needed - (index + 1) + if hits + remaining < required_hits: + return False + if index + 1 < needed: + time.sleep(max(interval_sec, 0.0)) + return hits >= required_hits + + def open_gripper(args: argparse.Namespace) -> None: env = os.environ.copy() env.setdefault("RG2_OPEN_TIMEOUT_SEC", "20.0") + env.setdefault("RG2_OPEN_RETRIES", str(args.gripper_open_retries)) + env.setdefault("RG2_OPEN_RETRY_SLEEP_SEC", f"{args.gripper_open_retry_sleep_sec:.1f}") rc = subprocess.run([str(RG2_OPEN)], cwd=str(ROOT), env=env, check=False).returncode if rc != 0: raise RuntimeError(f"RG2 open failed (rc={rc})") @@ -220,38 +399,96 @@ def require_typed_approval(phrase: str, *, prompt: str, preapproved: str = "") - def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) - parser.add_argument("--service-prefix", default="dsr01") + parser.add_argument("--service-prefix", default=os.environ.get("SERVICE_PREFIX", "")) + parser.add_argument("--no-service-prefix-fallback", action="store_true", + help="use exactly --service-prefix, including empty prefix; do not fall back to dsr01") parser.add_argument("--hand-eye-npy", type=Path, default=DEFAULT_HAND_EYE) parser.add_argument("--hand-sample-count", type=int, default=10) parser.add_argument("--hand-sample-timeout-sec", type=float, default=20.0) parser.add_argument("--hand-sample-spread-max-m", type=float, default=0.03) parser.add_argument("--hand-recheck-tolerance-m", type=float, default=0.05, help="abort if the palm moved more than this between plan and descent") + parser.add_argument("--skip-hand-recheck", action="store_true", + help="use the initially sampled palm for descent without the pre-descent palm re-check") parser.add_argument("--transit-z-m", type=float, default=0.45) + parser.add_argument("--diagonal-approach", action=argparse.BooleanOptionalAction, default=True, + help="move toward the palm with XYZ blended to the descent-start height; " + "--no-diagonal-approach keeps the older XY-at-transit-height behavior") parser.add_argument("--above-palm-m", type=float, default=0.12, help="TCP height above the palm before the staged descent") parser.add_argument("--release-tcp-above-palm-m", type=float, default=0.08, help="TCP height above the palm at release. TUNE WITH A FOAM-BLOCK " "DRY TEST FIRST: depends on where the side grip holds the cup") - parser.add_argument("--descent-step-m", type=float, default=0.02) - parser.add_argument("--force-abort-delta-n", type=float, default=10.0, + parser.add_argument("--force-search-start-above-palm-m", type=float, default=0.16, + help="with --release-on-contact, start force-only descent this far above the detected palm") + parser.add_argument("--force-search-below-palm-m", type=float, default=0.10, + help="with --release-on-contact, search down to this far below the detected palm before aborting") + parser.add_argument("--descent-step-m", type=float, default=0.03) + parser.add_argument("--max-descent-steps", type=int, default=0, + help="maximum staged descent steps; 0 means use the Z floor only") + parser.add_argument("--force-abort-delta-n", type=float, default=2.0, help="abort descent when |tool force| rises this much over the pre-descent baseline") + parser.add_argument("--force-axis-delta-n", type=float, default=1.0, + help="trigger contact when the monitored force axis changes by this much") + parser.add_argument("--contact-axis", choices=("z", "xy", "all"), default="z", + help="force axes used for contact release; z is safest for vertical handover") + parser.add_argument("--contact-z-direction", choices=("positive", "negative", "any"), default="positive", + help="when --contact-axis z, require this signed Z force delta for contact") + parser.add_argument("--contact-step-delta-n", type=float, default=2.0, + help="contact candidate also requires this force jump from the previous descent step") + parser.add_argument("--require-force-magnitude-delta", action=argparse.BooleanOptionalAction, default=True, + help="also require total force magnitude to rise before contact release") + parser.add_argument("--force-magnitude-delta-n", type=float, default=1.5, + help="minimum total force magnitude rise required with --require-force-magnitude-delta") + parser.add_argument("--force-baseline-samples", type=int, default=5, + help="average this many GetToolForce samples before descent") + parser.add_argument("--force-baseline-interval-sec", type=float, default=0.05, + help="delay between baseline force samples") + parser.add_argument("--force-read-settle-sec", type=float, default=0.15, + help="wait after each descent step before reading force") + parser.add_argument("--release-on-contact", action="store_true", + help="during staged descent, treat a force rise as palm contact: stop, open RG2, then retreat") + parser.add_argument("--require-contact-for-release", action=argparse.BooleanOptionalAction, default=True, + help="with --release-on-contact, only open RG2 after contact is detected") + parser.add_argument("--contact-confirm-samples", type=int, default=5, + help="consecutive above-threshold force samples required before opening RG2") + parser.add_argument("--contact-confirm-min-hits", type=int, default=0, + help="minimum hit samples needed within --contact-confirm-samples; " + "0 means all samples, preserving the strict default") + parser.add_argument("--contact-confirm-interval-sec", type=float, default=0.12, + help="delay between force confirmation samples") + parser.add_argument("--contact-relief-lift-m", type=float, default=0.0, + help="deprecated/ignored: contact release now opens RG2 at the confirmed contact pose") + parser.add_argument("--contact-search-below-release-m", type=float, default=0.20, + help="with --release-on-contact, keep descending this far below release height while seeking contact") parser.add_argument("--retreat-lift-m", type=float, default=0.20) - parser.add_argument("--transit-velocity", type=float, default=10.0) - parser.add_argument("--transit-acceleration", type=float, default=14.0) - parser.add_argument("--descent-velocity", type=float, default=4.0) - parser.add_argument("--descent-acceleration", type=float, default=6.0) + parser.add_argument("--transit-velocity", type=float, default=75.0) + parser.add_argument("--transit-acceleration", type=float, default=95.0) + parser.add_argument("--descent-velocity", type=float, default=22.0) + parser.add_argument("--descent-acceleration", type=float, default=32.0) # Palm workspace bounds (base frame). The palm itself must be inside these. - parser.add_argument("--x-min", type=float, default=0.25) - parser.add_argument("--x-max", type=float, default=0.75) - parser.add_argument("--y-min", type=float, default=-0.45) - parser.add_argument("--y-max", type=float, default=0.45) - parser.add_argument("--z-min", type=float, default=0.05) - parser.add_argument("--z-max", type=float, default=0.60) - parser.add_argument("--palm-z-max-m", type=float, default=0.40, + parser.add_argument("--x-min", type=float, default=0.15) + parser.add_argument("--x-max", type=float, default=1.50) + parser.add_argument("--y-min", type=float, default=-0.65) + parser.add_argument("--y-max", type=float, default=0.75) + parser.add_argument("--z-min", type=float, default=0.04) + parser.add_argument("--z-max", type=float, default=0.75) + parser.add_argument("--palm-z-max-m", type=float, default=0.50, help="reject palms higher than this (likely a mis-detection)") parser.add_argument("--move-timeout-sec", type=float, default=60.0) + parser.add_argument("--verify-timeout-sec", type=float, default=90.0) + parser.add_argument("--target-tolerance-mm", type=float, default=25.0) + parser.add_argument("--ikin-timeout-sec", type=float, default=20.0) + parser.add_argument("--ikin-retries", type=int, default=2) + parser.add_argument("--ikin-sol-spaces", default="2,0,1,3,4,5,6,7", + help="solution spaces to try for every MoveLine IK precheck") + parser.add_argument("--j5-min-deg", type=float, default=-160.0) + parser.add_argument("--j5-max-deg", type=float, default=160.0) parser.add_argument("--wait-service-sec", type=float, default=10.0) + parser.add_argument("--skip-force-monitor", action="store_true", + help="skip GetToolForce monitoring during descent; keeps staged descent and release approval") + parser.add_argument("--gripper-open-retries", type=int, default=3) + parser.add_argument("--gripper-open-retry-sleep-sec", type=float, default=1.0) parser.add_argument("--auto-release", action="store_true", help="skip the final typed release approval (NOT recommended)") parser.add_argument("--test-hand-xyz-m", default="", @@ -277,6 +514,9 @@ def main() -> int: if args.release_tcp_above_palm_m >= args.above_palm_m: print("[BLOCKED] --release-tcp-above-palm-m must be below --above-palm-m") return 2 + if args.release_on_contact and args.skip_force_monitor: + print("[BLOCKED] --release-on-contact requires force monitoring; remove --skip-force-monitor") + return 2 if not args.execute: print("[DRY-RUN] --execute not set; perception + plan only, no robot command sent.") @@ -296,18 +536,47 @@ def main() -> int: return 1 lift = [current_m[0], current_m[1], max(current_m[2], args.transit_z_m)] - above_high = [palm[0], palm[1], max(args.transit_z_m, palm[2] + args.above_palm_m)] - above_palm = [palm[0], palm[1], palm[2] + args.above_palm_m] + contact_start_z = palm[2] + max(args.force_search_start_above_palm_m, 0.0) + descent_start_z = contact_start_z if args.release_on_contact else palm[2] + args.above_palm_m + if args.diagonal_approach: + approach_z = max(args.z_min, min(args.z_max, descent_start_z)) + approach_label = "APPROACH" + else: + approach_z = max(args.transit_z_m, descent_start_z) + approach_label = "ABOVE_HIGH" + approach = [palm[0], palm[1], approach_z] + above_palm = [ + palm[0], + palm[1], + descent_start_z, + ] release = [palm[0], palm[1], palm[2] + args.release_tcp_above_palm_m] + contact_floor_z = max(args.z_min, palm[2] - max(args.force_search_below_palm_m, 0.0)) retreat = [palm[0], palm[1], palm[2] + args.retreat_lift_m] - for name, pose in (("LIFT", lift), ("ABOVE_HIGH", above_high), ("ABOVE_PALM", above_palm), - ("RELEASE", release), ("RETREAT", retreat)): + plan_items = [("LIFT", lift), (approach_label, approach), ("ABOVE_PALM", above_palm)] + if not args.release_on_contact: + plan_items.append(("RELEASE", release)) + plan_items.append(("RETREAT", retreat)) + for name, pose in plan_items: print(f"[PLAN] {name}: xyz_m=[{pose[0]:.3f}, {pose[1]:.3f}, {pose[2]:.3f}]") + if args.release_on_contact: + print(f"[PLAN] CONTACT_SEARCH_FLOOR: z_m={contact_floor_z:.3f}") + print( + "[PLAN] force-only Z search: " + f"start_z={above_palm[2]:.3f} palm_z={palm[2]:.3f} floor_z={contact_floor_z:.3f}; " + "gripper opens only after confirmed contact" + ) print( - "[PLAN] descent ABOVE_PALM -> RELEASE in " - f"{math.ceil((above_palm[2] - release[2]) / max(args.descent_step_m, 0.005))} steps of " + "[PLAN] descent ABOVE_PALM -> " + f"{'CONTACT_SEARCH_FLOOR' if args.release_on_contact else 'RELEASE'} in " + f"{math.ceil((above_palm[2] - (contact_floor_z if args.release_on_contact else release[2])) / max(args.descent_step_m, 0.005))} steps of " f"{args.descent_step_m * 1000.0:.0f}mm with force abort delta {args.force_abort_delta_n:.1f}N" ) + if args.max_descent_steps > 0: + no_contact_action = "retreat with cup" if args.require_contact_for_release else "open at final descent pose" + print(f"[PLAN] max descent steps: {args.max_descent_steps} (no confirmed contact => {no_contact_action})") + if args.release_on_contact: + print("[PLAN] contact-release mode: keep descending until force/contact trigger, then open RG2") if not args.execute: return 0 @@ -323,48 +592,161 @@ def main() -> int: ), preapproved=args.approve_motion, ) + preserved_rpy = current[3:6] run_movel(args, lift, label="LIFT to transit height (Z-only)", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) - run_movel(args, above_high, label="ABOVE_HIGH over palm at transit height", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) - run_movel(args, above_palm, label="ABOVE_PALM vertical pre-descent", - velocity=args.descent_velocity, acceleration=args.descent_acceleration) - - # Hand must still be where we planned; people move. - recheck = perception.sample_palm_base(label="palm re-check before descent") - moved = math.dist(recheck, palm) - if moved > args.hand_recheck_tolerance_m: - print(f"[ABORT] palm moved {moved * 1000.0:.0f}mm since planning; retreating without descent") - run_movel(args, retreat, label="RETREAT after palm moved", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) - return 1 + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + approach_motion_label = ( + "APPROACH to palm descent start (XYZ blended)" + if args.diagonal_approach else + "ABOVE_HIGH over palm at transit height" + ) + run_movel(args, approach, label=approach_motion_label, + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + if math.dist(approach, above_palm) > 0.001: + run_movel(args, above_palm, label="ABOVE_PALM vertical pre-descent", + velocity=args.descent_velocity, acceleration=args.descent_acceleration, + rpy_deg=preserved_rpy) + else: + print("[Azas] ABOVE_PALM equals approach target; skipping duplicate pre-descent move") - baseline = perception.tool_force_n() - baseline_mag = math.sqrt(sum(v * v for v in baseline)) + # Hand must still be where we planned; people move. This can be skipped + # when the camera re-check is known to jump after arm motion. + if args.skip_hand_recheck: + print("[Azas] palm re-check skipped; descending to the initially sampled palm target") + else: + recheck = perception.sample_palm_base(label="palm re-check before descent") + moved = math.dist(recheck, palm) + if moved > args.hand_recheck_tolerance_m: + print(f"[ABORT] palm moved {moved * 1000.0:.0f}mm since planning; retreating without descent") + run_movel(args, retreat, label="RETREAT after palm moved", + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + return 1 + + baseline = [0.0, 0.0, 0.0] + baseline_mag = 0.0 + if args.skip_force_monitor: + print("[Azas] force monitor skipped by operator option") + else: + baseline = perception.averaged_tool_force_n( + samples=args.force_baseline_samples, + interval_sec=args.force_baseline_interval_sec, + ) + baseline_mag = math.sqrt(sum(v * v for v in baseline)) + print( + "[Azas] force baseline: " + f"fx={baseline[0]:.2f} fy={baseline[1]:.2f} fz={baseline[2]:.2f} " + f"|f|={baseline_mag:.2f}N contact_axis={args.contact_axis} " + f"contact_z_direction={args.contact_z_direction}" + ) z = above_palm[2] - while z > release[2] + 1e-6: - z = max(z - max(args.descent_step_m, 0.005), release[2]) + contact_release = False + descent_floor_z = contact_floor_z if args.release_on_contact else release[2] + previous_force = list(baseline) + descent_step_index = 0 + while z > descent_floor_z + 1e-6: + if args.max_descent_steps > 0 and descent_step_index >= args.max_descent_steps: + print(f"[Azas] max descent steps reached ({args.max_descent_steps}) without confirmed contact") + break + descent_step_index += 1 + z = max(z - max(args.descent_step_m, 0.005), descent_floor_z) run_movel(args, [palm[0], palm[1], z], label=f"descent step to z={z:.3f}m", - velocity=args.descent_velocity, acceleration=args.descent_acceleration) - force = perception.tool_force_n() - force_mag = math.sqrt(sum(v * v for v in force)) - print(f"[Azas] tool force {force_mag:.1f}N (baseline {baseline_mag:.1f}N)") - if force_mag - baseline_mag > args.force_abort_delta_n: - print("[ABORT] force spike during descent (palm contact or obstruction); retreating with cup") - run_movel(args, retreat, label="RETREAT after force abort", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) + velocity=args.descent_velocity, acceleration=args.descent_acceleration, + rpy_deg=preserved_rpy) + if not args.skip_force_monitor: + time.sleep(max(args.force_read_settle_sec, 0.0)) + force = perception.tool_force_n() + force_mag = math.sqrt(sum(v * v for v in force)) + mag_delta, axis_delta, max_axis_delta = force_contact_metrics( + force, + baseline, + baseline_mag, + contact_axis=args.contact_axis, + ) + print( + "[Azas] tool force " + f"fx={force[0]:.2f} fy={force[1]:.2f} fz={force[2]:.2f} |f|={force_mag:.2f}N " + f"delta_mag={mag_delta:.2f}N " + f"delta_axis=[{axis_delta[0]:.2f}, {axis_delta[1]:.2f}, {axis_delta[2]:.2f}] " + f"step_delta=[{force[0] - previous_force[0]:.2f}, " + f"{force[1] - previous_force[1]:.2f}, {force[2] - previous_force[2]:.2f}] " + f"max_{args.contact_axis}_axis={max_axis_delta:.2f}N " + f"z_direction={args.contact_z_direction}" + ) + contact_candidate = ( + contact_axis_hit(axis_delta, mag_delta, args=args) + and contact_step_hit(force, previous_force, args=args) + ) + if contact_candidate: + if args.release_on_contact: + print("[Azas] contact candidate detected; checking confirmation samples before RG2 open") + candidate_reference_force = list(previous_force) + candidate_reference_mag = math.sqrt(sum(v * v for v in candidate_reference_force)) + if not force_contact_confirmed( + perception, + candidate_reference_force, + candidate_reference_mag, + force_delta_n=args.force_abort_delta_n, + axis_delta_n=args.force_axis_delta_n, + contact_axis=args.contact_axis, + samples=args.contact_confirm_samples, + min_hits=args.contact_confirm_min_hits or args.contact_confirm_samples, + interval_sec=args.contact_confirm_interval_sec, + ): + print( + "[Azas] contact candidate was not confirmed; " + "treating it as force noise and continuing descent" + ) + previous_force = force + continue + contact_release = True + print( + "[Azas] contact trigger during descent: " + f"delta_mag={mag_delta:.2f}N(limit {args.force_abort_delta_n:.2f}), " + f"max_{args.contact_axis}_axis={max_axis_delta:.2f}N(limit {args.force_axis_delta_n:.2f}); " + "opening RG2 at the contact candidate pose" + ) + break + print("[ABORT] force spike during descent (palm contact or obstruction); retreating with cup") + run_movel(args, retreat, label="RETREAT after force abort", + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + return 1 + previous_force = force + + if args.release_on_contact and not contact_release: + print("[Azas] contact search floor reached without contact trigger") + if args.require_contact_for_release: + print("[ABORT] contact was not detected; retreating with cup") + run_movel(args, retreat, label="RETREAT after no contact", + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) return 1 + if args.release_on_contact and args.require_contact_for_release and not contact_release: + print("[ABORT] fail-closed: contact release was not confirmed; gripper will stay closed") + run_movel(args, retreat, label="RETREAT after unconfirmed contact release", + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) + return 1 + if not args.auto_release: require_typed_approval( RELEASE_APPROVAL_PHRASE, - prompt="[Azas] Cup is at release height. Confirm the palm is directly under the cup.", + prompt=( + "[Azas] Contact detected; confirm the palm is supporting the cup." + if contact_release else + "[Azas] Cup is at release height. Confirm the palm is directly under the cup." + ), preapproved=args.approve_release, ) open_gripper(args) time.sleep(1.0) run_movel(args, retreat, label="RETREAT vertical after release", - velocity=args.transit_velocity, acceleration=args.transit_acceleration) + velocity=args.transit_velocity, acceleration=args.transit_acceleration, + rpy_deg=preserved_rpy) print("[PASS] palm handover sequence completed") return 0 except RuntimeError as exc: From b6fde53b9b2f367c5fb2b230604131619d3718a7 Mon Sep 17 00:00:00 2001 From: chris3471 Date: Mon, 15 Jun 2026 16:53:05 +0900 Subject: [PATCH 84/88] Connect post-shake MediaPipe handover --- .../launch/auto_cup_flow_router.launch.py | 43 ++++ .../azas_task_manager/auto_cup_flow_router.py | 193 ++++++++++++++++++ .../auto_flow_resume_state.py | 3 + tools/checks/check_image_latency.py | 88 ++++++++ tools/run/with_azas_ros_env.sh | 27 +++ tools/view/low_latency_image_view.py | 103 ++++++++++ 6 files changed, 457 insertions(+) create mode 100755 tools/checks/check_image_latency.py create mode 100755 tools/run/with_azas_ros_env.sh create mode 100755 tools/view/low_latency_image_view.py diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index cb6cca1..9f404a2 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -39,6 +39,26 @@ def generate_launch_description(): DeclareLaunchArgument("cup_holder_rz_offset_deg", default_value="-1.0"), DeclareLaunchArgument("cup_holder_z_min_m", default_value="0.06"), DeclareLaunchArgument("lid_shake_after_recipe", default_value="true"), + DeclareLaunchArgument("human_handover_after_shake", default_value="true"), + DeclareLaunchArgument("human_handover_command", default_value=""), + DeclareLaunchArgument("human_handover_auto_start_camera", default_value="true"), + DeclareLaunchArgument("human_handover_auto_start_detection", default_value="true"), + DeclareLaunchArgument( + "human_handover_camera_command", + default_value="tools/run/with_azas_ros_env.sh ros2 launch realsense2_camera rs_align_depth_launch.py", + ), + DeclareLaunchArgument( + "human_handover_detection_command", + default_value=( + "tools/run/with_azas_ros_env.sh bash tools/run/run_human_hand_detection.sh " + "--process-width-px 320 --overlay-width-px 640 --max-rate-hz 20 " + "--min-detection-confidence 0.35 --min-tracking-confidence 0.35 " + "--min-extended-fingers 3 --depth-window-px 21 --stable-radius-m 0.12 " + "--stable-min-samples 2 --stable-window-seconds 1.0" + ), + ), + DeclareLaunchArgument("human_handover_camera_ready_timeout_sec", default_value="30.0"), + DeclareLaunchArgument("human_handover_detection_ready_timeout_sec", default_value="30.0"), DeclareLaunchArgument("holder_pick_shake_command", default_value=""), DeclareLaunchArgument("shake_only_command", default_value=""), DeclareLaunchArgument("resume_mode", default_value="normal"), @@ -92,6 +112,29 @@ def generate_launch_description(): "cup_holder_rz_offset_deg": ParameterValue(LaunchConfiguration("cup_holder_rz_offset_deg"), value_type=float), "cup_holder_z_min_m": ParameterValue(LaunchConfiguration("cup_holder_z_min_m"), value_type=float), "lid_shake_after_recipe": ParameterValue(LaunchConfiguration("lid_shake_after_recipe"), value_type=bool), + "human_handover_after_shake": ParameterValue( + LaunchConfiguration("human_handover_after_shake"), + value_type=bool, + ), + "human_handover_command": LaunchConfiguration("human_handover_command"), + "human_handover_auto_start_camera": ParameterValue( + LaunchConfiguration("human_handover_auto_start_camera"), + value_type=bool, + ), + "human_handover_auto_start_detection": ParameterValue( + LaunchConfiguration("human_handover_auto_start_detection"), + value_type=bool, + ), + "human_handover_camera_command": LaunchConfiguration("human_handover_camera_command"), + "human_handover_detection_command": LaunchConfiguration("human_handover_detection_command"), + "human_handover_camera_ready_timeout_sec": ParameterValue( + LaunchConfiguration("human_handover_camera_ready_timeout_sec"), + value_type=float, + ), + "human_handover_detection_ready_timeout_sec": ParameterValue( + LaunchConfiguration("human_handover_detection_ready_timeout_sec"), + value_type=float, + ), "holder_pick_shake_command": LaunchConfiguration("holder_pick_shake_command"), "shake_only_command": LaunchConfiguration("shake_only_command"), "resume_mode": LaunchConfiguration("resume_mode"), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index ff51c62..3b446dc 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -127,6 +127,30 @@ def __init__(self) -> None: "lid_shake_command", "bash /home/ssu/Azas/tools/run/run_lid_close_then_shake_chain.sh", ) + self.declare_parameter("human_handover_after_shake", True) + self.declare_parameter("human_handover_command", "") + self.declare_parameter("human_handover_auto_start_camera", True) + self.declare_parameter("human_handover_auto_start_detection", True) + self.declare_parameter( + "human_handover_camera_command", + "tools/run/with_azas_ros_env.sh ros2 launch realsense2_camera rs_align_depth_launch.py", + ) + self.declare_parameter( + "human_handover_detection_command", + "tools/run/with_azas_ros_env.sh bash tools/run/run_human_hand_detection.sh " + "--process-width-px 320 " + "--overlay-width-px 640 " + "--max-rate-hz 20 " + "--min-detection-confidence 0.35 " + "--min-tracking-confidence 0.35 " + "--min-extended-fingers 3 " + "--depth-window-px 21 " + "--stable-radius-m 0.12 " + "--stable-min-samples 2 " + "--stable-window-seconds 1.0", + ) + self.declare_parameter("human_handover_camera_ready_timeout_sec", 30.0) + self.declare_parameter("human_handover_detection_ready_timeout_sec", 30.0) self.declare_parameter("holder_pick_shake_command", "") self.declare_parameter("shake_only_command", "") self.declare_parameter("resume_mode", "normal") @@ -196,6 +220,13 @@ def run(self) -> int: held_objects={"cup": "gripper", "lid": "on_cup"}, ): return 1 + if not self._run_resumable_stage( + "human_handover", + self._run_human_handover_sequence, + verified={"human_handover_done": True}, + held_objects={"cup": "human", "lid": "on_cup"}, + ): + return 1 if self._resume_store is not None: self._resume_store.complete_run() self.get_logger().info("auto cup router: selected flow completed; router exiting") @@ -711,6 +742,143 @@ def _run_lid_shake_sequence(self) -> bool: self.get_logger().info("recipe succeeded; starting lid close -> holder re-pick -> shake chain") return self._run_process(["bash", "-c", command], "lid_shake") + def _run_human_handover_sequence(self) -> bool: + if not bool(self.get_parameter("human_handover_after_shake").value): + self.get_logger().info("human_handover_after_shake=false; skipping MediaPipe palm handover") + return True + command = self._human_handover_command() + if not command: + self.get_logger().warning("human_handover_command is empty; skipping MediaPipe palm handover") + return True + helpers = self._start_human_handover_support_processes() + self.get_logger().info("shake succeeded; starting MediaPipe palm handover") + try: + return self._run_process(["bash", "-c", command], "human_handover") + finally: + for proc, label in helpers: + self._stop_process(proc, label) + + def _start_human_handover_support_processes(self) -> list[tuple[subprocess.Popen[str], str]]: + helpers: list[tuple[subprocess.Popen[str], str]] = [] + color_topic = "/camera/camera/color/image_raw" + depth_topic = "/camera/camera/aligned_depth_to_color/image_raw" + hand_overlay_topic = "/azas/human_hand_detection/overlay" + + if bool(self.get_parameter("human_handover_auto_start_camera").value): + if self._topic_has_publishers(color_topic) and self._topic_has_publishers(depth_topic): + self.get_logger().info("human handover camera topics already have publishers") + else: + command = str(self.get_parameter("human_handover_camera_command").value or "").strip() + if command: + self.get_logger().info("starting human handover camera support process") + helpers.append((self._popen(shlex.split(command), "human_handover_camera"), "human_handover_camera")) + if not self._wait_for_topic_publishers( + [color_topic, depth_topic], + timeout_sec=float(self.get_parameter("human_handover_camera_ready_timeout_sec").value), + label="human handover camera", + ): + raise RuntimeError("human handover camera topics did not become ready") + + if bool(self.get_parameter("human_handover_auto_start_detection").value): + if self._topic_has_publishers(hand_overlay_topic): + self.get_logger().info("human hand detection overlay already has a publisher") + else: + command = str(self.get_parameter("human_handover_detection_command").value or "").strip() + if command: + self.get_logger().info("starting MediaPipe human hand detection support process") + helpers.append((self._popen(shlex.split(command), "human_hand_detection"), "human_hand_detection")) + if not self._wait_for_topic_publishers( + [hand_overlay_topic], + timeout_sec=float(self.get_parameter("human_handover_detection_ready_timeout_sec").value), + label="MediaPipe human hand detection", + ): + raise RuntimeError("MediaPipe human hand detection overlay did not become ready") + return helpers + + def _topic_has_publishers(self, topic: str) -> bool: + try: + return bool(self.get_publishers_info_by_topic(topic)) + except Exception as exc: + self.get_logger().warn(f"topic publisher check failed for {topic}: {exc}") + return False + + def _wait_for_topic_publishers(self, topics: list[str], *, timeout_sec: float, label: str) -> bool: + deadline = time.monotonic() + max(0.0, timeout_sec) + missing = list(topics) + while time.monotonic() < deadline: + rclpy.spin_once(self, timeout_sec=0.1) + missing = [topic for topic in topics if not self._topic_has_publishers(topic)] + if not missing: + self.get_logger().info(f"{label}: topic publishers ready") + return True + time.sleep(0.2) + self.get_logger().error(f"{label}: missing topic publishers: {', '.join(missing)}") + return False + + def _human_handover_command(self) -> str: + configured = str(self.get_parameter("human_handover_command").value or "").strip() + if configured: + return configured + prefix = self._motion_service_prefix() + return ( + "cd /home/ssu/Azas && " + "tools/run/with_azas_ros_env.sh python3 tools/run/auto_handover_on_palm.py " + f"--service-prefix {shlex.quote(prefix)} " + "--no-service-prefix-fallback " + "--execute " + "--confirm AUTO_HANDOVER_ON_PALM " + "--trigger-stable-count 2 " + "--trigger-window-sec 1.5 " + "--trigger-min-stable-sec 1.0 " + "--trigger-min-depth-m 0.30 " + "--trigger-max-depth-m 0.75 " + "--skip-observe " + "--hand-sample-count 1 " + "--hand-sample-timeout-sec 5 " + "--hand-sample-spread-max-m 0.05 " + "--skip-hand-recheck " + "--release-on-contact " + "--no-require-contact-for-release " + "--force-search-start-above-palm-m 0.16 " + "--force-search-below-palm-m 0.10 " + "--max-descent-steps 10 " + "--contact-axis z " + "--contact-z-direction positive " + "--force-baseline-samples 5 " + "--force-baseline-interval-sec 0.05 " + "--force-read-settle-sec 0.08 " + "--force-abort-delta-n 3.5 " + "--force-axis-delta-n 3.5 " + "--contact-step-delta-n 2.5 " + "--require-force-magnitude-delta " + "--force-magnitude-delta-n 2.0 " + "--contact-confirm-samples 3 " + "--contact-confirm-min-hits 3 " + "--contact-confirm-interval-sec 0.08 " + "--descent-step-m 0.030 " + "--transit-velocity 55 " + "--transit-acceleration 75 " + "--descent-velocity 22 " + "--descent-acceleration 32 " + "--move-timeout-sec 90 " + "--verify-timeout-sec 120 " + "--target-tolerance-mm 35 " + "--ikin-timeout-sec 25 " + "--ikin-retries 2 " + "--ikin-sol-spaces 2,0,1,3,4,5,6,7 " + "--j5-min-deg -160 " + "--j5-max-deg 160 " + "--gripper-open-retries 5 " + "--gripper-open-retry-sleep-sec 1.5 " + "--x-min 0.10 " + "--x-max 1.50 " + "--y-min -0.65 " + "--y-max 0.75 " + "--z-min 0.02 " + "--z-max 0.75 " + "--palm-z-max-m 0.50" + ) + def _lid_shake_command_for_current_resume_state(self) -> str: if self._resume_store is None: return str(self.get_parameter("lid_shake_command").value).strip() @@ -891,6 +1059,8 @@ def _record_child_progress_from_output(self, label: str, text: str) -> None: self._record_side_grasp_progress(text) if label == "lid_shake": self._record_lid_shake_progress(text) + if label == "human_handover": + self._record_human_handover_progress(text) def _record_side_grasp_progress(self, text: str) -> None: if "Could not find a connection between 'world' and 'camera_" in text: @@ -947,6 +1117,29 @@ def _record_lid_shake_progress(self, text: str) -> None: self._stage_failure_reasons["lid_shake"] = "lid_shake_hardware_blocked" self._resume_store.update_progress("lid_shake", "hardware_blocked") + def _record_human_handover_progress(self, text: str) -> None: + if self._resume_store is None: + return + if "손 대기 시작" in text: + self._resume_store.update_progress( + "human_handover", + "waiting_for_open_palm", + held_objects={"cup": "gripper", "lid": "on_cup"}, + ) + elif "손 트리거 충족" in text: + self._resume_store.update_progress( + "human_handover", + "palm_triggered", + held_objects={"cup": "gripper", "lid": "on_cup"}, + ) + elif "[PASS] 자동 핸드오버 완료." in text or "[PASS] palm handover sequence completed" in text: + self._resume_store.update_progress( + "human_handover", + "handover_done", + verified={"human_handover_done": True}, + held_objects={"cup": "human", "lid": "on_cup"}, + ) + @staticmethod def _lid_status_payload(text: str) -> dict[str, object] | None: match = re.search(r"lid_grip_status=[^\s]+\s+payload=(\{.*\})", text) diff --git a/src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py b/src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py index d565f6a..5fac3e0 100644 --- a/src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py +++ b/src/azas_task_manager/azas_task_manager/auto_flow_resume_state.py @@ -19,6 +19,7 @@ "cup_pick", "recipe", "lid_shake", + "human_handover", ) STAGE_LABELS = { @@ -28,6 +29,7 @@ "cup_pick": "cup route and pick", "recipe": "measured dispenser recipe", "lid_shake": "lid close and shake", + "human_handover": "MediaPipe palm handover", } @@ -152,6 +154,7 @@ def _new_snapshot(self, *, status: str) -> dict[str, Any]: "lid_grasped": False, "lid_closed": False, "shake_done": False, + "human_handover_done": False, }, "stop_reason": None, "blocker": None, diff --git a/tools/checks/check_image_latency.py b/tools/checks/check_image_latency.py new file mode 100755 index 0000000..35136e3 --- /dev/null +++ b/tools/checks/check_image_latency.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +"""Measure ROS Image topic rate and header age. + +Useful for separating camera delay from perception/viewer delay: + - raw camera topic has low age: camera/DDS is fine + - overlay topic has high age: perception or viewer path is lagging +""" +from __future__ import annotations + +import argparse +import statistics +import time + +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import Image + + +LOW_LATENCY_QOS = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, +) + + +class ImageLatencyCheck(Node): + def __init__(self, topic: str, samples: int) -> None: + super().__init__("azas_image_latency_check") + self.topic = topic + self.samples = samples + self.ages_ms: list[float] = [] + self.arrival_times: list[float] = [] + self.create_subscription(Image, topic, self.on_image, LOW_LATENCY_QOS) + + def on_image(self, msg: Image) -> None: + now = self.get_clock().now() + stamp = rclpy.time.Time.from_msg(msg.header.stamp) + age_ms = (now - stamp).nanoseconds / 1_000_000.0 + self.ages_ms.append(age_ms) + self.arrival_times.append(time.monotonic()) + + @property + def done(self) -> bool: + return len(self.ages_ms) >= self.samples + + +def percentile(values: list[float], pct: float) -> float: + ordered = sorted(values) + index = min(len(ordered) - 1, max(0, round((pct / 100.0) * (len(ordered) - 1)))) + return ordered[index] + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("topic") + parser.add_argument("--samples", type=int, default=120) + parser.add_argument("--timeout-sec", type=float, default=10.0) + args = parser.parse_args() + + rclpy.init() + node = ImageLatencyCheck(args.topic, args.samples) + deadline = time.monotonic() + args.timeout_sec + try: + while rclpy.ok() and not node.done and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.05) + if not node.ages_ms: + print(f"[FAIL] no image samples received from {args.topic}") + return 2 + duration = max(node.arrival_times[-1] - node.arrival_times[0], 1e-6) + rate = (len(node.arrival_times) - 1) / duration if len(node.arrival_times) > 1 else 0.0 + print(f"topic: {args.topic}") + print(f"samples: {len(node.ages_ms)}") + print(f"rate_hz: {rate:.2f}") + print(f"age_ms_avg: {statistics.mean(node.ages_ms):.1f}") + print(f"age_ms_p50: {statistics.median(node.ages_ms):.1f}") + print(f"age_ms_p95: {percentile(node.ages_ms, 95):.1f}") + print(f"age_ms_max: {max(node.ages_ms):.1f}") + return 0 + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run/with_azas_ros_env.sh b/tools/run/with_azas_ros_env.sh new file mode 100755 index 0000000..8856f00 --- /dev/null +++ b/tools/run/with_azas_ros_env.sh @@ -0,0 +1,27 @@ +#!/usr/bin/env bash +# Run a ROS command with the Azas field defaults. +# This avoids asking operators to remember ROS_DOMAIN_ID / DDS env exports. +set -euo pipefail + +ROOT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd)" + +set +u +source /opt/ros/humble/setup.bash + +if [[ -f /home/ssu/ws_moveit/install/setup.bash ]]; then + source /home/ssu/ws_moveit/install/setup.bash +fi +if [[ -f /home/ssu/ros2_ws/install/setup.bash ]]; then + source /home/ssu/ros2_ws/install/setup.bash +fi +if [[ -f "${ROOT_DIR}/install/setup.bash" ]]; then + source "${ROOT_DIR}/install/setup.bash" +fi +set -u + +export ROS_DOMAIN_ID="${AZAS_ROS_DOMAIN_ID:-9}" +export ROS_LOCALHOST_ONLY="${AZAS_ROS_LOCALHOST_ONLY:-1}" +export FASTDDS_BUILTIN_TRANSPORTS="${FASTDDS_BUILTIN_TRANSPORTS:-UDPv4}" +export MPLCONFIGDIR="${MPLCONFIGDIR:-/tmp/azas_mpl_config}" + +exec "$@" diff --git a/tools/view/low_latency_image_view.py b/tools/view/low_latency_image_view.py new file mode 100755 index 0000000..9b13924 --- /dev/null +++ b/tools/view/low_latency_image_view.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +"""Low-latency ROS Image viewer. + +rqt_image_view is convenient, but in field testing it can appear to lag when +old image messages queue up. This viewer subscribes with BEST_EFFORT/depth=1 +and only displays the newest frame. +""" +from __future__ import annotations + +import argparse +import time + +import cv2 +import numpy as np +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import Image + + +LOW_LATENCY_QOS = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, +) + + +def image_msg_to_bgr(msg: Image) -> np.ndarray: + if msg.encoding == "bgr8": + return np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3).copy() + if msg.encoding == "rgb8": + rgb = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width, 3) + return cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + if msg.encoding == "mono8": + mono = np.frombuffer(msg.data, dtype=np.uint8).reshape(msg.height, msg.width) + return cv2.cvtColor(mono, cv2.COLOR_GRAY2BGR) + if msg.encoding == "16UC1": + depth = np.frombuffer(msg.data, dtype=np.uint16).reshape(msg.height, msg.width) + normalized = cv2.normalize(depth, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8) + return cv2.cvtColor(normalized, cv2.COLOR_GRAY2BGR) + raise ValueError(f"unsupported image encoding: {msg.encoding}") + + +class LowLatencyImageView(Node): + def __init__(self, topic: str, window_name: str) -> None: + super().__init__("azas_low_latency_image_view") + self.topic = topic + self.window_name = window_name + self.latest: Image | None = None + self.last_fps_report = time.monotonic() + self.frames = 0 + self.create_subscription(Image, topic, self.on_image, LOW_LATENCY_QOS) + self.get_logger().info(f"viewing {topic} with BEST_EFFORT depth=1") + + def on_image(self, msg: Image) -> None: + self.latest = msg + + def show_once(self) -> bool: + if self.latest is None: + return True + msg = self.latest + self.latest = None + try: + frame = image_msg_to_bgr(msg) + except ValueError as exc: + self.get_logger().error(str(exc)) + return False + self.frames += 1 + now = time.monotonic() + if now - self.last_fps_report >= 1.0: + fps = self.frames / (now - self.last_fps_report) + self.frames = 0 + self.last_fps_report = now + cv2.setWindowTitle(self.window_name, f"{self.topic} {fps:.1f} fps") + cv2.imshow(self.window_name, frame) + return cv2.waitKey(1) not in (27, ord("q")) + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("topic", nargs="?", default="/azas/human_hand_detection/overlay") + parser.add_argument("--window-name", default="Azas Low Latency Image View") + args = parser.parse_args() + + rclpy.init() + node = LowLatencyImageView(args.topic, args.window_name) + cv2.namedWindow(args.window_name, cv2.WINDOW_NORMAL) + try: + while rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0.001) + if not node.show_once(): + break + finally: + cv2.destroyAllWindows() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) From 311b3ad6dd36d1cad5042d95d1af3f388d407e6d Mon Sep 17 00:00:00 2001 From: shining-b-02 Date: Mon, 15 Jun 2026 18:23:06 +0900 Subject: [PATCH 85/88] side gripe target tcp --- .../azas_task_manager/auto_cup_flow_router.py | 37 +- .../dsr_practice/yolo_cup_pick_node.py | 567 ++++++++++++++---- .../launch/yolo_cup_pick_node.launch.py | 189 +++++- 3 files changed, 681 insertions(+), 112 deletions(-) diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 3b446dc..094f49b 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -88,6 +88,16 @@ def __init__(self) -> None: self.declare_parameter("side_target_x_offset_m", -0.02) self.declare_parameter("side_trajectory_execution_duration_scaling", 3.0) self.declare_parameter("side_trajectory_execution_goal_margin_sec", 3.0) + self.declare_parameter("side_cup_collision_enabled", True) + self.declare_parameter("side_cup_collision_radius_m", 0.045) + self.declare_parameter("side_cup_collision_height_m", 0.120) + self.declare_parameter("side_cup_collision_padding_m", 0.015) + self.declare_parameter("side_lid_collision_enabled", True) + self.declare_parameter("side_lid_collision_radius_m", 0.055) + self.declare_parameter("side_lid_collision_height_m", 0.025) + self.declare_parameter("side_lid_collision_padding_m", 0.010) + self.declare_parameter("side_cup_collision_clear_before_close", True) + self.declare_parameter("side_cup_collision_update_wait_sec", 0.15) self.declare_parameter("color_scan_at_start", True) self.declare_parameter( @@ -536,6 +546,12 @@ def _compact_status(status: str) -> str: parts.append(f"{key}={value}") return " ".join(parts) if parts else status[:120] + @staticmethod + def _bool_launch_arg(value) -> bool: + if isinstance(value, str): + return str(value).strip().lower() in {"1", "true", "yes", "on"} + return bool(value) + def _run_side_grasp(self, decision: RouteDecision) -> bool: self.get_logger().info(f"route=side_grasp: launching existing side grasp flow ({decision.status})") helpers = self._start_side_grasp_support_processes() @@ -543,6 +559,15 @@ def _run_side_grasp(self, decision: RouteDecision) -> bool: cmd.extend([ "auto_pick:=true", "grasp_mode:=side", + "motion_link:=gripper_tcp", + "camera_reference_link:=link_6", + "side_tcp_compensation_enabled:=true", + "side_tcp_reach_m:=0.213", + "side_tcp_stage_offset_m:=0.200", + "side_tcp_pre_offset_m:=0.100", + "side_tcp_close_offset_m:=0.055", + "side_candidate_axes:=y_axis", + "side_grasp_axis:=y_axis", "exit_after_pick:=true", "move_to_camera_home:=false", "skip_initial_home_move:=true", @@ -565,7 +590,17 @@ def _run_side_grasp(self, decision: RouteDecision) -> bool: "side_move_to_initial_center_before_close:=false", "side_linear_approach_enabled:=true", "side_low_retry_lift_m:=0.03", - "side_low_retry_attempts:=5", + "side_low_retry_attempts:=0", + f"side_cup_collision_enabled:={str(self._bool_launch_arg(self.get_parameter('side_cup_collision_enabled').value)).lower()}", + f"side_cup_collision_radius_m:={float(self.get_parameter('side_cup_collision_radius_m').value)}", + f"side_cup_collision_height_m:={float(self.get_parameter('side_cup_collision_height_m').value)}", + f"side_cup_collision_padding_m:={float(self.get_parameter('side_cup_collision_padding_m').value)}", + f"side_lid_collision_enabled:={str(self._bool_launch_arg(self.get_parameter('side_lid_collision_enabled').value)).lower()}", + f"side_lid_collision_radius_m:={float(self.get_parameter('side_lid_collision_radius_m').value)}", + f"side_lid_collision_height_m:={float(self.get_parameter('side_lid_collision_height_m').value)}", + f"side_lid_collision_padding_m:={float(self.get_parameter('side_lid_collision_padding_m').value)}", + f"side_cup_collision_clear_before_close:={str(self._bool_launch_arg(self.get_parameter('side_cup_collision_clear_before_close').value)).lower()}", + f"side_cup_collision_update_wait_sec:={float(self.get_parameter('side_cup_collision_update_wait_sec').value)}", "workspace_xy_clamp_enabled:=false", "table_collision_enabled:=true", "workspace_collision_scene_enabled:=false", diff --git a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py index 38fe04f..1c45822 100644 --- a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -28,7 +28,8 @@ GROUP_NAME = "manipulator" BASE_FRAME = "base_link" -EE_LINK = "link_6" +DEFAULT_MOTION_LINK = "gripper_tcp" +DEFAULT_CAMERA_REFERENCE_LINK = "link_6" HOME_JOINTS = { "joint_1": math.radians(0.0), @@ -92,6 +93,14 @@ class SideGraspPlan: place_approach_z: float side_direction: float close_backoff_m: float + stage_offset_m: float + pre_offset_m: float + guarded_offset_m: float + detected_cup_xyz: np.ndarray | None = None + target_offset_xy: np.ndarray | None = None + tcp_compensated: bool = False + legacy_stage_offset_m: float = 0.0 + legacy_guarded_offset_m: float = 0.0 score: float = 0.0 @@ -193,15 +202,15 @@ def quat_dict_from_euler(roll_deg, pitch_deg, yaw_deg): } -def get_ee_matrix(moveit_robot): +def get_link_matrix(moveit_robot, link_name): psm = moveit_robot.get_planning_scene_monitor() with psm.read_only() as scene: - transform = scene.current_state.get_global_link_transform(EE_LINK) + transform = scene.current_state.get_global_link_transform(link_name) return np.asarray(transform, dtype=float) -def get_ee_matrix_from_robot_state(robot_state): - transform = robot_state.get_global_link_transform(EE_LINK) +def get_link_matrix_from_robot_state(robot_state, link_name): + transform = robot_state.get_global_link_transform(link_name) return np.asarray(transform, dtype=float) @@ -227,8 +236,16 @@ def __init__(self): self.declare_parameter("redetect_on_approach", True) self.declare_parameter("redetect_settle_sec", 0.5) self.declare_parameter("grasp_mode", "side") + self.declare_parameter("motion_link", DEFAULT_MOTION_LINK) + self.declare_parameter("camera_reference_link", DEFAULT_CAMERA_REFERENCE_LINK) + self.declare_parameter("side_tcp_compensation_enabled", True) + self.declare_parameter("side_tcp_reach_m", 0.213) + self.declare_parameter("side_tcp_stage_offset_m", 0.120) + self.declare_parameter("side_tcp_pre_offset_m", 0.100) + self.declare_parameter("side_tcp_close_offset_m", 0.055) dynamic_param = ParameterDescriptor(dynamic_typing=True) self.declare_parameter("side_grasp_axis", "y_axis", dynamic_param) + self.declare_parameter("side_candidate_axes", "y", dynamic_param) self.declare_parameter("side_grasp_direction", 1.0) self.declare_parameter("side_approach_offset", 0.16) self.declare_parameter("side_staging_offset", 0.30) @@ -242,14 +259,26 @@ def __init__(self): self.declare_parameter("side_grasp_stop_backoff_m", 0.04) self.declare_parameter("side_close_underreach_m", 0.03) self.declare_parameter("side_low_retry_lift_m", 0.03) - self.declare_parameter("side_low_retry_attempts", 5) + self.declare_parameter("side_low_retry_attempts", 0) self.declare_parameter("side_auto_direction_by_cup_y", False) self.declare_parameter("side_candidate_plan_check_enabled", True) self.declare_parameter("side_linear_approach_enabled", True) self.declare_parameter("side_final_slide_enabled", False) - self.declare_parameter("side_fixed_grasp_z_enabled", False) + self.declare_parameter("side_fixed_grasp_z_enabled", True) self.declare_parameter("side_fixed_grasp_z", 0.07) self.declare_parameter("side_project_bbox_center_to_fixed_z", True) + self.declare_parameter("side_cup_collision_enabled", True) + self.declare_parameter("side_cup_collision_id", "side_grip_detected_cup") + self.declare_parameter("side_cup_collision_radius_m", 0.045) + self.declare_parameter("side_cup_collision_height_m", 0.120) + self.declare_parameter("side_cup_collision_padding_m", 0.015) + self.declare_parameter("side_lid_collision_enabled", True) + self.declare_parameter("side_lid_collision_id", "side_grip_detected_lid") + self.declare_parameter("side_lid_collision_radius_m", 0.055) + self.declare_parameter("side_lid_collision_height_m", 0.025) + self.declare_parameter("side_lid_collision_padding_m", 0.010) + self.declare_parameter("side_cup_collision_clear_before_close", True) + self.declare_parameter("side_cup_collision_update_wait_sec", 0.15) self.declare_parameter("table_collision_enabled", True) self.declare_parameter("table_collision_id", "side_grip_table") self.declare_parameter("table_surface_z", 0.0) @@ -399,7 +428,37 @@ def __init__(self): ) self.redetect_settle_sec = float(self.get_parameter("redetect_settle_sec").value) self.grasp_mode = str(self.get_parameter("grasp_mode").value).strip().lower() + self.motion_link = ( + str(self.get_parameter("motion_link").value).strip() + or DEFAULT_MOTION_LINK + ) + self.camera_reference_link = ( + str(self.get_parameter("camera_reference_link").value).strip() + or DEFAULT_CAMERA_REFERENCE_LINK + ) + self.side_tcp_compensation_enabled = parse_bool( + self.get_parameter("side_tcp_compensation_enabled").value + ) + self.side_tcp_reach_m = max( + 0.0, + float(self.get_parameter("side_tcp_reach_m").value), + ) + self.side_tcp_stage_offset_m = max( + 0.0, + float(self.get_parameter("side_tcp_stage_offset_m").value), + ) + self.side_tcp_pre_offset_m = max( + 0.0, + float(self.get_parameter("side_tcp_pre_offset_m").value), + ) + self.side_tcp_close_offset_m = max( + 0.0, + float(self.get_parameter("side_tcp_close_offset_m").value), + ) self.side_grasp_axis = parse_axis(self.get_parameter("side_grasp_axis").value) + self.side_candidate_axes = parse_axis( + self.get_parameter("side_candidate_axes").value + ) self.side_grasp_direction = float( self.get_parameter("side_grasp_direction").value ) @@ -434,7 +493,7 @@ def __init__(self): 0.0, float(self.get_parameter("side_low_retry_lift_m").value) ) self.side_low_retry_attempts = max( - 0, int(self.get_parameter("side_low_retry_attempts").value) + 0, int(float(self.get_parameter("side_low_retry_attempts").value)) ) self.side_auto_direction_by_cup_y = parse_bool( self.get_parameter("side_auto_direction_by_cup_y").value @@ -457,6 +516,49 @@ def __init__(self): self.side_project_bbox_center_to_fixed_z = parse_bool( self.get_parameter("side_project_bbox_center_to_fixed_z").value ) + self.side_cup_collision_enabled = parse_bool( + self.get_parameter("side_cup_collision_enabled").value + ) + self.side_cup_collision_id = str( + self.get_parameter("side_cup_collision_id").value + ).strip() + self.side_cup_collision_radius_m = max( + 0.001, + float(self.get_parameter("side_cup_collision_radius_m").value), + ) + self.side_cup_collision_height_m = max( + 0.001, + float(self.get_parameter("side_cup_collision_height_m").value), + ) + self.side_cup_collision_padding_m = max( + 0.0, + float(self.get_parameter("side_cup_collision_padding_m").value), + ) + self.side_lid_collision_enabled = parse_bool( + self.get_parameter("side_lid_collision_enabled").value + ) + self.side_lid_collision_id = str( + self.get_parameter("side_lid_collision_id").value + ).strip() + self.side_lid_collision_radius_m = max( + 0.001, + float(self.get_parameter("side_lid_collision_radius_m").value), + ) + self.side_lid_collision_height_m = max( + 0.001, + float(self.get_parameter("side_lid_collision_height_m").value), + ) + self.side_lid_collision_padding_m = max( + 0.0, + float(self.get_parameter("side_lid_collision_padding_m").value), + ) + self.side_cup_collision_clear_before_close = parse_bool( + self.get_parameter("side_cup_collision_clear_before_close").value + ) + self.side_cup_collision_update_wait_sec = max( + 0.0, + float(self.get_parameter("side_cup_collision_update_wait_sec").value), + ) self.table_collision_enabled = parse_bool( self.get_parameter("table_collision_enabled").value ) @@ -617,8 +719,18 @@ def __init__(self): raise ValueError("grasp_mode must be 'side' or 'top'") if self.camera_home_mode not in {"joint", "pose"}: raise ValueError("camera_home_mode must be 'joint' or 'pose'") - if self.side_grasp_axis not in {"x", "y"}: - raise ValueError("side_grasp_axis must be 'x' or 'y'") + if self.side_grasp_axis != "y": + self.get_logger().warning( + f"side_grasp_axis={self.side_grasp_axis!r} is no longer supported; " + "forcing y-axis side grasp." + ) + self.side_grasp_axis = "y" + if self.side_candidate_axes != "y": + self.get_logger().warning( + f"side_candidate_axes={self.side_candidate_axes!r} requested; " + "X-axis side-grip candidates are disabled, using y-axis only." + ) + self.side_candidate_axes = "y" self.side_grasp_direction = 1.0 if self.side_grasp_direction >= 0 else -1.0 if self.side_orientation_mode not in {"approach", "euler", "home"}: raise ValueError( @@ -641,6 +753,13 @@ def __init__(self): "to avoid pushing the cup after reaching the side close pose." ) self.side_final_slide_enabled = False + if self.side_fixed_grasp_z_enabled and self.side_low_retry_attempts > 0: + self.get_logger().warning( + "side_low_retry_attempts was requested with fixed side grasp Z; " + "disabling raised-Z retries to keep the close pose at " + f"side_fixed_grasp_z={self.side_fixed_grasp_z:.3f} m." + ) + self.side_low_retry_attempts = 0 if ( self.side_fixed_grasp_z_enabled and self.side_fixed_grasp_z < self.min_motion_z @@ -653,13 +772,35 @@ def __init__(self): if self.side_fixed_grasp_z_enabled: self.get_logger().info( "side_fixed_grasp_z is interpreted as a base_link Z target for " - f"{EE_LINK}; table/cup/lid geometry is not inferred from it." + f"{self.motion_link}; table/cup/lid geometry is not inferred from it." + ) + self.get_logger().info( + f"Motion pose targets use link {self.motion_link!r}; " + f"camera hand-eye transforms use link {self.camera_reference_link!r}." + ) + if self.side_tcp_compensation_active(): + self.get_logger().info( + "side TCP compensation enabled: legacy link_6 side offsets will be " + f"converted for {self.motion_link!r} " + f"(reach={self.side_tcp_reach_m:.3f} m, " + f"stage_min={self.side_tcp_stage_offset_m:.3f} m, " + f"pre_min={self.side_tcp_pre_offset_m:.3f} m, " + f"close_min={self.side_tcp_close_offset_m:.3f} m)." ) if abs(self.side_target_x_offset_m) > 1e-6: self.get_logger().warning( "side_target_x_offset_m applies only to side-grip motion planning; " f"detected cup poses are left unchanged (offset={self.side_target_x_offset_m:.3f} m)." ) + if self.side_cup_collision_enabled: + self.get_logger().info( + "Temporary detected cup/lid collision is enabled for side gross motion " + f"(cup_id={self.side_cup_collision_id!r}, " + f"cup_radius={self.side_cup_collision_radius_m + self.side_cup_collision_padding_m:.3f} m, " + f"cup_height={self.side_cup_collision_height_m:.3f} m, " + f"lid_enabled={self.side_lid_collision_enabled}, " + f"clear_before_close={self.side_cup_collision_clear_before_close})." + ) if not self.table_collision_enabled: self.get_logger().warning( "table_collision_enabled=false: MoveIt will only clamp the EE target Z, " @@ -670,6 +811,7 @@ def __init__(self): self.depth_image = None self.intrinsics = None self.last_detection = None + self.last_detections = [] self.picking = False self.has_picked_once = False self.last_pick_time = 0.0 @@ -860,6 +1002,149 @@ def make_box_collision_object(self, object_id, center_xyz, size_xyz): collision_object.operation = CollisionObject.ADD return collision_object + def make_cylinder_collision_object(self, object_id, center_xyz, height, radius): + collision_object = CollisionObject() + collision_object.id = object_id + collision_object.header.frame_id = BASE_FRAME + + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.CYLINDER + primitive.dimensions = [float(height), float(radius)] + + pose = Pose() + pose.position.x = float(center_xyz[0]) + pose.position.y = float(center_xyz[1]) + pose.position.z = float(center_xyz[2]) + pose.orientation.w = 1.0 + + collision_object.primitives.append(primitive) + collision_object.primitive_poses.append(pose) + collision_object.operation = CollisionObject.ADD + return collision_object + + def make_remove_collision_object(self, object_id): + collision_object = CollisionObject() + collision_object.id = object_id + collision_object.header.frame_id = BASE_FRAME + collision_object.operation = CollisionObject.REMOVE + return collision_object + + def side_lid_collision_xyz_for_cup(self, cup_base_xyz): + cup_xyz = np.array([float(v) for v in cup_base_xyz], dtype=float) + detections = [ + det for det in getattr(self, "last_detections", []) + if det.get("class_name") == "lid" + ] + if not detections: + return cup_xyz + + candidates = [] + for det in detections: + projected = self.bbox_center_to_fixed_base_z( + det["bbox"], + self.side_fixed_grasp_z, + ) + lid_xyz = None + if projected is not None: + lid_xyz = np.array(projected[0], dtype=float) + else: + depth_info = self.depth_from_bbox(det["bbox"], log_reason=False) + if depth_info is not None: + u, v, z_m = depth_info + lid_xyz = self.camera_to_base(self.pixel_to_camera(u, v, z_m)) + if lid_xyz is None: + continue + score = float(np.linalg.norm(lid_xyz[:2] - cup_xyz[:2])) + candidates.append((score, lid_xyz)) + + if not candidates: + return cup_xyz + return min(candidates, key=lambda item: item[0])[1] + + def publish_side_cup_collision_if_enabled(self, cup_base_xyz): + if not self.side_cup_collision_enabled: + return False + if self.collision_object_pub is None: + self.get_logger().warning( + "side cup/lid collision requested but collision publisher is not ready" + ) + return False + + cup_xyz = np.array([float(v) for v in cup_base_xyz], dtype=float) + collision_objects = [] + + cup_id = self.side_cup_collision_id or "side_grip_detected_cup" + cup_radius = self.side_cup_collision_radius_m + self.side_cup_collision_padding_m + cup_height = self.side_cup_collision_height_m + cup_center_z = self.table_surface_z + cup_height * 0.5 + collision_objects.append( + self.make_cylinder_collision_object( + cup_id, + [cup_xyz[0], cup_xyz[1], cup_center_z], + cup_height, + cup_radius, + ) + ) + + if self.side_lid_collision_enabled: + lid_xyz = self.side_lid_collision_xyz_for_cup(cup_xyz) + lid_id = self.side_lid_collision_id or "side_grip_detected_lid" + lid_radius = self.side_lid_collision_radius_m + self.side_lid_collision_padding_m + lid_height = self.side_lid_collision_height_m + lid_center_z = ( + self.table_surface_z + + self.side_cup_collision_height_m + + lid_height * 0.5 + ) + collision_objects.append( + self.make_cylinder_collision_object( + lid_id, + [lid_xyz[0], lid_xyz[1], lid_center_z], + lid_height, + lid_radius, + ) + ) + + for _ in range(self.table_publish_repeats): + for collision_object in collision_objects: + self.collision_object_pub.publish(collision_object) + time.sleep(0.05) + if self.side_cup_collision_update_wait_sec > 0.0: + time.sleep(self.side_cup_collision_update_wait_sec) + + details = [] + for collision_object in collision_objects: + pose = collision_object.primitive_poses[0].position + radius = collision_object.primitives[0].dimensions[1] + height = collision_object.primitives[0].dimensions[0] + details.append( + f"{collision_object.id}=({pose.x:.3f}, {pose.y:.3f}, {pose.z:.3f}, " + f"r={radius:.3f}, h={height:.3f})" + ) + self.get_logger().info( + "Added temporary side cup/lid collision objects: " + "; ".join(details) + ) + return True + + def remove_side_cup_collision_if_enabled(self): + if not self.side_cup_collision_enabled: + return + if self.collision_object_pub is None: + return + object_ids = [self.side_cup_collision_id or "side_grip_detected_cup"] + if self.side_lid_collision_enabled: + object_ids.append(self.side_lid_collision_id or "side_grip_detected_lid") + remove_objects = [self.make_remove_collision_object(object_id) for object_id in object_ids] + for _ in range(self.table_publish_repeats): + for remove_object in remove_objects: + self.collision_object_pub.publish(remove_object) + time.sleep(0.05) + if self.side_cup_collision_update_wait_sec > 0.0: + time.sleep(self.side_cup_collision_update_wait_sec) + self.get_logger().info( + "Removed temporary side cup/lid collision objects: " + ", ".join(object_ids) + ) + def publish_table_collision_if_enabled(self): if not self.table_collision_enabled: return @@ -1141,11 +1426,11 @@ def plan_and_execute( start_state = self.current_robot_state_from_joint_states(timeout_sec=1.0) if start_state is not None: self.arm.set_start_state(robot_state=start_state) - start_matrix = get_ee_matrix_from_robot_state(start_state) + start_matrix = get_link_matrix_from_robot_state(start_state, self.motion_link) else: log.warning("Could not seed MoveIt start state from /joint_states; falling back to current state") self.arm.set_start_state_to_current_state() - start_matrix = get_ee_matrix(self.robot) + start_matrix = get_link_matrix(self.robot, self.motion_link) start_xyz = start_matrix[:3, 3].copy() goal_xyz = None @@ -1171,7 +1456,7 @@ def plan_and_execute( f"Planning pose goal -> ({x:.3f}, {y:.3f}, {z:.3f}) " f"from ({start_xyz[0]:.3f}, {start_xyz[1]:.3f}, {start_xyz[2]:.3f})" ) - self.arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + self.arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=self.motion_link) elif state_goal is not None: log.info( f"Planning joint/state goal from EE " @@ -1199,10 +1484,10 @@ def plan_and_execute( end_state = self.current_robot_state_from_joint_states(timeout_sec=1.0) if end_state is not None: - end_matrix = get_ee_matrix_from_robot_state(end_state) + end_matrix = get_link_matrix_from_robot_state(end_state, self.motion_link) else: log.warning("Could not verify EE pose from /joint_states; falling back to planning scene") - end_matrix = get_ee_matrix(self.robot) + end_matrix = get_link_matrix(self.robot, self.motion_link) end_xyz = end_matrix[:3, 3].copy() moved = float(np.linalg.norm(end_xyz - start_xyz)) requested_pose_delta = ( @@ -1275,7 +1560,7 @@ def can_plan_pose_goal(self, pose_goal, params=None, label="candidate"): if not self.validate_workspace_goal(x, y, z, label): return False log.info(f"{label}: plan-check pose -> ({x:.3f}, {y:.3f}, {z:.3f})") - self.arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=EE_LINK) + self.arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link=self.motion_link) plan_result = self.arm.plan(parameters=params) if params else self.arm.plan() if not plan_result: log.warning(f"{label}: plan-check failed") @@ -1334,7 +1619,7 @@ def move_joint_home(self): ): return False - transform = get_ee_matrix(self.robot) + transform = get_link_matrix(self.robot, self.motion_link) self.update_home_orientation_from_matrix(transform) return True @@ -1364,7 +1649,7 @@ def move_camera_joint_home(self): ): return False - transform = get_ee_matrix(self.robot) + transform = get_link_matrix(self.robot, self.motion_link) self.update_home_orientation_from_matrix(transform) return True @@ -1495,7 +1780,7 @@ def move_camera_home(self): continue self.camera_home_z = z - transform = get_ee_matrix(self.robot) + transform = get_link_matrix(self.robot, self.motion_link) self.update_home_orientation_from_matrix(transform) return True @@ -1545,6 +1830,7 @@ def detect_objects(self, image): boxes = results[0].boxes if boxes is None or len(boxes) == 0: self.last_detection = None + self.last_detections = [] return [] detections = [] @@ -1574,6 +1860,7 @@ def detect_objects(self, image): key=lambda det: det["conf"], ) + self.last_detections = detections return detections def depth_candidates_from_bbox(self, bbox): @@ -1645,8 +1932,8 @@ def pixel_to_camera(self, u, v, z_m): def camera_to_base(self, camera_xyz): coord = np.append(camera_xyz, 1.0) - base2ee = get_ee_matrix(self.robot) - base2cam = base2ee @ self.gripper2cam + base2camera_reference = get_link_matrix(self.robot, self.camera_reference_link) + base2cam = base2camera_reference @ self.gripper2cam return (base2cam @ coord)[:3] def bbox_center_to_fixed_base_z(self, bbox, base_z): @@ -1660,8 +1947,8 @@ def bbox_center_to_fixed_base_z(self, bbox, base_z): cx = self.intrinsics["cx"] cy = self.intrinsics["cy"] ray_camera = np.array([(u - cx) / fx, (v - cy) / fy, 1.0], dtype=float) - base2ee = get_ee_matrix(self.robot) - base2cam = base2ee @ self.gripper2cam + base2camera_reference = get_link_matrix(self.robot, self.camera_reference_link) + base2cam = base2camera_reference @ self.gripper2cam origin_base = base2cam[:3, 3] ray_base = base2cam[:3, :3] @ ray_camera if abs(float(ray_base[2])) < 1e-6: @@ -1675,43 +1962,40 @@ def bbox_center_to_fixed_base_z(self, bbox, base_z): def side_direction_for_cup(self, cup_base_xyz): direction = self.side_grasp_direction - if self.side_grasp_axis == "y" and self.side_auto_direction_by_cup_y: + if self.side_auto_direction_by_cup_y: direction = -1.0 if float(cup_base_xyz[1]) >= self.side_prepose_split_y else 1.0 - if self.side_grasp_axis == "y": - cup_y = float(cup_base_xyz[1]) - candidates = [direction, -direction] - stage_offset = ( - self.side_staging_offset - if self.side_far_stage_enabled - else self.side_approach_offset + self.side_short_stage_backoff_m - ) - - def y_violation(candidate_direction): - stage_y = cup_y + candidate_direction * stage_offset - return max( - self.side_stage_y_min - stage_y, - 0.0, - stage_y - self.side_stage_y_max, - ) + cup_y = float(cup_base_xyz[1]) + candidates = [direction, -direction] + stage_offset = ( + self.side_staging_offset + if self.side_far_stage_enabled + else self.side_approach_offset + self.side_short_stage_backoff_m + ) - best_direction = min(candidates, key=y_violation) - if best_direction != direction and y_violation(best_direction) < y_violation(direction): - old_stage_y = cup_y + direction * stage_offset - new_stage_y = cup_y + best_direction * stage_offset - self.get_logger().warning( - "side staging Y would leave reachable workspace; " - f"flipping side direction {direction:.0f}->{best_direction:.0f} " - f"(stage_y {old_stage_y:.3f}->{new_stage_y:.3f}, " - f"limit=[{self.side_stage_y_min:.3f}, {self.side_stage_y_max:.3f}])" - ) - return best_direction + def y_violation(candidate_direction): + stage_y = cup_y + candidate_direction * stage_offset + return max( + self.side_stage_y_min - stage_y, + 0.0, + stage_y - self.side_stage_y_max, + ) + + best_direction = min(candidates, key=y_violation) + if best_direction != direction and y_violation(best_direction) < y_violation(direction): + old_stage_y = cup_y + direction * stage_offset + new_stage_y = cup_y + best_direction * stage_offset + self.get_logger().warning( + "side staging Y would leave reachable workspace; " + f"flipping side direction {direction:.0f}->{best_direction:.0f} " + f"(stage_y {old_stage_y:.3f}->{new_stage_y:.3f}, " + f"limit=[{self.side_stage_y_min:.3f}, {self.side_stage_y_max:.3f}])" + ) + return best_direction return direction def side_direction_candidates(self, cup_base_xyz): - first = self.side_direction_for_cup(cup_base_xyz) - second = -first - return [first, second] + return [self.side_direction_for_cup(cup_base_xyz)] def side_unit_vector(self, cup_base_xyz=None, direction=None): if direction is None: @@ -1720,8 +2004,6 @@ def side_unit_vector(self, cup_base_xyz=None, direction=None): if cup_base_xyz is not None else self.side_grasp_direction ) - if self.side_grasp_axis == "x": - return np.array([direction, 0.0], dtype=float) return np.array([0.0, direction], dtype=float) def side_grasp_orientation(self, side_vec): @@ -1771,9 +2053,6 @@ def side_plan_score(self, plan: SideGraspPlan, current_xyz): dtype=float, ) distance_score = float(np.linalg.norm(stage_goal - current_xyz)) - if self.side_grasp_axis != "y": - return distance_score - y_values = [ float(plan.stage_xy[1]), float(plan.pre_xy[1]), @@ -1785,6 +2064,16 @@ def side_plan_score(self, plan: SideGraspPlan, current_xyz): ) return distance_score + 10.0 * y_violation + def side_tcp_compensation_active(self): + return ( + self.side_tcp_compensation_enabled + and self.motion_link != self.camera_reference_link + and self.side_tcp_reach_m > 1e-6 + ) + + def tcp_compensated_side_offset(self, legacy_offset, minimum_offset): + return max(float(legacy_offset) - self.side_tcp_reach_m, float(minimum_offset)) + def compute_side_grasp_plan(self, cup_base_xyz, side_direction=None) -> SideGraspPlan: cup_xyz = np.array([float(v) for v in cup_base_xyz], dtype=float) if side_direction is None: @@ -1798,10 +2087,41 @@ def compute_side_grasp_plan(self, cup_base_xyz, side_direction=None) -> SideGras if self.side_far_stage_enabled else self.side_approach_offset + self.side_short_stage_backoff_m ) + legacy_stage_offset = stage_offset + legacy_pre_offset = self.side_approach_offset + legacy_grasp_offset = self.side_grasp_offset + legacy_close_backoff_m = ( + self.side_grasp_stop_backoff_m + self.side_close_underreach_m + ) + legacy_guarded_offset = legacy_grasp_offset + legacy_close_backoff_m + tcp_compensated = self.side_tcp_compensation_active() + if tcp_compensated: + stage_offset = self.tcp_compensated_side_offset( + legacy_stage_offset, + self.side_tcp_stage_offset_m, + ) + pre_offset = self.tcp_compensated_side_offset( + legacy_pre_offset, + self.side_tcp_pre_offset_m, + ) + guarded_offset = self.tcp_compensated_side_offset( + legacy_guarded_offset, + self.side_tcp_close_offset_m, + ) + grasp_offset = min( + max(legacy_grasp_offset - self.side_tcp_reach_m, 0.0), + guarded_offset, + ) + close_backoff_m = max(0.0, guarded_offset - grasp_offset) + else: + pre_offset = legacy_pre_offset + grasp_offset = legacy_grasp_offset + close_backoff_m = legacy_close_backoff_m + guarded_offset = legacy_guarded_offset + stage_xy = cup_xy + side_vec * stage_offset - pre_xy = cup_xy + side_vec * self.side_approach_offset - grasp_xy = cup_xy + side_vec * self.side_grasp_offset - close_backoff_m = self.side_grasp_stop_backoff_m + self.side_close_underreach_m + pre_xy = cup_xy + side_vec * pre_offset + grasp_xy = cup_xy + side_vec * grasp_offset guarded_grasp_xy = grasp_xy + side_vec * close_backoff_m if self.side_fixed_grasp_z_enabled: grasp_z = max(self.side_fixed_grasp_z, self.min_motion_z) @@ -1826,30 +2146,46 @@ def compute_side_grasp_plan(self, cup_base_xyz, side_direction=None) -> SideGras place_approach_z=place_approach_z, side_direction=side_direction, close_backoff_m=close_backoff_m, + stage_offset_m=stage_offset, + pre_offset_m=pre_offset, + guarded_offset_m=guarded_offset, + tcp_compensated=tcp_compensated, + legacy_stage_offset_m=legacy_stage_offset, + legacy_guarded_offset_m=legacy_guarded_offset, ) def build_side_grasp_candidates(self, cup_base_xyz): - current_xyz = get_ee_matrix(self.robot)[:3, 3].copy() + current_xyz = get_link_matrix(self.robot, self.motion_link)[:3, 3].copy() candidates = [] for direction in self.side_direction_candidates(cup_base_xyz): plan = self.compute_side_grasp_plan(cup_base_xyz, direction) + plan.detected_cup_xyz = np.array([float(v) for v in cup_base_xyz], dtype=float) + plan.target_offset_xy = np.array([0.0, 0.0], dtype=float) plan.score = self.side_plan_score(plan, current_xyz) candidates.append(plan) return sorted(candidates, key=lambda candidate: candidate.score) def log_side_grasp_plan(self, plan: SideGraspPlan, prefix="Side grasp target"): bx, by, bz = [float(v) for v in plan.cup_xyz] + compensation_detail = "" + if plan.tcp_compensated: + compensation_detail = ( + f", tcp_comp=on(stage {plan.legacy_stage_offset_m:.3f}->{plan.stage_offset_m:.3f}, " + f"close {plan.legacy_guarded_offset_m:.3f}->{plan.guarded_offset_m:.3f})" + ) self.get_logger().info( f"{prefix} base=({bx:.3f}, {by:.3f}, {bz:.3f}), " f"axis={self.side_grasp_axis}, dir={plan.side_direction:.0f}, " f"ori_mode={self.side_orientation_mode}, " - f"tool_roll={self.side_tool_roll_deg:.1f}deg, " + f"tool_roll={self.side_tool_roll_deg:.1f}deg" + f"{compensation_detail}, " f"stage=({plan.stage_xy[0]:.3f}, {plan.stage_xy[1]:.3f}, {plan.pre_z:.3f}), " f"pre=({plan.pre_xy[0]:.3f}, {plan.pre_xy[1]:.3f}, {plan.pre_z:.3f}), " f"grasp=({plan.grasp_xy[0]:.3f}, {plan.grasp_xy[1]:.3f}, {plan.grasp_z:.3f}), " f"guarded=({plan.guarded_grasp_xy[0]:.3f}, {plan.guarded_grasp_xy[1]:.3f}, " f"{plan.grasp_z:.3f}), close_backoff={plan.close_backoff_m:.3f}m " - f"(stop={self.side_grasp_stop_backoff_m:.3f}+underreach={self.side_close_underreach_m:.3f}), " + f"offsets(stage={plan.stage_offset_m:.3f}, pre={plan.pre_offset_m:.3f}, " + f"guarded={plan.guarded_offset_m:.3f}), " f"place_z={plan.place_z:.3f}, " f"linear_final={self.side_linear_approach_enabled}, " f"final_slide={self.side_final_slide_enabled}, " @@ -2023,6 +2359,13 @@ def execute_side_grasp_plan(self, plan: SideGraspPlan): if active_pre_z is None: return False + if self.side_cup_collision_clear_before_close: + log.info( + "clear temporary side cup/lid collision before final close approach " + "so the gripper can intentionally contact the cup" + ) + self.remove_side_cup_collision_if_enabled() + log.info(f"{side_close_label} (z={active_pre_z:.3f})") if not self.plan_and_execute( pose_goal=make_pose( @@ -2065,53 +2408,61 @@ def pick_and_place_side(self, base_xyz): refined_base = self.center_check_redetect(initial_base) cup_base = initial_base if refined_base is None else np.array(refined_base, dtype=float) - planning_cup_base = self.apply_side_target_offset(cup_base) - candidates = self.build_side_grasp_candidates(planning_cup_base) - if not candidates: - log.error("No side-grasp candidates generated") - return False + cup_collision_added = self.publish_side_cup_collision_if_enabled(cup_base) + try: + planning_cup_base = self.apply_side_target_offset(cup_base) + candidates = self.build_side_grasp_candidates(planning_cup_base) + for candidate in candidates: + candidate.detected_cup_xyz = np.array(cup_base, dtype=float) + candidate.target_offset_xy = planning_cup_base[:2] - cup_base[:2] + if not candidates: + log.error("No side-grasp candidates generated") + return False - for idx, candidate in enumerate(candidates, start=1): - log.info( - f"Side candidate {idx}: dir={candidate.side_direction:.0f}, " - f"score={candidate.score:.3f}, " - f"ready=({candidate.stage_xy[0]:.3f}, {candidate.stage_xy[1]:.3f}, {candidate.lift_z:.3f}), " - f"close=({candidate.guarded_grasp_xy[0]:.3f}, {candidate.guarded_grasp_xy[1]:.3f}, {candidate.pre_z:.3f})" - ) + for idx, candidate in enumerate(candidates, start=1): + log.info( + f"Side candidate {idx}: dir={candidate.side_direction:.0f}, " + f"score={candidate.score:.3f}, " + f"ready=({candidate.stage_xy[0]:.3f}, {candidate.stage_xy[1]:.3f}, {candidate.lift_z:.3f}), " + f"close=({candidate.guarded_grasp_xy[0]:.3f}, {candidate.guarded_grasp_xy[1]:.3f}, {candidate.pre_z:.3f})" + ) - if not self.move_to_side_prepose_if_configured(planning_cup_base): - return False - if not self.move_joint1_clearance_before_side_grip(): - return False + if not self.move_to_side_prepose_if_configured(planning_cup_base): + return False + if not self.move_joint1_clearance_before_side_grip(): + return False - for candidate in candidates: - if self.side_candidate_plan_check_enabled: - ready_pose = make_pose( - candidate.stage_xy[0], - candidate.stage_xy[1], - candidate.lift_z, - candidate.orientation, + for candidate in candidates: + if self.side_candidate_plan_check_enabled: + ready_pose = make_pose( + candidate.stage_xy[0], + candidate.stage_xy[1], + candidate.lift_z, + candidate.orientation, + ) + if not self.can_plan_pose_goal( + ready_pose, + self.ompl_params, + label=f"side candidate dir={candidate.side_direction:.0f} ready", + ): + continue + + self.log_side_grasp_plan( + candidate, + prefix=f"Selected side-grasp candidate dir={candidate.side_direction:.0f}", + ) + if self.execute_side_grasp_plan(candidate): + return True + log.warning( + f"Side candidate dir={candidate.side_direction:.0f} failed before gripper close; " + "trying next candidate if available" ) - if not self.can_plan_pose_goal( - ready_pose, - self.ompl_params, - label=f"side candidate dir={candidate.side_direction:.0f} ready", - ): - continue - - self.log_side_grasp_plan( - candidate, - prefix=f"Selected side-grasp candidate dir={candidate.side_direction:.0f}", - ) - if self.execute_side_grasp_plan(candidate): - return True - log.warning( - f"Side candidate dir={candidate.side_direction:.0f} failed before gripper close; " - "trying next candidate if available" - ) - log.error("No feasible side-grasp candidate succeeded") - return False + log.error("No feasible side-grasp candidate succeeded") + return False + finally: + if cup_collision_added: + self.remove_side_cup_collision_if_enabled() def pick_and_place_top(self, base_xyz): log = self.get_logger() diff --git a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py index 9b33e70..3be4683 100644 --- a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py +++ b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py @@ -220,10 +220,35 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params LaunchConfiguration("grasp_mode"), value_type=str, ), + "motion_link": ParameterValue( + LaunchConfiguration("motion_link"), + value_type=str, + ), + "camera_reference_link": ParameterValue( + LaunchConfiguration("camera_reference_link"), + value_type=str, + ), + "side_tcp_compensation_enabled": LaunchConfiguration( + "side_tcp_compensation_enabled" + ), + "side_tcp_reach_m": LaunchConfiguration("side_tcp_reach_m"), + "side_tcp_stage_offset_m": LaunchConfiguration( + "side_tcp_stage_offset_m" + ), + "side_tcp_pre_offset_m": LaunchConfiguration( + "side_tcp_pre_offset_m" + ), + "side_tcp_close_offset_m": LaunchConfiguration( + "side_tcp_close_offset_m" + ), "side_grasp_axis": ParameterValue( LaunchConfiguration("side_grasp_axis"), value_type=str, ), + "side_candidate_axes": ParameterValue( + LaunchConfiguration("side_candidate_axes"), + value_type=str, + ), "side_grasp_direction": LaunchConfiguration("side_grasp_direction"), "side_approach_offset": LaunchConfiguration("side_approach_offset"), "side_staging_offset": LaunchConfiguration("side_staging_offset"), @@ -271,6 +296,44 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params "side_project_bbox_center_to_fixed_z": LaunchConfiguration( "side_project_bbox_center_to_fixed_z" ), + "side_cup_collision_enabled": LaunchConfiguration( + "side_cup_collision_enabled" + ), + "side_cup_collision_id": ParameterValue( + LaunchConfiguration("side_cup_collision_id"), + value_type=str, + ), + "side_cup_collision_radius_m": LaunchConfiguration( + "side_cup_collision_radius_m" + ), + "side_cup_collision_height_m": LaunchConfiguration( + "side_cup_collision_height_m" + ), + "side_cup_collision_padding_m": LaunchConfiguration( + "side_cup_collision_padding_m" + ), + "side_lid_collision_enabled": LaunchConfiguration( + "side_lid_collision_enabled" + ), + "side_lid_collision_id": ParameterValue( + LaunchConfiguration("side_lid_collision_id"), + value_type=str, + ), + "side_lid_collision_radius_m": LaunchConfiguration( + "side_lid_collision_radius_m" + ), + "side_lid_collision_height_m": LaunchConfiguration( + "side_lid_collision_height_m" + ), + "side_lid_collision_padding_m": LaunchConfiguration( + "side_lid_collision_padding_m" + ), + "side_cup_collision_clear_before_close": LaunchConfiguration( + "side_cup_collision_clear_before_close" + ), + "side_cup_collision_update_wait_sec": LaunchConfiguration( + "side_cup_collision_update_wait_sec" + ), "table_collision_enabled": LaunchConfiguration( "table_collision_enabled" ), @@ -467,9 +530,49 @@ def generate_launch_description(): "redetect_settle_sec", default_value="0.5" ) grasp_mode_arg = DeclareLaunchArgument("grasp_mode", default_value="side") + motion_link_arg = DeclareLaunchArgument( + "motion_link", + default_value="gripper_tcp", + description="MoveIt pose target link. Use gripper_tcp so Cartesian cup goals command the RG2 TCP, not link_6.", + ) + camera_reference_link_arg = DeclareLaunchArgument( + "camera_reference_link", + default_value="link_6", + description="Robot link used with T_gripper2camera.npy for hand-eye camera transforms.", + ) + side_tcp_compensation_enabled_arg = DeclareLaunchArgument( + "side_tcp_compensation_enabled", + default_value="true", + description="Convert legacy link_6 side-grip offsets to safe gripper_tcp standoff distances.", + ) + side_tcp_reach_m_arg = DeclareLaunchArgument( + "side_tcp_reach_m", + default_value="0.213", + description="Fixed link_6-to-gripper_tcp reach used to compensate legacy side-grip offsets.", + ) + side_tcp_stage_offset_m_arg = DeclareLaunchArgument( + "side_tcp_stage_offset_m", + default_value="0.120", + description="Minimum TCP standoff from cup center for the low/high side staging waypoint.", + ) + side_tcp_pre_offset_m_arg = DeclareLaunchArgument( + "side_tcp_pre_offset_m", + default_value="0.100", + description="Minimum TCP standoff from cup center for the side pre-grasp waypoint.", + ) + side_tcp_close_offset_m_arg = DeclareLaunchArgument( + "side_tcp_close_offset_m", + default_value="0.055", + description="Minimum TCP standoff from cup center before closing to avoid pushing through the cup.", + ) side_grasp_axis_arg = DeclareLaunchArgument( "side_grasp_axis", default_value="y_axis" ) + side_candidate_axes_arg = DeclareLaunchArgument( + "side_candidate_axes", + default_value="y_axis", + description="Compatibility argument; side grasp is constrained to the legacy Y-axis approach.", + ) side_grasp_direction_arg = DeclareLaunchArgument( "side_grasp_direction", default_value="1.0", @@ -535,8 +638,8 @@ def generate_launch_description(): ) side_low_retry_attempts_arg = DeclareLaunchArgument( "side_low_retry_attempts", - default_value="5", - description="Number of raised-Z retries for the low side-grip staging pose.", + default_value="0", + description="Number of raised-Z retries for the low side-grip staging pose. Keep 0 for fixed 7cm side grasp.", ) side_auto_direction_by_cup_y_arg = DeclareLaunchArgument( "side_auto_direction_by_cup_y", @@ -560,7 +663,7 @@ def generate_launch_description(): ) side_fixed_grasp_z_enabled_arg = DeclareLaunchArgument( "side_fixed_grasp_z_enabled", - default_value="false", + default_value="true", description="Use a fixed base_link Z height for side grasp instead of detected depth Z plus offset.", ) side_fixed_grasp_z_arg = DeclareLaunchArgument( @@ -573,6 +676,66 @@ def generate_launch_description(): default_value="true", description="Project the initial bbox center ray onto side_fixed_grasp_z for side target X/Y.", ) + side_cup_collision_enabled_arg = DeclareLaunchArgument( + "side_cup_collision_enabled", + default_value="true", + description="Add temporary detected cup/lid collision objects during gross side-grip motion.", + ) + side_cup_collision_id_arg = DeclareLaunchArgument( + "side_cup_collision_id", + default_value="side_grip_detected_cup", + description="Collision object id for the temporary detected cup.", + ) + side_cup_collision_radius_m_arg = DeclareLaunchArgument( + "side_cup_collision_radius_m", + default_value="0.045", + description="Nominal detected cup collision radius.", + ) + side_cup_collision_height_m_arg = DeclareLaunchArgument( + "side_cup_collision_height_m", + default_value="0.120", + description="Detected cup collision cylinder height.", + ) + side_cup_collision_padding_m_arg = DeclareLaunchArgument( + "side_cup_collision_padding_m", + default_value="0.015", + description="Extra detected cup collision radius padding for gross motion.", + ) + side_lid_collision_enabled_arg = DeclareLaunchArgument( + "side_lid_collision_enabled", + default_value="true", + description="Add a temporary lid/cap collision object above the detected cup during side gross motion.", + ) + side_lid_collision_id_arg = DeclareLaunchArgument( + "side_lid_collision_id", + default_value="side_grip_detected_lid", + description="Collision object id for the temporary detected lid/cap.", + ) + side_lid_collision_radius_m_arg = DeclareLaunchArgument( + "side_lid_collision_radius_m", + default_value="0.055", + description="Nominal detected lid/cap collision radius.", + ) + side_lid_collision_height_m_arg = DeclareLaunchArgument( + "side_lid_collision_height_m", + default_value="0.025", + description="Detected lid/cap collision cylinder height.", + ) + side_lid_collision_padding_m_arg = DeclareLaunchArgument( + "side_lid_collision_padding_m", + default_value="0.010", + description="Extra detected lid/cap collision radius padding for gross motion.", + ) + side_cup_collision_clear_before_close_arg = DeclareLaunchArgument( + "side_cup_collision_clear_before_close", + default_value="true", + description="Remove detected cup/lid collision before the final intentional close approach.", + ) + side_cup_collision_update_wait_sec_arg = DeclareLaunchArgument( + "side_cup_collision_update_wait_sec", + default_value="0.15", + description="Small wait after publishing cup/lid collision add/remove messages.", + ) dispenser_collision_enabled_arg = DeclareLaunchArgument( "dispenser_collision_enabled", default_value="true", @@ -887,7 +1050,15 @@ def generate_launch_description(): redetect_on_approach_arg, redetect_settle_sec_arg, grasp_mode_arg, + motion_link_arg, + camera_reference_link_arg, + side_tcp_compensation_enabled_arg, + side_tcp_reach_m_arg, + side_tcp_stage_offset_m_arg, + side_tcp_pre_offset_m_arg, + side_tcp_close_offset_m_arg, side_grasp_axis_arg, + side_candidate_axes_arg, side_grasp_direction_arg, side_approach_offset_arg, side_staging_offset_arg, @@ -909,6 +1080,18 @@ def generate_launch_description(): side_fixed_grasp_z_enabled_arg, side_fixed_grasp_z_arg, side_project_bbox_center_to_fixed_z_arg, + side_cup_collision_enabled_arg, + side_cup_collision_id_arg, + side_cup_collision_radius_m_arg, + side_cup_collision_height_m_arg, + side_cup_collision_padding_m_arg, + side_lid_collision_enabled_arg, + side_lid_collision_id_arg, + side_lid_collision_radius_m_arg, + side_lid_collision_height_m_arg, + side_lid_collision_padding_m_arg, + side_cup_collision_clear_before_close_arg, + side_cup_collision_update_wait_sec_arg, dispenser_collision_enabled_arg, dispenser_collision_config_path_arg, dispenser_collision_publish_period_sec_arg, From 1bb4397f0eff9ddc9d3672e2bf6d4435f7009542 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Mon, 15 Jun 2026 18:24:06 +0900 Subject: [PATCH 86/88] =?UTF-8?q?10=EC=B0=A8=20=ED=85=8C=EC=8A=A4=ED=8A=B8?= =?UTF-8?q?=20=ED=95=9C=EC=82=AC=EC=9D=B4=ED=81=B4=20(=EC=82=AC=EC=9D=B4?= =?UTF-8?q?=EB=93=9C=EA=B7=B8=EB=A6=BD=EC=99=84=EB=A3=8C)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/auto_cup_flow_router.launch.py | 13 +- .../azas_task_manager/auto_cup_flow_router.py | 16 +-- .../test/test_voice_auto_cup_flow_wrapper.py | 101 ++++++++++++++++ tools/perception/human_hand_detection_node.py | 23 +++- tools/run/direct_movel_xyz.py | 113 +++++++++++++----- tools/run/handover_cup_to_palm.py | 7 -- tools/run/run_kang_lid_grip_close_direct.sh | 24 ++-- tools/run/run_voice_auto_cup_flow.sh | 22 +++- 8 files changed, 260 insertions(+), 59 deletions(-) create mode 100644 src/azas_task_manager/test/test_voice_auto_cup_flow_wrapper.py diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index 9f404a2..4d5f31e 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -9,10 +9,13 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument("enable_real_motion", default_value="false"), DeclareLaunchArgument("router_confirm", default_value=""), - DeclareLaunchArgument("service_prefix", default_value=""), - DeclareLaunchArgument("motion_service_prefix", default_value="auto"), - DeclareLaunchArgument("moveit_controller_name", default_value="/dsr_moveit_controller"), - DeclareLaunchArgument("controller_action_name", default_value="/dsr_moveit_controller/follow_joint_trajectory"), + DeclareLaunchArgument("service_prefix", default_value="dsr01"), + DeclareLaunchArgument("motion_service_prefix", default_value="dsr01"), + DeclareLaunchArgument("moveit_controller_name", default_value="/dsr01/dsr_moveit_controller"), + DeclareLaunchArgument( + "controller_action_name", + default_value="/dsr01/dsr_moveit_controller/follow_joint_trajectory", + ), DeclareLaunchArgument("yolo_model_path", default_value="/home/ssu/Azas/local_models/best.pt"), DeclareLaunchArgument("classifier_path", default_value="/home/ssu/Azas/cup_classifier_best.pth"), DeclareLaunchArgument("classifier_arch", default_value="resnet18"), @@ -51,7 +54,7 @@ def generate_launch_description(): "human_handover_detection_command", default_value=( "tools/run/with_azas_ros_env.sh bash tools/run/run_human_hand_detection.sh " - "--process-width-px 320 --overlay-width-px 640 --max-rate-hz 20 " + "--max-rate-hz 20 " "--min-detection-confidence 0.35 --min-tracking-confidence 0.35 " "--min-extended-fingers 3 --depth-window-px 21 --stable-radius-m 0.12 " "--stable-min-samples 2 --stable-window-seconds 1.0" diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 3b446dc..37aa82c 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -9,6 +9,7 @@ import sys import threading import time +import traceback from dataclasses import dataclass from typing import Optional @@ -60,8 +61,8 @@ def __init__(self) -> None: self.declare_parameter("observe_acc", 30.0) self.declare_parameter("observe_time", 0.0) self.declare_parameter("motion_timeout_sec", 25.0) - self.declare_parameter("service_prefix", "") - self.declare_parameter("motion_service_prefix", "auto") + self.declare_parameter("service_prefix", "dsr01") + self.declare_parameter("motion_service_prefix", "dsr01") self.declare_parameter("gripper_open_service", "/jarvis/rg2/open") self.declare_parameter("detection_topic", "/azas/cup_detection") @@ -80,8 +81,11 @@ def __init__(self) -> None: self.declare_parameter("side_launch", "dsr_practice yolo_cup_pick_node.launch.py") self.declare_parameter("cup_uprighting_launch", "azas_cup_uprighting yolo_cup_uprighting.launch.py") - self.declare_parameter("moveit_controller_name", "/dsr_moveit_controller") - self.declare_parameter("controller_action_name", "/dsr_moveit_controller/follow_joint_trajectory") + self.declare_parameter("moveit_controller_name", "/dsr01/dsr_moveit_controller") + self.declare_parameter( + "controller_action_name", + "/dsr01/dsr_moveit_controller/follow_joint_trajectory", + ) self.declare_parameter("side_extra_args", "") self.declare_parameter("cup_uprighting_extra_args", "") # 사이드 그립에서 base x가 +20mm 정도 어긋나는 실측 보정값 @@ -138,8 +142,6 @@ def __init__(self) -> None: self.declare_parameter( "human_handover_detection_command", "tools/run/with_azas_ros_env.sh bash tools/run/run_human_hand_detection.sh " - "--process-width-px 320 " - "--overlay-width-px 640 " "--max-rate-hz 20 " "--min-detection-confidence 0.35 " "--min-tracking-confidence 0.35 " @@ -280,7 +282,7 @@ def _run_resumable_stage( try: ok = bool(action()) except Exception as exc: - self.get_logger().exception(f"{stage}: unexpected exception") + self.get_logger().error(f"{stage}: unexpected exception: {exc}\n{traceback.format_exc()}") if self._resume_store is not None: self._resume_store.fail_stage(stage, f"{stage}_exception:{exc}", auto_recoverable=True) return False diff --git a/src/azas_task_manager/test/test_voice_auto_cup_flow_wrapper.py b/src/azas_task_manager/test/test_voice_auto_cup_flow_wrapper.py new file mode 100644 index 0000000..7907347 --- /dev/null +++ b/src/azas_task_manager/test/test_voice_auto_cup_flow_wrapper.py @@ -0,0 +1,101 @@ +from pathlib import Path + + +def test_voice_auto_cup_flow_uses_meter_offset_for_three_millimeters(): + repo_root = Path(__file__).resolve().parents[3] + script = repo_root / "tools" / "run" / "run_voice_auto_cup_flow.sh" + text = script.read_text(encoding="utf-8") + + assert 'CUP_HOLDER_PLACE_FINAL_X_OFFSET_M="${CUP_HOLDER_PLACE_FINAL_X_OFFSET_M:-0.003}"' in text + assert "cup_holder_place_x_offset_m:=3.0" not in text + assert 'cup_holder_place_x_offset_m:="${CUP_HOLDER_PLACE_FINAL_X_OFFSET_M}"' in text + + +def test_voice_auto_cup_flow_blocks_meter_scale_holder_offsets(): + repo_root = Path(__file__).resolve().parents[3] + script = repo_root / "tools" / "run" / "run_voice_auto_cup_flow.sh" + text = script.read_text(encoding="utf-8") + + assert "abs(offset_m) > 0.05" in text + assert "must be in meters" in text + + +def test_voice_auto_cup_flow_passes_dsr01_motion_namespace_by_default(): + repo_root = Path(__file__).resolve().parents[3] + script = repo_root / "tools" / "run" / "run_voice_auto_cup_flow.sh" + text = script.read_text(encoding="utf-8") + + assert 'SERVICE_PREFIX="${SERVICE_PREFIX:-dsr01}"' in text + assert 'MOTION_SERVICE_PREFIX="${MOTION_SERVICE_PREFIX:-${SERVICE_PREFIX}}"' in text + assert 'service_prefix:="${SERVICE_PREFIX}"' in text + assert 'motion_service_prefix:="${MOTION_SERVICE_PREFIX}"' in text + assert "moveit_controller_name:=/${SERVICE_PREFIX}/dsr_moveit_controller" in text + assert ( + "controller_action_name:=/${SERVICE_PREFIX}/dsr_moveit_controller/follow_joint_trajectory" + in text + ) + + +def test_auto_cup_flow_router_launch_defaults_to_dsr01_motion_namespace(): + repo_root = Path(__file__).resolve().parents[3] + launch = repo_root / "src" / "azas_bringup" / "launch" / "auto_cup_flow_router.launch.py" + text = launch.read_text(encoding="utf-8") + + assert 'DeclareLaunchArgument("service_prefix", default_value="dsr01")' in text + assert 'DeclareLaunchArgument("motion_service_prefix", default_value="dsr01")' in text + assert ( + 'DeclareLaunchArgument("moveit_controller_name", default_value="/dsr01/dsr_moveit_controller")' + in text + ) + assert 'default_value="/dsr01/dsr_moveit_controller/follow_joint_trajectory"' in text + + +def test_auto_cup_flow_router_node_defaults_to_dsr01_motion_namespace(): + repo_root = Path(__file__).resolve().parents[3] + router = ( + repo_root + / "src" + / "azas_task_manager" + / "azas_task_manager" + / "auto_cup_flow_router.py" + ) + text = router.read_text(encoding="utf-8") + + assert 'self.declare_parameter("service_prefix", "dsr01")' in text + assert 'self.declare_parameter("motion_service_prefix", "dsr01")' in text + assert 'self.declare_parameter("moveit_controller_name", "/dsr01/dsr_moveit_controller")' in text + assert '"/dsr01/dsr_moveit_controller/follow_joint_trajectory"' in text + assert 'base = f"/{prefix}/motion" if prefix else "/motion"' in text + + +def test_human_handover_detection_default_command_matches_current_cli(): + repo_root = Path(__file__).resolve().parents[3] + launch = repo_root / "src" / "azas_bringup" / "launch" / "auto_cup_flow_router.launch.py" + router = ( + repo_root + / "src" + / "azas_task_manager" + / "azas_task_manager" + / "auto_cup_flow_router.py" + ) + text = launch.read_text(encoding="utf-8") + "\n" + router.read_text(encoding="utf-8") + + assert "--process-width-px" not in text + assert "--overlay-width-px" not in text + assert "--max-rate-hz 20" in text + assert "--stable-window-seconds 1.0" in text + + +def test_auto_cup_flow_router_does_not_use_rcutils_logger_exception(): + repo_root = Path(__file__).resolve().parents[3] + router = ( + repo_root + / "src" + / "azas_task_manager" + / "azas_task_manager" + / "auto_cup_flow_router.py" + ) + text = router.read_text(encoding="utf-8") + + assert "traceback.format_exc()" in text + assert "get_logger().exception" not in text diff --git a/tools/perception/human_hand_detection_node.py b/tools/perception/human_hand_detection_node.py index 699dde7..2006471 100755 --- a/tools/perception/human_hand_detection_node.py +++ b/tools/perception/human_hand_detection_node.py @@ -77,6 +77,14 @@ def bgr_array_to_image_msg(array: np.ndarray, header) -> Image: return msg +def resize_to_width(array: np.ndarray, width_px: int) -> np.ndarray: + if width_px <= 0 or array.shape[1] == width_px: + return array + scale = float(width_px) / float(array.shape[1]) + height_px = max(int(round(array.shape[0] * scale)), 1) + return cv2.resize(array, (width_px, height_px), interpolation=cv2.INTER_AREA) + + class HumanHandDetectionNode(Node): """Perception-only node: no motion service client is created here.""" @@ -132,13 +140,14 @@ def on_color(self, msg: Image) -> None: return color = image_msg_to_array(msg) - rgb = cv2.cvtColor(color, cv2.COLOR_BGR2RGB) + process_color = resize_to_width(color, int(self.args.process_width_px)) + rgb = cv2.cvtColor(process_color, cv2.COLOR_BGR2RGB) timestamp_ms = max(int(now * 1000.0), self.last_timestamp_ms + 1) self.last_timestamp_ms = timestamp_ms mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb) result = self.landmarker.detect_for_video(mp_image, timestamp_ms) - overlay = color if self.overlay_pub is not None else None + overlay = color.copy() if self.overlay_pub is not None else None status: dict[str, object] = {"detected": False} try: if not result.hand_landmarks: @@ -147,7 +156,10 @@ def on_color(self, msg: Image) -> None: return landmarks = result.hand_landmarks[0] height, width = color.shape[:2] - pixels = [(lm.x * width, lm.y * height) for lm in landmarks] + process_height, process_width = process_color.shape[:2] + scale_x = float(width) / float(process_width) + scale_y = float(height) / float(process_height) + pixels = [(lm.x * process_width * scale_x, lm.y * process_height * scale_y) for lm in landmarks] open_fingers = self.count_extended_fingers(pixels) hand_open = open_fingers >= self.args.min_extended_fingers palm_px = ( @@ -199,6 +211,7 @@ def on_color(self, msg: Image) -> None: finally: self.publish_status(status) if overlay is not None and self.overlay_pub is not None: + overlay = resize_to_width(overlay, int(self.args.overlay_width_px)) self.overlay_pub.publish(bgr_array_to_image_msg(overlay, msg.header)) def count_extended_fingers(self, pixels: list[tuple[float, float]]) -> int: @@ -257,6 +270,10 @@ def parse_bool(value: str) -> bool: def main() -> int: parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument("--model-path", default=DEFAULT_MODEL_PATH) + parser.add_argument("--process-width-px", type=int, default=0, + help="resize color frames to this width before MediaPipe; 0 keeps camera width") + parser.add_argument("--overlay-width-px", type=int, default=0, + help="resize published overlay images to this width; 0 keeps camera width") parser.add_argument("--max-rate-hz", type=float, default=15.0) parser.add_argument("--min-detection-confidence", type=float, default=0.6) parser.add_argument("--min-tracking-confidence", type=float, default=0.6) diff --git a/tools/run/direct_movel_xyz.py b/tools/run/direct_movel_xyz.py index 4697f9a..5ebbae5 100755 --- a/tools/run/direct_movel_xyz.py +++ b/tools/run/direct_movel_xyz.py @@ -181,12 +181,23 @@ def parse_args() -> argparse.Namespace: help="number of /motion/ikin precheck attempts before failing closed", ) parser.add_argument("--ikin-sol-space", type=int, default=2, help="solution space used by --precheck-ikin") + parser.add_argument( + "--ikin-sol-spaces", + default="", + help="comma-separated solution spaces to try in order before failing the IK precheck", + ) parser.add_argument("--j5-min-deg", type=float, default=-135.0, help="safe lower limit for joint 5") parser.add_argument("--j5-max-deg", type=float, default=135.0, help="safe upper limit for joint 5") parser.add_argument("--service-prefix", default="", help="optional namespace before /motion/move_line") parser.add_argument("--velocity", type=float, default=20.0, help="line velocity") parser.add_argument("--acceleration", type=float, default=20.0, help="line acceleration") parser.add_argument("--timeout-sec", type=float, default=10.0, help="service response timeout") + parser.add_argument( + "--motion-timeout-sec", + type=float, + default=None, + help="compatibility alias accepted from sequenced motion wrappers", + ) parser.add_argument("--wait-service-sec", type=float, default=5.0, help="service availability timeout") parser.add_argument("--x-min", type=float, default=0.10) parser.add_argument("--x-max", type=float, default=0.70) @@ -206,6 +217,13 @@ def parse_args() -> argparse.Namespace: action="store_true", help="actually call MoveLine; without this, only prints the request", ) + parser.add_argument( + "--fallback-movej-on-verify-fail", + action="store_true", + help="compatibility flag only; this tool keeps failing closed on MoveLine verification failure", + ) + parser.add_argument("--fallback-movej-velocity", type=float, default=20.0) + parser.add_argument("--fallback-movej-acceleration", type=float, default=20.0) parser.add_argument( "--confirm", default="", @@ -214,6 +232,16 @@ def parse_args() -> argparse.Namespace: return parser.parse_args() +def parse_ikin_sol_spaces(args: argparse.Namespace) -> list[int]: + raw_value = str(args.ikin_sol_spaces).strip() + if not raw_value: + return [int(args.ikin_sol_space)] + values = [int(part.strip()) for part in raw_value.split(",") if part.strip()] + if not values: + raise ValueError("--ikin-sol-spaces did not contain any solution spaces") + return values + + def main() -> int: args = parse_args() bounds = Bounds(args.x_min, args.x_max, args.y_min, args.y_max, args.z_min, args.z_max) @@ -269,41 +297,68 @@ def main() -> int: assert node is not None if args.precheck_ikin: + try: + sol_spaces = parse_ikin_sol_spaces(args) + except ValueError as exc: + print(f"[BLOCKED] {exc}") + return 2 + response = None + selected_sol_space = None + last_failure = "" attempts = max(int(args.ikin_retries), 1) for attempt in range(1, attempts + 1): - req = Ikin.Request() - req.pos = pos_mm_deg - req.sol_space = int(args.ikin_sol_space) - req.ref = DR_BASE - try: - response = call_service( - node, - Ikin, - prefixed_service(args.service_prefix, "motion/ikin"), - req, - timeout_sec=max(args.ikin_timeout_sec, 0.1), - label="Ikin", - ) + for sol_space in sol_spaces: + req = Ikin.Request() + req.pos = pos_mm_deg + req.sol_space = int(sol_space) + req.ref = DR_BASE + try: + candidate = call_service( + node, + Ikin, + prefixed_service(args.service_prefix, "motion/ikin"), + req, + timeout_sec=max(args.ikin_timeout_sec, 0.1), + label=f"Ikin sol_space={sol_space}", + ) + except RuntimeError as exc: + last_failure = str(exc) + print( + f"[WARN] Ikin attempt {attempt}/{attempts} " + f"sol_space={sol_space} failed: {exc}" + ) + continue + if not candidate.success: + last_failure = f"Ikin sol_space={sol_space} returned success=false" + print(f"[WARN] {last_failure}") + continue + if len(candidate.conv_posj) >= 5: + joint5 = float(candidate.conv_posj[4]) + if not float(args.j5_min_deg) <= joint5 <= float(args.j5_max_deg): + last_failure = ( + f"Ikin sol_space={sol_space} predicted joint_5={joint5:.3f} deg outside " + f"[{float(args.j5_min_deg):.3f}, {float(args.j5_max_deg):.3f}] deg" + ) + print(f"[WARN] {last_failure}") + continue + response = candidate + selected_sol_space = int(sol_space) + break + if response is not None: break - except RuntimeError as exc: - if attempt >= attempts: - raise - print(f"[WARN] Ikin attempt {attempt}/{attempts} failed: {exc}; retrying") + if attempt < attempts: time.sleep(1.0) - if not response.success: - print("[FAIL] Ikin returned success=false") + if response is None: + print("[FAIL] Ikin precheck failed for all configured solution spaces") + if last_failure: + print(f"[FAIL] last failure: {last_failure}") return 1 - print("[Azas] Ikin precheck success: joints_deg=[" + ", ".join(f"{value:.1f}" for value in response.conv_posj) + "]") - if len(response.conv_posj) >= 5: - joint5 = float(response.conv_posj[4]) - if not float(args.j5_min_deg) <= joint5 <= float(args.j5_max_deg): - print( - f"[BLOCKED] Ikin predicted joint_5={joint5:.3f} deg outside " - f"[{float(args.j5_min_deg):.3f}, {float(args.j5_max_deg):.3f}] deg; " - "refusing MoveLine." - ) - return 2 + print( + f"[Azas] Ikin precheck success: sol_space={selected_sol_space} joints_deg=[" + + ", ".join(f"{value:.1f}" for value in response.conv_posj) + + "]" + ) client = node.create_client(MoveLine, move_service) if not client.wait_for_service(timeout_sec=max(args.wait_service_sec, 0.1)): diff --git a/tools/run/handover_cup_to_palm.py b/tools/run/handover_cup_to_palm.py index c68796d..ac945ff 100755 --- a/tools/run/handover_cup_to_palm.py +++ b/tools/run/handover_cup_to_palm.py @@ -233,7 +233,6 @@ def run_movel( velocity: float, acceleration: float, rpy_deg: list[float], - fallback_movej: bool = True, ) -> None: cmd = [ sys.executable, str(DIRECT_MOVEL), @@ -258,12 +257,6 @@ def run_movel( ] if args.execute: cmd += ["--precheck-ikin", "--verify-target", "--execute", "--confirm", DIRECT_CONFIRM_PHRASE] - if fallback_movej: - cmd += [ - "--fallback-movej-on-verify-fail", - "--fallback-movej-velocity", f"{min(max(velocity, 5.0), args.transit_velocity):.3f}", - "--fallback-movej-acceleration", f"{min(max(acceleration, 10.0), args.transit_acceleration):.3f}", - ] print(f"[Azas] MOVE {label}: xyz_m=[{xyz_m[0]:.3f}, {xyz_m[1]:.3f}, {xyz_m[2]:.3f}] vel={velocity:.1f}") rc = subprocess.run(cmd, cwd=str(ROOT), check=False).returncode if rc != 0: diff --git a/tools/run/run_kang_lid_grip_close_direct.sh b/tools/run/run_kang_lid_grip_close_direct.sh index 3ad5911..fb9698f 100755 --- a/tools/run/run_kang_lid_grip_close_direct.sh +++ b/tools/run/run_kang_lid_grip_close_direct.sh @@ -65,15 +65,25 @@ if [[ "${MOVE_TO_LID_VIEW_POSE}" == "true" ]]; then --execute --confirm ENABLE_DIRECT_MOVEJ fi -ros2 pkg executables azas_perception | grep -q '^azas_perception lid_sticker_detector_node$' || { - echo "[Azas][FAIL] missing azas_perception lid_sticker_detector_node" >&2 - exit 2 -} -ros2 pkg executables azas_motion | grep -q '^azas_motion lid_grip_planner_node$' || { - echo "[Azas][FAIL] missing azas_motion lid_grip_planner_node" >&2 - exit 3 +require_ros_executable() { + local package="$1" + local executable="$2" + local exit_code="$3" + local executables + + if ! executables="$(ros2 pkg executables "${package}")"; then + echo "[Azas][FAIL] cannot list ${package} executables" >&2 + exit "${exit_code}" + fi + if ! grep -Fxq "${package} ${executable}" <<<"${executables}"; then + echo "[Azas][FAIL] missing ${package} ${executable}" >&2 + exit "${exit_code}" + fi } +require_ros_executable azas_perception lid_sticker_detector_node 2 +require_ros_executable azas_motion lid_grip_planner_node 3 + launch_args=( azas_bringup lid_sticker_grip_planning.launch.py model_path:="${MODEL_PATH}" \ diff --git a/tools/run/run_voice_auto_cup_flow.sh b/tools/run/run_voice_auto_cup_flow.sh index 83a89ad..dc1962f 100755 --- a/tools/run/run_voice_auto_cup_flow.sh +++ b/tools/run/run_voice_auto_cup_flow.sh @@ -22,12 +22,32 @@ AUTO_FLOW_RESUME_MODE="${AUTO_FLOW_RESUME_MODE:-normal}" AUTO_FLOW_RESUME_STATE_FILE="${AUTO_FLOW_RESUME_STATE_FILE:-/home/ssu/Azas/outputs/auto_cup_flow_resume.json}" AUTO_FLOW_RESUME_EVENTS_FILE="${AUTO_FLOW_RESUME_EVENTS_FILE:-/home/ssu/Azas/outputs/auto_cup_flow_events.jsonl}" AUTO_FLOW_DISPENSER_RESUME_STATE_FILE="${AUTO_FLOW_DISPENSER_RESUME_STATE_FILE:-/home/ssu/Azas/outputs/measured_dispenser_recipe_resume.json}" +CUP_HOLDER_PLACE_FINAL_X_OFFSET_M="${CUP_HOLDER_PLACE_FINAL_X_OFFSET_M:-0.003}" ROUTER_CONFIRM="${ROUTER_CONFIRM:-}" if [[ "${ROUTER_CONFIRM}" != "ENABLE_AUTO_CUP_ROUTER" ]]; then echo "[voice_flow] BLOCKED: set ROUTER_CONFIRM=ENABLE_AUTO_CUP_ROUTER to run real motion." >&2 exit 3 fi +python3 - "${CUP_HOLDER_PLACE_FINAL_X_OFFSET_M}" <<'PY' +import sys + +value = sys.argv[1] +try: + offset_m = float(value) +except ValueError: + print(f"[voice_flow] invalid CUP_HOLDER_PLACE_FINAL_X_OFFSET_M={value!r}; expected meters", file=sys.stderr) + sys.exit(4) + +if abs(offset_m) > 0.05: + print( + "[voice_flow] BLOCKED: CUP_HOLDER_PLACE_FINAL_X_OFFSET_M " + f"must be in meters and within +/-0.05m; got {offset_m}m", + file=sys.stderr, + ) + sys.exit(4) +PY + cd /home/ssu/Azas set +u source /opt/ros/humble/setup.bash @@ -49,7 +69,7 @@ set +e ros2 launch azas_bringup auto_cup_flow_router.launch.py \ enable_real_motion:=true \ router_confirm:=ENABLE_AUTO_CUP_ROUTER \ - cup_holder_place_x_offset_m:=3.0 \ + cup_holder_place_x_offset_m:="${CUP_HOLDER_PLACE_FINAL_X_OFFSET_M}" \ service_prefix:="${SERVICE_PREFIX}" \ motion_service_prefix:="${MOTION_SERVICE_PREFIX}" \ moveit_controller_name:=/${SERVICE_PREFIX}/dsr_moveit_controller \ From 0b6791c134668327750f517f268c6743a342439c Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Mon, 15 Jun 2026 18:43:13 +0900 Subject: [PATCH 87/88] maybe failed --- DESIGN.md | 15 +- .../voice_pipeline_executor_node.py | 9 + .../azas_voice/voice_screen_node.py | 410 +++++++++++++++++- src/azas_voice/launch/azas_voice.launch.py | 8 + .../test_voice_pipeline_recovery_helpers.py | 9 + src/azas_voice/test/test_voice_screen_node.py | 53 ++- src/azas_voice/web/voice.css | 114 +++++ src/azas_voice/web/voice.html | 16 + src/azas_voice/web/voice.js | 111 ++++- 9 files changed, 729 insertions(+), 16 deletions(-) diff --git a/DESIGN.md b/DESIGN.md index 1639901..8f4dd28 100644 --- a/DESIGN.md +++ b/DESIGN.md @@ -2,7 +2,7 @@ ## Source of truth - Status: Draft -- Last refreshed: 2026-06-13 +- Last refreshed: 2026-06-15 - Primary product surfaces: Azas voice order screen, menu preview panel, robot pipeline status UI, kiosk/menu surfaces. - Evidence reviewed: `src/azas_voice/web/voice.html`, `src/azas_voice/web/voice.css`, `src/azas_voice/web/voice.js`, `src/azas_voice/azas_voice/voice_screen_node.py`, `src/azas_voice/azas_voice/voice_pipeline_executor_node.py`, `src/azas_voice/config/recipes.yaml`, `src/azas_kiosk/`. @@ -12,9 +12,9 @@ - Avoid: marketing hero pages, decorative-only UI, hidden robot motion state, fake coordinates or unsupported safety claims. ## Product goals -- Goals: let users order many named drinks by voice or touch, preview the finished drink, and understand the robot's current manufacturing stage. +- Goals: let users order many named drinks by voice or touch, preview the finished drink, watch the active vision feed, and understand the robot's current manufacturing stage. - Non-goals: manual robot coordinate entry, free-form motion generation, unsupported recipe execution outside measured dispenser/color mappings. -- Success signals: users can pick from a larger menu, see ingredient amounts, see the current robot step, and recover from interrupted dispenser sequences. +- Success signals: users can pick from a larger menu, see ingredient amounts, see the current robot step, see the active camera/detection process, and recover from interrupted dispenser sequences. ## Personas and jobs - Primary personas: demo operator, guest ordering a drink, developer validating the robot flow. @@ -24,7 +24,7 @@ ## Information architecture - Primary navigation: single voice order screen with adjacent menu/status panel. - Core routes/screens: voice conversation, selected drink preview, robot process stage, catalog list. -- Content hierarchy: current order and confirmation first, finished drink preview second, recipe catalog and process detail nearby. +- Content hierarchy: current order and confirmation first, finished drink preview second, vision feed and robot process status nearby, recipe catalog below. ## Design principles - Principle 1: show operational state directly instead of explaining the system. @@ -41,8 +41,8 @@ ## Components - Existing components to reuse: voice orb, dialogue bubbles, status grid, recipe glass SVG, ingredient chips, pipeline step list. -- New/changed components: catalog item buttons, drink stat block, robot process scene, resume-aware pipeline stage. -- Variants and states: idle, recommended, confirmed, making, completed, failed, dry-run, resume recovery. +- New/changed components: catalog item buttons, drink stat block, robot process scene, resume-aware pipeline stage, live vision camera panel. +- Variants and states: idle, recommended, confirmed, making, completed, failed, dry-run, resume recovery, Realsense live, cup upright/lying, lid detection, hand detection. - Token/component ownership: `src/azas_voice/web/voice.css` owns current web styling; recipe data comes from `src/azas_voice/config/recipes.yaml`. ## Accessibility @@ -64,6 +64,7 @@ - Success: show completed badge and final drink preview. - Disabled: hardware execution may remain dry-run from launch parameters. - Offline/slow network, if applicable: periodic refresh should keep the last known UI state visible. +- Camera transitions: cup upright/lying view remains visible for 2 seconds after classification leaves the active stage; lid detection view stops when the pipeline enters shake; hand detection view appears during the handover stage. ## Content voice - Tone: concise Korean service copy. @@ -74,7 +75,7 @@ - Framework/styling system: static HTML/CSS/JavaScript served by `voice_screen_node.py`. - Design-token constraints: no central token system yet; keep colors local and ingredient-specific. - Performance constraints: catalog rendering should avoid repeated full DOM rebuilds unless catalog data changes. -- Compatibility constraints: ROS nodes publish JSON status; browser UI polls `/api/state`. +- Compatibility constraints: ROS nodes publish JSON status and ROS image topics; browser UI polls `/api/state` and fetches cache-busted JPEG camera frames from `voice_screen_node.py`. - Test/screenshot expectations: run parser/mapper tests for recipe changes and smoke browser/server behavior when launch environment is available. ## Open questions diff --git a/src/azas_voice/azas_voice/voice_pipeline_executor_node.py b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py index ea20e3a..6c92f24 100644 --- a/src/azas_voice/azas_voice/voice_pipeline_executor_node.py +++ b/src/azas_voice/azas_voice/voice_pipeline_executor_node.py @@ -27,12 +27,21 @@ # (auto_cup_flow_router의 로그 문구가 바뀌면 여기도 같이 갱신할 것) STAGE_MARKERS: tuple[tuple[str, str], ...] = ( ("auto cup router: color scan", "디스펜서 색 스캔"), + ("starting perception with cup classifier", "컵 자세 구분"), + ("waiting for stable route", "컵 자세 구분"), + ("route candidate stable", "컵 자세 구분"), ("route decided: side_grasp", "컵 픽업 (세워진 컵)"), ("route decided: cup_uprighting", "컵 픽업 (쓰러진 컵)"), ("starting integrated dispenser recipe sequence", "디스펜서 레시피 진행"), ("resume_state loaded", "중단 지점 복구"), ("resume_state step_start", "디스펜서 레시피 진행"), ("starting lid close", "뚜껑 체결 / 쉐이킹"), + ("ArUco lid_grip_close 성공 status 확인", "쉐이킹"), + ("Cup-holder pick completed; continuing to shake", "쉐이킹"), + ("SHAKE START", "쉐이킹"), + ("SHAKE DONE", "손 검출 / 핸드오버"), + ("starting MediaPipe human hand detection support process", "손 검출 / 핸드오버"), + ("shake succeeded; starting MediaPipe palm handover", "손 검출 / 핸드오버"), ("selected flow completed", "완료"), ) diff --git a/src/azas_voice/azas_voice/voice_screen_node.py b/src/azas_voice/azas_voice/voice_screen_node.py index c72f240..3328800 100644 --- a/src/azas_voice/azas_voice/voice_screen_node.py +++ b/src/azas_voice/azas_voice/voice_screen_node.py @@ -3,12 +3,14 @@ from collections import deque import json import mimetypes +import re from http import HTTPStatus from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer from pathlib import Path import threading import time from typing import Any +from urllib.parse import urlparse try: from ament_index_python.packages import get_package_share_directory @@ -18,15 +20,55 @@ try: import rclpy from rclpy.node import Node + from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy from std_msgs.msg import String except ImportError: # pragma: no cover - allows helper tests without sourced ROS rclpy = None Node = object + DurabilityPolicy = None + HistoryPolicy = None + QoSProfile = None + ReliabilityPolicy = None String = None +try: + import cv2 + import numpy as np +except ImportError: # pragma: no cover - camera support is optional for pure tests + cv2 = None + np = None + +try: + from azas_interfaces.msg import CupDetection + from sensor_msgs.msg import Image +except ImportError: # pragma: no cover - ROS message types are unavailable in pure tests + CupDetection = None + Image = None + from azas_voice.recipe_catalog import build_public_catalog +if QoSProfile is not None: + LOW_LATENCY_IMAGE_QOS = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) +else: # pragma: no cover - only used outside ROS test environments + LOW_LATENCY_IMAGE_QOS = 10 + +_CAMERA_STREAMS = {"realsense", "cup", "lid", "hand"} +_CENTER_PATTERNS = { + "cup": (r"\bcenter=\((\d+),(\d+)\)",), + "lid": ( + r"\blid_center=\((\d+),(\d+)\)", + r"\baruco_center=\((\d+),(\d+)\)", + r"\bcenter=\((\d+),(\d+)\)", + ), +} + + def build_initial_state() -> dict[str, Any]: return { "started_at": time.time(), @@ -37,6 +79,7 @@ def build_initial_state() -> dict[str, Any]: "decision": {}, "confirmed_decision": {}, "pipeline_status": {}, + "camera_status": {}, "events": [], } @@ -57,10 +100,25 @@ def __init__(self): self.declare_parameter("ui_state_topic", "/azas/voice/ui_state") self.declare_parameter("confirmed_decision_topic", "/azas/voice/confirmed_recipe_decision") self.declare_parameter("pipeline_status_topic", "/azas/voice/pipeline_status") + self.declare_parameter("camera_color_topic", "/camera/camera/color/image_raw") + self.declare_parameter("cup_detection_topic", "/azas/cup_detection") + self.declare_parameter("lid_detection_topic", "/azas/lid_detection") + self.declare_parameter("hand_overlay_topic", "/azas/human_hand_detection/overlay") + self.declare_parameter("camera_stream_width_px", 720) + self.declare_parameter("camera_jpeg_quality", 78) self._lock = threading.Lock() + self._camera_lock = threading.Lock() self._events: deque[dict[str, Any]] = deque(maxlen=12) self._state = build_initial_state() + self._camera_frames: dict[str, dict[str, Any]] = {} + self._detection_status: dict[str, dict[str, Any]] = { + "cup": {"status": "", "at": 0.0}, + "lid": {"status": "", "at": 0.0}, + } + self._camera_errors: dict[str, float] = {} + self._camera_stream_width_px = max(240, int(self.get_parameter("camera_stream_width_px").value)) + self._camera_jpeg_quality = max(35, min(95, int(self.get_parameter("camera_jpeg_quality").value))) self._stt_pub = self.create_publisher( String, @@ -103,6 +161,7 @@ def __init__(self): self._on_pipeline_status, 10, ) + self._start_camera_subscriptions() self._web_root = Path(get_package_share_directory("azas_voice")) / "web" host = str(self.get_parameter("host").value) @@ -116,8 +175,40 @@ def snapshot(self) -> dict[str, Any]: with self._lock: payload = dict(self._state) payload["events"] = list(self._events) + payload["camera_status"] = self._camera_status_snapshot() return payload + def camera_jpeg(self, stream: str) -> bytes: + if stream not in _CAMERA_STREAMS: + raise ValueError(f"unknown camera stream: {stream}") + if cv2 is None or np is None: + raise ValueError("opencv/numpy camera support is not available") + + frame = self._frame_for_stream(stream) + if frame is None: + raise ValueError(f"no frame available for camera stream: {stream}") + + if stream == "cup": + status = self._latest_detection("cup") + _draw_detection_overlay(frame, status["status"], kind="cup", status_time=status["at"]) + elif stream == "lid": + status = self._latest_detection("lid") + _draw_detection_overlay(frame, status["status"], kind="lid", status_time=status["at"]) + elif stream == "hand" and not self._has_recent_frame("hand", max_age_sec=2.0): + _draw_stream_label(frame, "HAND DETECTION WAITING", "waiting for /azas/human_hand_detection/overlay") + else: + _draw_stream_label(frame, _stream_label(stream), "") + + frame = _resize_for_stream(frame, max_width=self._camera_stream_width_px) + ok, encoded = cv2.imencode( + ".jpg", + frame, + [int(cv2.IMWRITE_JPEG_QUALITY), self._camera_jpeg_quality], + ) + if not ok: + raise ValueError("failed to encode camera frame") + return encoded.tobytes() + def publish_test_utterance(self, text: str) -> None: utterance = text.strip() if not utterance: @@ -169,6 +260,117 @@ def _on_pipeline_status(self, msg: String) -> None: self._state["pipeline_status"] = _json_or_text(msg.data) self._state["pipeline_status_at"] = time.time() + def _start_camera_subscriptions(self) -> None: + if Image is None or cv2 is None or np is None: + self.get_logger().warn("Camera UI disabled: sensor_msgs, OpenCV, or numpy is unavailable") + return + + self.create_subscription( + Image, + str(self.get_parameter("camera_color_topic").value), + lambda msg: self._on_camera_image("realsense", msg), + LOW_LATENCY_IMAGE_QOS, + ) + self.create_subscription( + Image, + str(self.get_parameter("hand_overlay_topic").value), + lambda msg: self._on_camera_image("hand", msg), + LOW_LATENCY_IMAGE_QOS, + ) + if CupDetection is None: + self.get_logger().warn("Camera overlays disabled: azas_interfaces/CupDetection is unavailable") + return + self.create_subscription( + CupDetection, + str(self.get_parameter("cup_detection_topic").value), + lambda msg: self._on_detection("cup", msg), + 10, + ) + self.create_subscription( + CupDetection, + str(self.get_parameter("lid_detection_topic").value), + lambda msg: self._on_detection("lid", msg), + 10, + ) + self.get_logger().info( + "Camera UI streams ready: " + f"color={self.get_parameter('camera_color_topic').value}, " + f"cup={self.get_parameter('cup_detection_topic').value}, " + f"lid={self.get_parameter('lid_detection_topic').value}, " + f"hand={self.get_parameter('hand_overlay_topic').value}" + ) + + def _on_camera_image(self, stream: str, msg: Any) -> None: + try: + frame = _image_msg_to_bgr(msg) + except ValueError as exc: + now = time.monotonic() + last = self._camera_errors.get(stream, 0.0) + if now - last > 2.0: + self._camera_errors[stream] = now + self.get_logger().warn(f"{stream} camera frame ignored: {exc}") + return + with self._camera_lock: + self._camera_frames[stream] = { + "frame": frame, + "at": time.monotonic(), + "encoding": str(getattr(msg, "encoding", "")), + "width": int(getattr(msg, "width", 0) or frame.shape[1]), + "height": int(getattr(msg, "height", 0) or frame.shape[0]), + } + + def _on_detection(self, kind: str, msg: Any) -> None: + with self._camera_lock: + self._detection_status[kind] = { + "status": str(getattr(msg, "status", "")), + "at": time.monotonic(), + } + + def _frame_for_stream(self, stream: str) -> Any | None: + source = "hand" if stream == "hand" and self._has_recent_frame("hand", max_age_sec=5.0) else "realsense" + with self._camera_lock: + item = self._camera_frames.get(source) + if item is None: + return None + return item["frame"].copy() + + def _latest_detection(self, kind: str) -> dict[str, Any]: + with self._camera_lock: + return dict(self._detection_status.get(kind, {"status": "", "at": 0.0})) + + def _has_recent_frame(self, stream: str, *, max_age_sec: float) -> bool: + with self._camera_lock: + item = self._camera_frames.get(stream) + return item is not None and time.monotonic() - float(item["at"]) <= max_age_sec + + def _camera_status_snapshot(self) -> dict[str, Any]: + now = time.monotonic() + with self._camera_lock: + frames = { + name: { + "available": True, + "age_sec": round(now - float(item["at"]), 3), + "width": item.get("width"), + "height": item.get("height"), + "encoding": item.get("encoding"), + } + for name, item in self._camera_frames.items() + } + detections = { + name: { + "status": item.get("status", ""), + "age_sec": round(now - float(item.get("at", 0.0)), 3) + if float(item.get("at", 0.0)) > 0.0 + else None, + } + for name, item in self._detection_status.items() + } + return { + "enabled": Image is not None and cv2 is not None and np is not None, + "frames": frames, + "detections": detections, + } + def _remember(self, speaker: str, text: str) -> None: with self._lock: self._events.appendleft( @@ -184,24 +386,35 @@ def _build_handler(self): class VoiceScreenRequestHandler(BaseHTTPRequestHandler): def do_GET(self) -> None: - if self.path in {"/", "/voice.html"}: + route = urlparse(self.path).path + if route in {"/", "/voice.html"}: self._send_file(node._web_root / "voice.html") return - if self.path == "/voice.css": + if route == "/voice.css": self._send_file(node._web_root / "voice.css") return - if self.path == "/voice.js": + if route == "/voice.js": self._send_file(node._web_root / "voice.js") return - if self.path == "/api/state": + if route == "/api/state": self._send_json(node.snapshot()) return + if route.startswith("/api/camera/") and route.endswith(".jpg"): + stream = route.removeprefix("/api/camera/").removesuffix(".jpg") + try: + data = node.camera_jpeg(stream) + except ValueError as exc: + self.send_error(HTTPStatus.SERVICE_UNAVAILABLE, str(exc)) + return + self._send_bytes(data, "image/jpeg") + return self.send_error(HTTPStatus.NOT_FOUND) def do_POST(self) -> None: try: + route = urlparse(self.path).path payload = self._read_json() - if self.path != "/api/utterance": + if route != "/api/utterance": raise ValueError(f"unknown endpoint: {self.path}") text = str(payload.get("text", "")) node.publish_test_utterance(text) @@ -238,6 +451,14 @@ def _send_json( self.end_headers() self.wfile.write(data) + def _send_bytes(self, data: bytes, content_type: str) -> None: + self.send_response(HTTPStatus.OK) + self.send_header("Content-Type", content_type) + self.send_header("Content-Length", str(len(data))) + self.send_header("Cache-Control", "no-store, max-age=0") + self.end_headers() + self.wfile.write(data) + def _send_file(self, path: Path) -> None: if not path.is_file(): self.send_error(HTTPStatus.NOT_FOUND) @@ -266,6 +487,185 @@ def _json_or_text(text: str) -> Any: return {"text": text} +def _image_msg_to_bgr(msg: Any) -> Any: + if cv2 is None or np is None: + raise ValueError("opencv/numpy are required") + height = int(getattr(msg, "height", 0)) + width = int(getattr(msg, "width", 0)) + if height <= 0 or width <= 0: + raise ValueError(f"invalid image dimensions: {width}x{height}") + + encoding = str(getattr(msg, "encoding", "")).lower() + if encoding in {"bgr8", "rgb8"}: + rows = _image_rows(msg, bytes_per_pixel=3) + image = rows.reshape((height, width, 3)) + if encoding == "rgb8": + return cv2.cvtColor(image, cv2.COLOR_RGB2BGR) + return image.copy() + + if encoding in {"bgra8", "rgba8"}: + rows = _image_rows(msg, bytes_per_pixel=4) + image = rows.reshape((height, width, 4)) + code = cv2.COLOR_BGRA2BGR if encoding == "bgra8" else cv2.COLOR_RGBA2BGR + return cv2.cvtColor(image, code) + + if encoding in {"mono8", "8uc1"}: + rows = _image_rows(msg, bytes_per_pixel=1) + gray = rows.reshape((height, width)) + return cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR) + + if encoding == "16uc1": + rows = _image_rows(msg, bytes_per_pixel=2) + depth = np.ascontiguousarray(rows).view(np.uint16).reshape((height, width)) + return _depth_to_bgr(depth) + + if encoding == "32fc1": + rows = _image_rows(msg, bytes_per_pixel=4) + depth = np.ascontiguousarray(rows).view(np.float32).reshape((height, width)) + return _depth_to_bgr(depth) + + raise ValueError(f"unsupported image encoding: {getattr(msg, 'encoding', '')}") + + +def _image_rows(msg: Any, *, bytes_per_pixel: int) -> Any: + if np is None: + raise ValueError("numpy is required") + height = int(getattr(msg, "height", 0)) + width = int(getattr(msg, "width", 0)) + step = int(getattr(msg, "step", 0)) or width * bytes_per_pixel + expected = height * step + raw = np.frombuffer(getattr(msg, "data", b""), dtype=np.uint8) + if raw.size < expected: + raise ValueError(f"image buffer too small: {raw.size} < {expected}") + return np.ascontiguousarray(raw[:expected].reshape((height, step))[:, : width * bytes_per_pixel]) + + +def _depth_to_bgr(depth: Any) -> Any: + if cv2 is None or np is None: + raise ValueError("opencv/numpy are required") + finite = np.asarray(depth, dtype=np.float32) + finite = np.nan_to_num(finite, nan=0.0, posinf=0.0, neginf=0.0) + if float(np.max(finite)) <= float(np.min(finite)): + normalized = np.zeros(finite.shape, dtype=np.uint8) + else: + normalized = cv2.normalize(finite, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8) + return cv2.cvtColor(normalized, cv2.COLOR_GRAY2BGR) + + +def _parse_detection_overlay(status: str, *, kind: str) -> dict[str, Any]: + text = str(status or "") + center = None + for pattern in _CENTER_PATTERNS.get(kind, (r"\bcenter=\((\d+),(\d+)\)",)): + match = re.search(pattern, text) + if match: + center = (int(match.group(1)), int(match.group(2))) + break + + bbox = None + bbox_match = re.search(r"\bbbox=(\d+)x(\d+)", text) + if bbox_match: + bbox = (int(bbox_match.group(1)), int(bbox_match.group(2))) + + orientation = _parse_orientation(text) + detected = text.startswith("detected:") + return { + "text": text, + "center": center, + "bbox": bbox, + "orientation": orientation, + "detected": detected, + } + + +def _parse_orientation(status: str) -> str: + normalized = str(status or "").lower() + match = re.search(r"\borientation=([a-z_]+)", normalized) + if match: + return match.group(1) + if normalized.startswith("detected:upright"): + return "upright" + if normalized.startswith("rejected:lying"): + return "lying" + return "" + + +def _draw_detection_overlay(frame: Any, status: str, *, kind: str, status_time: float) -> None: + if cv2 is None: + return + parsed = _parse_detection_overlay(status, kind=kind) + age = time.monotonic() - status_time if status_time > 0.0 else float("inf") + stale = age > 1.2 + + if kind == "cup": + if parsed["orientation"] == "upright": + label = "CUP UPRIGHT" + color = (44, 220, 125) + elif parsed["orientation"] == "lying": + label = "CUP LYING" + color = (0, 176, 255) + else: + label = "CUP DETECTION WAITING" + color = (74, 74, 255) + elif parsed["detected"]: + label = "LID DETECTED" + color = (64, 220, 230) + else: + label = "LID DETECTION WAITING" + color = (74, 74, 255) + + if stale: + label = f"{label} STALE" + color = (160, 160, 160) + + center = parsed["center"] + bbox = parsed["bbox"] + if center is not None and bbox is not None: + cx, cy = center + bw, bh = bbox + x1 = max(int(cx - bw / 2), 0) + y1 = max(int(cy - bh / 2), 0) + x2 = min(int(cx + bw / 2), frame.shape[1] - 1) + y2 = min(int(cy + bh / 2), frame.shape[0] - 1) + cv2.rectangle(frame, (x1, y1), (x2, y2), color, 3) + cv2.circle(frame, (cx, cy), 5, color, -1) + + detail = parsed["text"][:96] if parsed["text"] else "" + _draw_stream_label(frame, label, detail, color=color) + + +def _draw_stream_label(frame: Any, label: str, detail: str = "", *, color: tuple[int, int, int] = (95, 216, 173)) -> None: + if cv2 is None: + return + height, width = frame.shape[:2] + box_width = min(width - 24, 700) + box_height = 78 if detail else 56 + overlay = frame.copy() + cv2.rectangle(overlay, (12, 12), (12 + box_width, 12 + box_height), (0, 0, 0), -1) + cv2.addWeighted(overlay, 0.54, frame, 0.46, 0, frame) + cv2.putText(frame, label, (26, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.88, color, 2, cv2.LINE_AA) + if detail: + cv2.putText(frame, detail, (26, 76), cv2.FONT_HERSHEY_SIMPLEX, 0.48, (235, 235, 235), 1, cv2.LINE_AA) + + +def _resize_for_stream(frame: Any, *, max_width: int) -> Any: + if cv2 is None: + return frame + height, width = frame.shape[:2] + if width <= max_width: + return frame + scale = max_width / float(width) + return cv2.resize(frame, (max_width, int(round(height * scale))), interpolation=cv2.INTER_AREA) + + +def _stream_label(stream: str) -> str: + return { + "realsense": "REALSENSE LIVE", + "cup": "CUP DETECTION", + "lid": "LID DETECTION", + "hand": "HAND DETECTION", + }.get(stream, "CAMERA") + + def main(args=None): if rclpy is None: raise RuntimeError("ROS 2 Python packages are not available. Source the ROS environment first.") diff --git a/src/azas_voice/launch/azas_voice.launch.py b/src/azas_voice/launch/azas_voice.launch.py index b3e3f42..9eba07d 100644 --- a/src/azas_voice/launch/azas_voice.launch.py +++ b/src/azas_voice/launch/azas_voice.launch.py @@ -46,6 +46,10 @@ def generate_launch_description(): DeclareLaunchArgument("run_voice_screen", default_value="true"), DeclareLaunchArgument("voice_screen_host", default_value="0.0.0.0"), DeclareLaunchArgument("voice_screen_port", default_value="8090"), + DeclareLaunchArgument("voice_screen_camera_color_topic", default_value="/camera/camera/color/image_raw"), + DeclareLaunchArgument("voice_screen_cup_detection_topic", default_value="/azas/cup_detection"), + DeclareLaunchArgument("voice_screen_lid_detection_topic", default_value="/azas/lid_detection"), + DeclareLaunchArgument("voice_screen_hand_overlay_topic", default_value="/azas/human_hand_detection/overlay"), DeclareLaunchArgument("use_tts", default_value="true"), DeclareLaunchArgument("enable_tts_audio", default_value="true"), DeclareLaunchArgument("tts_speech_rate", default_value="1.25"), @@ -198,6 +202,10 @@ def generate_launch_description(): LaunchConfiguration("voice_screen_port"), value_type=int ), "stt_topic": stt_topic, + "camera_color_topic": LaunchConfiguration("voice_screen_camera_color_topic"), + "cup_detection_topic": LaunchConfiguration("voice_screen_cup_detection_topic"), + "lid_detection_topic": LaunchConfiguration("voice_screen_lid_detection_topic"), + "hand_overlay_topic": LaunchConfiguration("voice_screen_hand_overlay_topic"), } ], condition=IfCondition(run_voice_screen), diff --git a/src/azas_voice/test/test_voice_pipeline_recovery_helpers.py b/src/azas_voice/test/test_voice_pipeline_recovery_helpers.py index aa63912..2a81644 100644 --- a/src/azas_voice/test/test_voice_pipeline_recovery_helpers.py +++ b/src/azas_voice/test/test_voice_pipeline_recovery_helpers.py @@ -1,6 +1,7 @@ from azas_voice.voice_pipeline_executor_node import ( load_resume_snapshot, recipe_colors_from_resume_snapshot, + stage_from_line, ) @@ -28,3 +29,11 @@ def test_load_resume_snapshot_rejects_missing_and_invalid_files(tmp_path): valid = tmp_path / "valid.json" valid.write_text('{"status": "stopped"}\n', encoding="utf-8") assert load_resume_snapshot(valid) == {"status": "stopped"} + + +def test_stage_from_line_tracks_lid_shake_and_hand_detection_boundaries(): + assert stage_from_line("starting perception with cup classifier: ros2 launch azas_bringup yolo_perception.launch.py") == "컵 자세 구분" + assert stage_from_line("waiting for stable route: samples=5, min_sec=0.80, view_hold=3.50s") == "컵 자세 구분" + assert stage_from_line("[Azas] ArUco lid_grip_close 성공 status 확인 -> 컵홀더 컵 다시 잡기 후 쉐이킹") == "쉐이킹" + assert stage_from_line("[Azas] SHAKE START: 컵홀더에 놓인 닫힌 컵을 측정 pose로 다시 side-grip 픽업") == "쉐이킹" + assert stage_from_line("shake succeeded; starting MediaPipe palm handover") == "손 검출 / 핸드오버" diff --git a/src/azas_voice/test/test_voice_screen_node.py b/src/azas_voice/test/test_voice_screen_node.py index 4fe02b5..87e9632 100644 --- a/src/azas_voice/test/test_voice_screen_node.py +++ b/src/azas_voice/test/test_voice_screen_node.py @@ -1,4 +1,13 @@ -from azas_voice.voice_screen_node import _json_or_text, build_initial_state +from types import SimpleNamespace + +import pytest + +from azas_voice.voice_screen_node import ( + _image_msg_to_bgr, + _json_or_text, + _parse_detection_overlay, + build_initial_state, +) def test_voice_screen_initial_state_has_dialogue_fields(): @@ -20,3 +29,45 @@ def test_json_or_text_wraps_plain_text(): payload = _json_or_text("진행할까요?") assert payload == {"text": "진행할까요?"} + + +def test_parse_cup_detection_overlay_extracts_orientation_center_and_bbox(): + payload = _parse_detection_overlay( + "detected:upright class=tumbler bbox=120x220 orientation=upright center=(321,240)", + kind="cup", + ) + + assert payload["detected"] is True + assert payload["orientation"] == "upright" + assert payload["center"] == (321, 240) + assert payload["bbox"] == (120, 220) + + +def test_parse_lid_detection_overlay_prefers_lid_center(): + payload = _parse_detection_overlay( + "detected:lid class=lid bbox=80x64 lid_center=(410,220) aruco_center=(398,218)", + kind="lid", + ) + + assert payload["detected"] is True + assert payload["center"] == (410, 220) + assert payload["bbox"] == (80, 64) + + +def test_image_msg_to_bgr_converts_rgb8_with_row_step(): + pytest.importorskip("cv2") + np = pytest.importorskip("numpy") + + msg = SimpleNamespace( + height=1, + width=2, + encoding="rgb8", + step=8, + data=bytes([255, 0, 0, 0, 255, 0, 99, 99]), + ) + + frame = _image_msg_to_bgr(msg) + + assert frame.shape == (1, 2, 3) + np.testing.assert_array_equal(frame[0, 0], np.array([0, 0, 255], dtype=np.uint8)) + np.testing.assert_array_equal(frame[0, 1], np.array([0, 255, 0], dtype=np.uint8)) diff --git a/src/azas_voice/web/voice.css b/src/azas_voice/web/voice.css index e781c2d..ba85a32 100644 --- a/src/azas_voice/web/voice.css +++ b/src/azas_voice/web/voice.css @@ -479,6 +479,27 @@ input { bottom: 31px; } +.robot-scene[data-step="handover"] .robot-cup { + left: calc(100% - 104px); + bottom: 68px; + transform: rotate(4deg); +} + +.robot-scene[data-step="handover"] .robot-gripper { + left: calc(100% - 132px); + bottom: 86px; +} + +.robot-scene[data-step="handover"] .robot-shaker { + right: 22px; + bottom: 34px; + width: 52px; + height: 18px; + border-radius: 999px 999px 10px 10px; + background: linear-gradient(180deg, #ffd9b0, #f0ae82); + opacity: 1; +} + .robot-status-text { margin: 0; color: var(--muted); @@ -486,6 +507,99 @@ input { font-weight: 900; } +.vision-camera-panel { + display: grid; + gap: 10px; + padding: 16px; + border: 1px solid var(--line); + border-radius: 8px; + background: rgba(255, 255, 255, 0.68); + box-shadow: 0 16px 36px rgba(67, 119, 96, 0.1); +} + +.vision-camera-title-row { + display: flex; + align-items: flex-start; + justify-content: space-between; + gap: 12px; +} + +.vision-camera-title-row h2 { + margin: 0; + font-size: 22px; + line-height: 1.18; +} + +.vision-camera-badge { + flex: 0 0 auto; + max-width: 180px; + padding: 8px 11px; + border-radius: 999px; + color: #10745b; + background: rgba(178, 245, 216, 0.82); + overflow-wrap: anywhere; + text-align: center; + font-size: 13px; + font-weight: 900; +} + +.vision-camera-badge[data-stream="cup"] { + color: #0f5fa8; + background: rgba(176, 219, 255, 0.9); +} + +.vision-camera-badge[data-stream="lid"] { + color: #8b5310; + background: rgba(255, 226, 143, 0.92); +} + +.vision-camera-badge[data-stream="hand"] { + color: #a8253c; + background: rgba(255, 196, 206, 0.88); +} + +.vision-camera-frame { + position: relative; + width: 100%; + aspect-ratio: 16 / 9; + min-height: 210px; + overflow: hidden; + border: 1px solid rgba(38, 54, 49, 0.2); + border-radius: 8px; + background: #101815; +} + +.vision-camera-frame img { + display: block; + width: 100%; + height: 100%; + object-fit: contain; + background: #101815; +} + +.vision-camera-empty { + position: absolute; + inset: 0; + display: grid; + place-items: center; + padding: 18px; + color: rgba(255, 255, 255, 0.78); + text-align: center; + font-weight: 900; +} + +.vision-camera-empty[hidden] { + display: none; +} + +.vision-camera-detail { + margin: 0; + color: var(--muted); + overflow-wrap: anywhere; + font-size: 13px; + font-weight: 900; +} + .catalog-panel { display: grid; gap: 10px; diff --git a/src/azas_voice/web/voice.html b/src/azas_voice/web/voice.html index f2d85e8..4500dc0 100644 --- a/src/azas_voice/web/voice.html +++ b/src/azas_voice/web/voice.html @@ -128,6 +128,7 @@
  • 컵 픽업
  • 디스펜서
  • 뚜껑·쉐이킹
  • +
  • 손 검출
  • 완료
  • @@ -149,6 +150,21 @@
    +
    +
    +
    +

    Azas Vision

    +

    Realsense 실시간 화면

    +
    + 대기 +
    +
    + Azas 비전 카메라 실시간 화면 +
    카메라 프레임 대기
    +
    +

    /camera/camera/color/image_raw

    +
    +

    메뉴 카탈로그

    diff --git a/src/azas_voice/web/voice.js b/src/azas_voice/web/voice.js index abd10d5..c8fb599 100644 --- a/src/azas_voice/web/voice.js +++ b/src/azas_voice/web/voice.js @@ -26,6 +26,12 @@ const statAcidity = document.querySelector("#stat-acidity"); const statStrength = document.querySelector("#stat-strength"); const robotScene = document.querySelector("#robot-scene"); const robotStatusText = document.querySelector("#robot-status-text"); +const visionCameraPanel = document.querySelector("#vision-camera-panel"); +const visionCameraTitle = document.querySelector("#vision-camera-title"); +const visionCameraBadge = document.querySelector("#vision-camera-badge"); +const visionCameraImage = document.querySelector("#vision-camera-image"); +const visionCameraEmpty = document.querySelector("#vision-camera-empty"); +const visionCameraDetail = document.querySelector("#vision-camera-detail"); const INGREDIENTS = { red: { label: "주스", color: "#ff7e96" }, @@ -57,18 +63,52 @@ let catalogSignature = ""; // 라우터 단계명(/azas/voice/pipeline_status의 stage) -> 진행 스텝 인덱스 const STAGE_TO_STEP = { "디스펜서 색 스캔": 0, + "컵 자세 구분": 1, "컵 픽업 (세워진 컵)": 1, "컵 픽업 (쓰러진 컵)": 1, "디스펜서 레시피 진행": 2, "중단 지점 복구": 2, "뚜껑 체결 / 쉐이킹": 3, - "완료": 4, + "쉐이킹": 3, + "손 검출 / 핸드오버": 4, + "완료": 5, +}; + +const CUP_CAMERA_STAGES = new Set(["컵 자세 구분"]); + +const CAMERA_MODES = { + realsense: { + stream: "realsense", + title: "Realsense 실시간 화면", + badge: "실시간", + detail: "/camera/camera/color/image_raw", + }, + cup: { + stream: "cup", + title: "컵 자세 검출", + badge: "upright / lying", + detail: "/azas/cup_detection + Realsense", + }, + lid: { + stream: "lid", + title: "뚜껑 검출", + badge: "lid detection", + detail: "/azas/lid_detection + Realsense", + }, + hand: { + stream: "hand", + title: "손 검출", + badge: "open palm", + detail: "/azas/human_hand_detection/overlay", + }, }; // 잔 내부(clip-path 기준): y 30~167, x 33~127 const GLASS_TOP = 30; const GLASS_BOTTOM = 167; const FILL_RATIO = 0.86; +let cupCameraHoldUntil = 0; +let currentCameraStream = ""; function amountsFromDecision(decision) { const amounts = {}; @@ -219,8 +259,15 @@ function renderRobot(activeIndex, pipeline, hasMenu) { robotStatusText.textContent = "완료"; return; } - const stepNames = ["scan", "pick", "dispense", "shake", "done"]; - const statusText = ["디스펜서 색 스캔", "컵 픽업", "디스펜서 토출", "뚜껑 체결 / 쉐이킹", "완료"]; + const stepNames = ["scan", "pick", "dispense", "shake", "handover", "done"]; + const statusText = [ + "디스펜서 색 스캔", + "컵 픽업", + "디스펜서 토출", + "뚜껑 체결 / 쉐이킹", + "손 검출 / 핸드오버", + "완료", + ]; const index = activeIndex >= 0 ? activeIndex : 0; robotScene.dataset.step = stepNames[Math.min(index, stepNames.length - 1)]; robotStatusText.textContent = pipeline.stage || statusText[Math.min(index, statusText.length - 1)]; @@ -360,6 +407,55 @@ function renderMenu(state) { } } +function selectCameraMode(state) { + const pipeline = state.pipeline_status || {}; + const status = pipeline.status || ""; + const stage = String(pipeline.stage || ""); + const now = Date.now(); + + if (status === "running" && CUP_CAMERA_STAGES.has(stage)) { + cupCameraHoldUntil = now + 2000; + return CAMERA_MODES.cup; + } + if (status === "running" && stage === "뚜껑 체결 / 쉐이킹") { + return CAMERA_MODES.lid; + } + if (status === "running" && stage === "손 검출 / 핸드오버") { + return CAMERA_MODES.hand; + } + if (cupCameraHoldUntil > now) { + return CAMERA_MODES.cup; + } + return CAMERA_MODES.realsense; +} + +function renderVisionCamera(state) { + if (!visionCameraPanel) return; + + const mode = selectCameraMode(state); + const cameraStatus = state.camera_status || {}; + const frames = cameraStatus.frames || {}; + const sourceAvailable = + mode.stream === "hand" ? Boolean(frames.hand || frames.realsense) : Boolean(frames.realsense); + + visionCameraTitle.textContent = mode.title; + visionCameraBadge.textContent = mode.badge; + visionCameraBadge.dataset.stream = mode.stream; + visionCameraDetail.textContent = mode.detail; + visionCameraEmpty.hidden = sourceAvailable; + visionCameraImage.hidden = !sourceAvailable; + + if (!sourceAvailable) { + currentCameraStream = ""; + visionCameraImage.removeAttribute("src"); + return; + } + + const nextSrc = `/api/camera/${mode.stream}.jpg?t=${Date.now()}`; + currentCameraStream = mode.stream; + visionCameraImage.src = nextSrc; +} + let analyser = null; let timeData = null; let micLevel = 0; @@ -529,6 +625,7 @@ async function refreshState() { intent.textContent = decision.intent || "대기"; confirmed.textContent = confirmedDecision.confirmed ? "확정됨" : "대기"; renderMenu(state); + renderVisionCamera(state); } async function postUtterance(text) { @@ -566,6 +663,14 @@ testForm.addEventListener("submit", async (event) => { } }); +visionCameraImage.addEventListener("error", () => { + visionCameraImage.hidden = true; + visionCameraEmpty.hidden = false; + if (currentCameraStream) { + visionCameraEmpty.textContent = `${currentCameraStream} 프레임 대기`; + } +}); + drawWaveform(); refreshState().catch(() => {}); setInterval(() => { From 5bd7452aa311491d47e5a70d4e520557f11a0b18 Mon Sep 17 00:00:00 2001 From: oyeong011 Date: Mon, 15 Jun 2026 20:42:51 +0900 Subject: [PATCH 88/88] Preserve side-grip and cup-holder offsets before handover merge Constraint: User requested committing current branch work before merging fix/eliminate-eleven-mediapipe-handover. Rejected: Stashing local edits | User explicitly asked not to stash. Confidence: medium Scope-risk: moderate Directive: Preserve current side-grip behavior and dispenser 4 cup-holder offset during the merge. Tested: Not run by user directive. Not-tested: Dry-run, build, syntax checks, hardware validation. --- omx_wiki/log.md | 5 +- omx_wiki/robot-operation-rules.md | 12 +++ .../launch/auto_cup_flow_router.launch.py | 18 ++++ .../azas_task_manager/auto_cup_flow_router.py | 17 ++- .../dsr_practice/yolo_cup_pick_node.py | 102 +++++++++++++++++- .../launch/yolo_cup_pick_node.launch.py | 18 ++++ tools/run/robot_pipeline_control_server.py | 16 ++- tools/run/run_changhyun_side_grip_direct.sh | 4 +- tools/run/run_color_recipe_sequence.py | 4 +- .../run_measured_dispenser_recipe_sequence.py | 21 +++- tools/run/run_tmux_logic_sequence.sh | 2 + tools/run/run_voice_auto_cup_flow.sh | 2 +- 12 files changed, 210 insertions(+), 11 deletions(-) diff --git a/omx_wiki/log.md b/omx_wiki/log.md index 97ffc6a..cde1ac1 100644 --- a/omx_wiki/log.md +++ b/omx_wiki/log.md @@ -1,5 +1,9 @@ # Wiki Log +## [2026-06-15] update +- **Pages:** robot-operation-rules.md +- **Summary:** Recorded the user directive that future dry-run, build, syntax-check, test, smoke-test, and verification commands are not to be run automatically; the user owns operational checks and the agent must report validation gaps instead. + ## [2026-06-10T16:48:48+09:00] update - **Pages:** robot-operation-rules.md - **Summary:** Recorded the operator confirmation that dispenser 4 press-only real-motion check also works normally. @@ -84,4 +88,3 @@ ## [2026-06-11T23:23:12.252Z] session-end - **Pages:** session-log-2026-06-11-6-dnt3rj.md - **Summary:** Auto-captured session log for omx-1781218680276-dnt3rj - diff --git a/omx_wiki/robot-operation-rules.md b/omx_wiki/robot-operation-rules.md index 08b8e13..3cdf863 100644 --- a/omx_wiki/robot-operation-rules.md +++ b/omx_wiki/robot-operation-rules.md @@ -1,5 +1,17 @@ # Robot Operation Rules +## 2026-06-15 Agent Validation Ownership Rule + +User directive: 앞으로 점검은 사용자의 몫임. 드라이런, 빌드, 검증 금지. + +Required behavior: + +- Do not automatically run dry-run, build, lint, test, syntax-check, simulation, smoke-test, or verification commands after edits unless the user explicitly requests that exact check. +- Read-only inspection needed to understand logs, source code, configuration, and requested edit targets is allowed. +- After making edits, report that dry-run/build/verification were intentionally not run by user directive. +- Do not present unverified edits as validated or tested. State the validation gap plainly. +- Robot-motion acceptance, runtime checks, and final operational confirmation are the user's responsibility. + ## 2026-06-10 Real Motion Rule: Dry-Run Banned User directive: 드라이런은 앞으로 금지. diff --git a/src/azas_bringup/launch/auto_cup_flow_router.launch.py b/src/azas_bringup/launch/auto_cup_flow_router.launch.py index 9f404a2..090348f 100644 --- a/src/azas_bringup/launch/auto_cup_flow_router.launch.py +++ b/src/azas_bringup/launch/auto_cup_flow_router.launch.py @@ -25,6 +25,10 @@ def generate_launch_description(): DeclareLaunchArgument("side_extra_args", default_value=""), DeclareLaunchArgument("cup_uprighting_extra_args", default_value=""), DeclareLaunchArgument("side_target_x_offset_m", default_value="-0.02"), + DeclareLaunchArgument("side_target_joint6_inset_m", default_value="0.070"), + DeclareLaunchArgument("side_target_joint6_inset_sign", default_value="1.0"), + DeclareLaunchArgument("side_pre_pick_joint1_clearance_deg", default_value="12.0"), + DeclareLaunchArgument("side_return_to_camera_home_after_attempt", default_value="true"), DeclareLaunchArgument("side_trajectory_execution_duration_scaling", default_value="3.0"), DeclareLaunchArgument("side_trajectory_execution_goal_margin_sec", default_value="3.0"), DeclareLaunchArgument("color_scan_at_start", default_value="true"), @@ -32,9 +36,11 @@ def generate_launch_description(): DeclareLaunchArgument("recipe_colors", default_value=""), DeclareLaunchArgument("cup_pre_from_place_x_offset_m", default_value="-0.12"), DeclareLaunchArgument("dispenser_3_cup_pre_extra_x_offset_m", default_value="-0.01"), + DeclareLaunchArgument("final_regrasp_x_offset_m", default_value="0.020"), DeclareLaunchArgument("final_regrasp_z_offset_m", default_value="0.0"), DeclareLaunchArgument("cup_holder_place_z_offset_m", default_value="-0.04"), DeclareLaunchArgument("cup_holder_place_x_offset_m", default_value="0.010"), + DeclareLaunchArgument("cup_holder_place_final_dispenser_4_x_extra_offset_m", default_value="-0.010"), DeclareLaunchArgument("cup_holder_place_y_offset_m", default_value="-0.010"), DeclareLaunchArgument("cup_holder_rz_offset_deg", default_value="-1.0"), DeclareLaunchArgument("cup_holder_z_min_m", default_value="0.06"), @@ -92,6 +98,13 @@ def generate_launch_description(): "side_extra_args": LaunchConfiguration("side_extra_args"), "cup_uprighting_extra_args": LaunchConfiguration("cup_uprighting_extra_args"), "side_target_x_offset_m": ParameterValue(LaunchConfiguration("side_target_x_offset_m"), value_type=float), + "side_target_joint6_inset_m": ParameterValue(LaunchConfiguration("side_target_joint6_inset_m"), value_type=float), + "side_target_joint6_inset_sign": ParameterValue(LaunchConfiguration("side_target_joint6_inset_sign"), value_type=float), + "side_pre_pick_joint1_clearance_deg": ParameterValue(LaunchConfiguration("side_pre_pick_joint1_clearance_deg"), value_type=float), + "side_return_to_camera_home_after_attempt": ParameterValue( + LaunchConfiguration("side_return_to_camera_home_after_attempt"), + value_type=bool, + ), "side_trajectory_execution_duration_scaling": ParameterValue( LaunchConfiguration("side_trajectory_execution_duration_scaling"), value_type=float, @@ -105,9 +118,14 @@ def generate_launch_description(): "recipe_colors": LaunchConfiguration("recipe_colors"), "cup_pre_from_place_x_offset_m": ParameterValue(LaunchConfiguration("cup_pre_from_place_x_offset_m"), value_type=float), "dispenser_3_cup_pre_extra_x_offset_m": ParameterValue(LaunchConfiguration("dispenser_3_cup_pre_extra_x_offset_m"), value_type=float), + "final_regrasp_x_offset_m": ParameterValue(LaunchConfiguration("final_regrasp_x_offset_m"), value_type=float), "final_regrasp_z_offset_m": ParameterValue(LaunchConfiguration("final_regrasp_z_offset_m"), value_type=float), "cup_holder_place_z_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_z_offset_m"), value_type=float), "cup_holder_place_x_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_x_offset_m"), value_type=float), + "cup_holder_place_final_dispenser_4_x_extra_offset_m": ParameterValue( + LaunchConfiguration("cup_holder_place_final_dispenser_4_x_extra_offset_m"), + value_type=float, + ), "cup_holder_place_y_offset_m": ParameterValue(LaunchConfiguration("cup_holder_place_y_offset_m"), value_type=float), "cup_holder_rz_offset_deg": ParameterValue(LaunchConfiguration("cup_holder_rz_offset_deg"), value_type=float), "cup_holder_z_min_m": ParameterValue(LaunchConfiguration("cup_holder_z_min_m"), value_type=float), diff --git a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py index 094f49b..f416895 100644 --- a/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py +++ b/src/azas_task_manager/azas_task_manager/auto_cup_flow_router.py @@ -86,6 +86,10 @@ def __init__(self) -> None: self.declare_parameter("cup_uprighting_extra_args", "") # 사이드 그립에서 base x가 +20mm 정도 어긋나는 실측 보정값 self.declare_parameter("side_target_x_offset_m", -0.02) + self.declare_parameter("side_target_joint6_inset_m", 0.07) + self.declare_parameter("side_target_joint6_inset_sign", 1.0) + self.declare_parameter("side_pre_pick_joint1_clearance_deg", 12.0) + self.declare_parameter("side_return_to_camera_home_after_attempt", True) self.declare_parameter("side_trajectory_execution_duration_scaling", 3.0) self.declare_parameter("side_trajectory_execution_goal_margin_sec", 3.0) self.declare_parameter("side_cup_collision_enabled", True) @@ -122,13 +126,15 @@ def __init__(self) -> None: # 키오스크/음성 주문(latest_recipe.json) 없이 색을 직접 내릴 때: 예) "red:2,blue:1" self.declare_parameter("recipe_colors", "") # 디스펜서 누르기 종료 후 디스펜서 앞의 컵을 마지막으로 재파지할 때 z 실측 보정값 + self.declare_parameter("final_regrasp_x_offset_m", 0.02) self.declare_parameter("final_regrasp_z_offset_m", 0.0) # 잡기 직전 pre 위치(cup_place 기준 X offset) 보정값. 스크립트 기본 -0.09에 -30mm 추가 self.declare_parameter("cup_pre_from_place_x_offset_m", -0.12) self.declare_parameter("dispenser_3_cup_pre_extra_x_offset_m", -0.01) # 컵홀더에 놓을 때 보정값과 place 목표 z 안전 하한 (필요 시 조정) self.declare_parameter("cup_holder_place_z_offset_m", -0.04) - self.declare_parameter("cup_holder_place_x_offset_m", 0.015) + self.declare_parameter("cup_holder_place_x_offset_m", 0.010) + self.declare_parameter("cup_holder_place_final_dispenser_4_x_extra_offset_m", -0.010) self.declare_parameter("cup_holder_place_y_offset_m", -0.010) self.declare_parameter("cup_holder_rz_offset_deg", -1.0) self.declare_parameter("cup_holder_z_min_m", 0.06) @@ -572,7 +578,7 @@ def _run_side_grasp(self, decision: RouteDecision) -> bool: "move_to_camera_home:=false", "skip_initial_home_move:=true", "return_home_after_task:=false", - "return_to_camera_home_after_attempt:=false", + f"return_to_camera_home_after_attempt:={str(self._bool_launch_arg(self.get_parameter('side_return_to_camera_home_after_attempt').value)).lower()}", "center_check_enabled:=false", "redetect_on_approach:=false", "verify_motion:=true", @@ -617,6 +623,9 @@ def _run_side_grasp(self, decision: RouteDecision) -> bool: f"{float(self.get_parameter('side_trajectory_execution_goal_margin_sec').value)}", f"moveit_controller_name:={self.get_parameter('moveit_controller_name').value}", f"side_target_x_offset_m:={float(self.get_parameter('side_target_x_offset_m').value)}", + f"side_target_joint6_inset_m:={float(self.get_parameter('side_target_joint6_inset_m').value)}", + f"side_target_joint6_inset_sign:={float(self.get_parameter('side_target_joint6_inset_sign').value)}", + f"pre_pick_joint1_clearance_deg:={float(self.get_parameter('side_pre_pick_joint1_clearance_deg').value)}", "start_joint_state_relay:=false", f"model_path:={self.get_parameter('yolo_model_path').value}", ]) @@ -749,16 +758,20 @@ def _run_recipe_sequence(self) -> bool: command += f" --cup-pre-from-place-x-offset-m {cup_pre_x}" dispenser_3_pre_x = float(self.get_parameter("dispenser_3_cup_pre_extra_x_offset_m").value) command += f" --dispenser-3-cup-pre-extra-x-offset-m {dispenser_3_pre_x}" + regrasp_x = float(self.get_parameter("final_regrasp_x_offset_m").value) regrasp_z = float(self.get_parameter("final_regrasp_z_offset_m").value) place_z = float(self.get_parameter("cup_holder_place_z_offset_m").value) place_x = float(self.get_parameter("cup_holder_place_x_offset_m").value) + place_d4_x = float(self.get_parameter("cup_holder_place_final_dispenser_4_x_extra_offset_m").value) place_y = float(self.get_parameter("cup_holder_place_y_offset_m").value) place_rz = float(self.get_parameter("cup_holder_rz_offset_deg").value) z_min = float(self.get_parameter("cup_holder_z_min_m").value) command += ( + f" --final-regrasp-extra-x-offset-m {regrasp_x}" f" --final-regrasp-extra-z-offset-m {regrasp_z}" f" --cup-holder-place-final-z-offset-m {place_z}" f" --cup-holder-place-final-x-offset-m {place_x}" + f" --cup-holder-place-final-dispenser-4-x-extra-offset-m {place_d4_x}" f" --cup-holder-place-final-y-offset-m {place_y}" f" --cup-holder-rz-offset-deg {place_rz}" f" --cup-holder-z-min-m {z_min}" diff --git a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py index 1c45822..5b21e65 100644 --- a/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py +++ b/src/dsr_practice/dsr_practice/yolo_cup_pick_node.py @@ -98,6 +98,9 @@ class SideGraspPlan: guarded_offset_m: float detected_cup_xyz: np.ndarray | None = None target_offset_xy: np.ndarray | None = None + target_joint6_inset_vec: np.ndarray | None = None + target_joint6_inset_m: float = 0.0 + target_joint6_inset_sign: float = 1.0 tcp_compensated: bool = False legacy_stage_offset_m: float = 0.0 legacy_guarded_offset_m: float = 0.0 @@ -202,6 +205,12 @@ def quat_dict_from_euler(roll_deg, pitch_deg, yaw_deg): } +def quat_dict_to_matrix(ori): + return Rotation.from_quat( + [ori["x"], ori["y"], ori["z"], ori["w"]] + ).as_matrix() + + def get_link_matrix(moveit_robot, link_name): psm = moveit_robot.get_planning_scene_monitor() with psm.read_only() as scene: @@ -254,6 +263,8 @@ def __init__(self): self.declare_parameter("side_stage_y_min", SAFE_Y_MIN) self.declare_parameter("side_stage_y_max", SAFE_Y_MAX) self.declare_parameter("side_target_x_offset_m", 0.0) + self.declare_parameter("side_target_joint6_inset_m", 0.07) + self.declare_parameter("side_target_joint6_inset_sign", 1.0) self.declare_parameter("side_grasp_offset", 0.035) self.declare_parameter("side_grasp_z_offset", 0.05) self.declare_parameter("side_grasp_stop_backoff_m", 0.04) @@ -479,6 +490,15 @@ def __init__(self): self.side_target_x_offset_m = float( self.get_parameter("side_target_x_offset_m").value ) + self.side_target_joint6_inset_m = max( + 0.0, + float(self.get_parameter("side_target_joint6_inset_m").value), + ) + self.side_target_joint6_inset_sign = ( + 1.0 + if float(self.get_parameter("side_target_joint6_inset_sign").value) >= 0.0 + else -1.0 + ) self.side_grasp_offset = float(self.get_parameter("side_grasp_offset").value) self.side_grasp_z_offset = float( self.get_parameter("side_grasp_z_offset").value @@ -792,6 +812,14 @@ def __init__(self): "side_target_x_offset_m applies only to side-grip motion planning; " f"detected cup poses are left unchanged (offset={self.side_target_x_offset_m:.3f} m)." ) + if self.side_target_joint6_inset_m > 1e-6: + self.get_logger().warning( + "side_target_joint6_inset_m applies only to side-grip motion pose targets; " + "detected cup poses are left unchanged " + f"(inset={self.side_target_joint6_inset_m:.3f} m, " + f"sign={self.side_target_joint6_inset_sign:.0f}, " + "default +1 moves gripper_tcp targets toward the cup for y-axis side grip)." + ) if self.side_cup_collision_enabled: self.get_logger().info( "Temporary detected cup/lid collision is enabled for side gross motion " @@ -2074,6 +2102,25 @@ def side_tcp_compensation_active(self): def tcp_compensated_side_offset(self, legacy_offset, minimum_offset): return max(float(legacy_offset) - self.side_tcp_reach_m, float(minimum_offset)) + def side_target_joint6_inset_vector(self, orientation): + if self.side_target_joint6_inset_m <= 1e-6: + return np.zeros(3, dtype=float) + tool_z_in_base = quat_dict_to_matrix(orientation)[:, 2] + norm = np.linalg.norm(tool_z_in_base) + if norm < 1e-6: + return np.zeros(3, dtype=float) + tool_z_in_base = tool_z_in_base / norm + return ( + tool_z_in_base + * self.side_target_joint6_inset_sign + * self.side_target_joint6_inset_m + ) + + def apply_side_target_joint6_inset(self, xy, z, inset_vec, z_min): + adjusted_xy = np.array(xy, dtype=float) + inset_vec[:2] + adjusted_z = max(float(z) + float(inset_vec[2]), float(z_min)) + return adjusted_xy, adjusted_z + def compute_side_grasp_plan(self, cup_base_xyz, side_direction=None) -> SideGraspPlan: cup_xyz = np.array([float(v) for v in cup_base_xyz], dtype=float) if side_direction is None: @@ -2126,11 +2173,48 @@ def compute_side_grasp_plan(self, cup_base_xyz, side_direction=None) -> SideGras if self.side_fixed_grasp_z_enabled: grasp_z = max(self.side_fixed_grasp_z, self.min_motion_z) else: - grasp_z = max(float(cup_xyz[2]) + self.side_grasp_z_offset, self.min_motion_z) + grasp_z = max( + float(cup_xyz[2]) + self.side_grasp_z_offset, + self.min_motion_z, + ) pre_z = grasp_z lift_z = max(grasp_z + self.approach_offset, self.safe_z) place_z = max(grasp_z, self.min_motion_z) place_approach_z = max(place_z + self.approach_offset, self.safe_z) + target_joint6_inset_vec = self.side_target_joint6_inset_vector(side_ori) + if np.linalg.norm(target_joint6_inset_vec) > 1e-6: + stage_xy, lift_z = self.apply_side_target_joint6_inset( + stage_xy, + lift_z, + target_joint6_inset_vec, + self.safe_z, + ) + pre_xy, pre_z = self.apply_side_target_joint6_inset( + pre_xy, + pre_z, + target_joint6_inset_vec, + self.min_motion_z, + ) + grasp_xy, grasp_z = self.apply_side_target_joint6_inset( + grasp_xy, + grasp_z, + target_joint6_inset_vec, + self.min_motion_z, + ) + guarded_grasp_xy, _ = self.apply_side_target_joint6_inset( + guarded_grasp_xy, + grasp_z, + target_joint6_inset_vec, + self.min_motion_z, + ) + place_z = max( + place_z + float(target_joint6_inset_vec[2]), + self.min_motion_z, + ) + place_approach_z = max( + place_approach_z + float(target_joint6_inset_vec[2]), + self.safe_z, + ) return SideGraspPlan( cup_xyz=cup_xyz, side_vec=side_vec, @@ -2149,6 +2233,9 @@ def compute_side_grasp_plan(self, cup_base_xyz, side_direction=None) -> SideGras stage_offset_m=stage_offset, pre_offset_m=pre_offset, guarded_offset_m=guarded_offset, + target_joint6_inset_vec=target_joint6_inset_vec, + target_joint6_inset_m=self.side_target_joint6_inset_m, + target_joint6_inset_sign=self.side_target_joint6_inset_sign, tcp_compensated=tcp_compensated, legacy_stage_offset_m=legacy_stage_offset, legacy_guarded_offset_m=legacy_guarded_offset, @@ -2173,12 +2260,23 @@ def log_side_grasp_plan(self, plan: SideGraspPlan, prefix="Side grasp target"): f", tcp_comp=on(stage {plan.legacy_stage_offset_m:.3f}->{plan.stage_offset_m:.3f}, " f"close {plan.legacy_guarded_offset_m:.3f}->{plan.guarded_offset_m:.3f})" ) + inset_detail = "" + if ( + plan.target_joint6_inset_vec is not None + and np.linalg.norm(plan.target_joint6_inset_vec) > 1e-6 + ): + vx, vy, vz = [float(v) for v in plan.target_joint6_inset_vec] + inset_detail = ( + f", joint6_inset={plan.target_joint6_inset_m:.3f}m" + f"(sign={plan.target_joint6_inset_sign:.0f}, " + f"vec=({vx:.3f}, {vy:.3f}, {vz:.3f}))" + ) self.get_logger().info( f"{prefix} base=({bx:.3f}, {by:.3f}, {bz:.3f}), " f"axis={self.side_grasp_axis}, dir={plan.side_direction:.0f}, " f"ori_mode={self.side_orientation_mode}, " f"tool_roll={self.side_tool_roll_deg:.1f}deg" - f"{compensation_detail}, " + f"{compensation_detail}{inset_detail}, " f"stage=({plan.stage_xy[0]:.3f}, {plan.stage_xy[1]:.3f}, {plan.pre_z:.3f}), " f"pre=({plan.pre_xy[0]:.3f}, {plan.pre_xy[1]:.3f}, {plan.pre_z:.3f}), " f"grasp=({plan.grasp_xy[0]:.3f}, {plan.grasp_xy[1]:.3f}, {plan.grasp_z:.3f}), " diff --git a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py index 3be4683..3049843 100644 --- a/src/dsr_practice/launch/yolo_cup_pick_node.launch.py +++ b/src/dsr_practice/launch/yolo_cup_pick_node.launch.py @@ -263,6 +263,12 @@ def _runtime_nodes(context, moveit_params, moveit_py_params, side_prepose_params "side_target_x_offset_m": LaunchConfiguration( "side_target_x_offset_m" ), + "side_target_joint6_inset_m": LaunchConfiguration( + "side_target_joint6_inset_m" + ), + "side_target_joint6_inset_sign": LaunchConfiguration( + "side_target_joint6_inset_sign" + ), "side_grasp_offset": LaunchConfiguration("side_grasp_offset"), "side_grasp_z_offset": LaunchConfiguration("side_grasp_z_offset"), "side_grasp_stop_backoff_m": LaunchConfiguration( @@ -613,6 +619,16 @@ def generate_launch_description(): default_value="0.0", description="Planning-only base_link X compensation added to side-grip cup targets after vision/refinement.", ) + side_target_joint6_inset_m_arg = DeclareLaunchArgument( + "side_target_joint6_inset_m", + default_value="0.070", + description="Planning-only side-grip target shift distance from gripper_tcp toward joint_6.", + ) + side_target_joint6_inset_sign_arg = DeclareLaunchArgument( + "side_target_joint6_inset_sign", + default_value="1.0", + description="Tool local Z sign for side_target_joint6_inset_m; +1 moves default y-axis side-grip targets toward the cup.", + ) side_grasp_offset_arg = DeclareLaunchArgument( "side_grasp_offset", default_value="0.035" ) @@ -1067,6 +1083,8 @@ def generate_launch_description(): side_stage_y_min_arg, side_stage_y_max_arg, side_target_x_offset_m_arg, + side_target_joint6_inset_m_arg, + side_target_joint6_inset_sign_arg, side_grasp_offset_arg, side_grasp_z_offset_arg, side_grasp_stop_backoff_m_arg, diff --git a/tools/run/robot_pipeline_control_server.py b/tools/run/robot_pipeline_control_server.py index 34d7094..96bc238 100755 --- a/tools/run/robot_pipeline_control_server.py +++ b/tools/run/robot_pipeline_control_server.py @@ -774,10 +774,10 @@ class Step: "side_grip", "PR #20 RealSense 컵 인식 후 side grip", "background", - "SIDE_TARGET_X_OFFSET_M=-0.020 bash tools/run/run_changhyun_side_grip_direct.sh", + "SIDE_TARGET_X_OFFSET_M=-0.020 SIDE_TARGET_JOINT6_INSET_M=0.070 SIDE_TARGET_JOINT6_INSET_SIGN=1.0 bash tools/run/run_changhyun_side_grip_direct.sh", True, True, - "OpenCV 창에서 컵 확인 후 p 키로 side-grip 실행. 패널은 direct runner를 tmux로 띄우며 기본 X 보정은 -20mm", + "OpenCV 창에서 컵 확인 후 p 키로 side-grip 실행. 패널은 direct runner를 tmux로 띄우며 기본 X 보정은 -20mm, y축 side-grip target 보정은 컵 방향 70mm", ), Step( "cup_uprighting", @@ -3556,10 +3556,22 @@ def tumbler_scene_once(action: str, *, object_id: str = "carried_tumbler", dispe or os.environ.get("SIDE_TARGET_X_OFFSET_M") or "-0.020" ) + side_target_joint6_inset_m = str( + payload.get("side_target_joint6_inset_m") + or os.environ.get("SIDE_TARGET_JOINT6_INSET_M") + or "0.070" + ) + side_target_joint6_inset_sign = str( + payload.get("side_target_joint6_inset_sign") + or os.environ.get("SIDE_TARGET_JOINT6_INSET_SIGN") + or "1.0" + ) manual_cmd = ( f"cd {ROOT} && " f"SERVICE_PREFIX={shlex.quote(service_prefix)} " f"SIDE_TARGET_X_OFFSET_M={shlex.quote(side_target_x_offset_m)} " + f"SIDE_TARGET_JOINT6_INSET_M={shlex.quote(side_target_joint6_inset_m)} " + f"SIDE_TARGET_JOINT6_INSET_SIGN={shlex.quote(side_target_joint6_inset_sign)} " "DISPLAY=${DISPLAY:-:0} " "XAUTHORITY=${XAUTHORITY:-/run/user/1000/gdm/Xauthority} " f"bash {shlex.quote(str(direct_script))}" diff --git a/tools/run/run_changhyun_side_grip_direct.sh b/tools/run/run_changhyun_side_grip_direct.sh index 175dde0..322ec1b 100755 --- a/tools/run/run_changhyun_side_grip_direct.sh +++ b/tools/run/run_changhyun_side_grip_direct.sh @@ -98,7 +98,7 @@ fi side_grip_success_log="$(mktemp /tmp/azas_changhyun_side_grip.XXXXXX.log)" set +e -ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ +ros2 launch "${ROOT}/src/dsr_practice/launch/yolo_cup_pick_node.launch.py" \ model_path:="${ROOT}/local_models/best.pt" \ conf:=0.35 imgsz:=640 device:=cpu target_class:=cup \ auto_pick:=false auto_pick_interval:=8.0 exit_after_pick:="${EXIT_AFTER_PICK:-true}" \ @@ -107,6 +107,8 @@ ros2 launch dsr_practice yolo_cup_pick_node.launch.py \ grasp_mode:=side side_far_stage_enabled:=false side_approach_offset:=0.18 \ side_short_stage_backoff_m:=0.08 side_grasp_stop_backoff_m:=0.04 side_close_underreach_m:=0.03 \ side_target_x_offset_m:="${SIDE_TARGET_X_OFFSET_M:--0.020}" \ + side_target_joint6_inset_m:="${SIDE_TARGET_JOINT6_INSET_M:-0.070}" \ + side_target_joint6_inset_sign:="${SIDE_TARGET_JOINT6_INSET_SIGN:-1.0}" \ side_low_retry_lift_m:=0.0 side_low_retry_attempts:=0 \ side_linear_approach_enabled:=true side_final_slide_enabled:=false \ side_fixed_grasp_z_enabled:=false side_grasp_z_offset:=0.05 side_project_bbox_center_to_fixed_z:=false \ diff --git a/tools/run/run_color_recipe_sequence.py b/tools/run/run_color_recipe_sequence.py index 365a52a..37c5c6c 100644 --- a/tools/run/run_color_recipe_sequence.py +++ b/tools/run/run_color_recipe_sequence.py @@ -366,7 +366,7 @@ def main() -> int: ) parser.add_argument("--regrasp-rear-entry-offset-x-m", default="-0.090") parser.add_argument("--regrasp-rear-entry-offset-y-m", default="0.0") - parser.add_argument("--final-regrasp-extra-x-offset-m", default="0.000") + parser.add_argument("--final-regrasp-extra-x-offset-m", default="0.020") parser.add_argument("--skip-initial-move-release", action="store_true", help="복구 모드: 컵이 이미 첫 디스펜서 front-hold에 놓여 있다고 가정하고 press부터 시작") parser.add_argument( @@ -405,6 +405,7 @@ def main() -> int: ) parser.add_argument("--cup-holder-place-final-z-offset-m", default="-0.040") parser.add_argument("--cup-holder-place-final-x-offset-m", default="0.015") + parser.add_argument("--cup-holder-place-final-dispenser-4-x-extra-offset-m", default="-0.010") parser.add_argument("--cup-holder-place-final-y-offset-m", default="-0.010") parser.add_argument( "--cup-holder-rz-offset-deg", @@ -505,6 +506,7 @@ def scaled_motion_capped(value: str, cap: float) -> str: "--gripper-settle-seconds", str(args.gripper_settle_seconds), "--cup-holder-place-final-z-offset-m", str(args.cup_holder_place_final_z_offset_m), "--cup-holder-place-final-x-offset-m", str(args.cup_holder_place_final_x_offset_m), + "--cup-holder-place-final-dispenser-4-x-extra-offset-m", str(args.cup_holder_place_final_dispenser_4_x_extra_offset_m), "--cup-holder-place-final-y-offset-m", str(args.cup_holder_place_final_y_offset_m), "--cup-holder-rz-offset-deg", str(args.cup_holder_rz_offset_deg), "--cup-holder-z-min-m", str(args.cup_holder_z_min_m), diff --git a/tools/run/run_measured_dispenser_recipe_sequence.py b/tools/run/run_measured_dispenser_recipe_sequence.py index aa98981..da6a9c5 100755 --- a/tools/run/run_measured_dispenser_recipe_sequence.py +++ b/tools/run/run_measured_dispenser_recipe_sequence.py @@ -3731,7 +3731,7 @@ def parse_args() -> argparse.Namespace: parser.add_argument( "--final-regrasp-extra-x-offset-m", type=float, - default=0.000, + default=0.020, help=( "Only for the final re-grasp before cup-holder placement: add this X offset " "to the cup re-grasp target. Positive X moves closer toward the dispenser/cup." @@ -3749,6 +3749,15 @@ def parse_args() -> argparse.Namespace: ) parser.add_argument("--cup-holder-place-final-z-offset-m", type=float, default=-0.040) parser.add_argument("--cup-holder-place-final-x-offset-m", type=float, default=0.015) + parser.add_argument( + "--cup-holder-place-final-dispenser-4-x-extra-offset-m", + type=float, + default=-0.010, + help=( + "When the final re-grasp is from physical dispenser 4, add this extra X " + "offset to CUP_HOLDER_PLACE_FINAL without editing calibration.yaml." + ), + ) parser.add_argument("--cup-holder-place-final-y-offset-m", type=float, default=-0.010) parser.add_argument( "--cup-holder-rz-offset-deg", @@ -3912,6 +3921,16 @@ def main() -> int: print("[Azas] Measured dispenser recipe sequence") print(f"[Azas] dispenser_ids={','.join(dispenser_ids)}") grouped_dispenser_ids = group_consecutive_dispenser_ids(dispenser_ids) + final_dispenser_id = grouped_dispenser_ids[-1][0] if grouped_dispenser_ids else "" + if final_dispenser_id == "4": + args.cup_holder_place_final_x_offset_m += ( + args.cup_holder_place_final_dispenser_4_x_extra_offset_m + ) + print( + "[Azas] dispenser 4 final cup-holder X extra offset applied: " + f"{args.cup_holder_place_final_dispenser_4_x_extra_offset_m:.3f} m, " + f"effective_place_final_x_offset_m={args.cup_holder_place_final_x_offset_m:.3f}" + ) print( "[Azas] grouped_press_counts=" + ",".join(f"{dispenser_id}x{count}" for dispenser_id, count in grouped_dispenser_ids) diff --git a/tools/run/run_tmux_logic_sequence.sh b/tools/run/run_tmux_logic_sequence.sh index c6cc3c7..e190233 100755 --- a/tools/run/run_tmux_logic_sequence.sh +++ b/tools/run/run_tmux_logic_sequence.sh @@ -161,6 +161,8 @@ run_side_grip() { grasp_mode:=side side_far_stage_enabled:=false side_approach_offset:=0.18 \ side_short_stage_backoff_m:=0.08 side_grasp_stop_backoff_m:=0.04 side_close_underreach_m:=0.03 \ side_target_x_offset_m:="${SIDE_TARGET_X_OFFSET_M:--0.020}" \ + side_target_joint6_inset_m:="${SIDE_TARGET_JOINT6_INSET_M:-0.070}" \ + side_target_joint6_inset_sign:="${SIDE_TARGET_JOINT6_INSET_SIGN:-1.0}" \ side_low_retry_lift_m:=0.0 side_low_retry_attempts:=0 \ side_linear_approach_enabled:=true side_final_slide_enabled:=false \ side_fixed_grasp_z_enabled:=false side_grasp_z_offset:=0.05 side_project_bbox_center_to_fixed_z:=false \ diff --git a/tools/run/run_voice_auto_cup_flow.sh b/tools/run/run_voice_auto_cup_flow.sh index 83a89ad..61783f4 100755 --- a/tools/run/run_voice_auto_cup_flow.sh +++ b/tools/run/run_voice_auto_cup_flow.sh @@ -49,7 +49,7 @@ set +e ros2 launch azas_bringup auto_cup_flow_router.launch.py \ enable_real_motion:=true \ router_confirm:=ENABLE_AUTO_CUP_ROUTER \ - cup_holder_place_x_offset_m:=3.0 \ + cup_holder_place_x_offset_m:=0.010 \ service_prefix:="${SERVICE_PREFIX}" \ motion_service_prefix:="${MOTION_SERVICE_PREFIX}" \ moveit_controller_name:=/${SERVICE_PREFIX}/dsr_moveit_controller \